问:
Choosing the right dc motor
I'm trying to find the optimal components list for  a radio controlled lawn mover i'm trying to build.

The blades will be rotated by a 140 cc engine. I choose this engine because it's already mine. It's weight is 25 kg

The movement will be electrical powered by 2 battery 12v and 18 amp h

i will use arduino and 2 motor drivers. Each driver can handle 6v to  27v and  a maximum of 43 amp

I wanna move "that thing" with a speed of 4/5 km hour.
It have to climb hill until 40% (with dry terrain).

It is possible to calculate the specs of a dc engine that could "approximately" move that thing in that conditions ?

I would like to know: watt,gear ratio, rpm, volt ...
I know, it's all approximate but anyway it will give me an help.

Thanks!

答:
Your maximum power is already given by the driver.
Assuming that 24V motors are easier to come by, $$24V * 43A = 1023  W$$ electrical power. I am assuming that you have the combined maximum output at $43A$.

Depending on what type of motor you find with $24V$ Voltage and $1kW$ power you can add a gearbox and adjust wheel diameters to get to a desired performance.

A $150mm$ wheel diameter gives you $150 mm \times \pi = 471 mm$ of travel for one rotation of the wheel. The target $5 km/h$ equals to  $83 m$ each minute. If you advance $0.47 m$ at each wheel rotation, this $83m$ each minute results in a requirement of $176 rpm$ rotations per minute on the wheel. Using a different unit of measurement for the rotational velocity, this gives you $18.43 rad/s$. Basically, you need to find a $24V$ $1kW$ motor and gearbox that will result in an output rpm of $176 rpm$. Please note that this is at the wheel axis, so any differential transmission will also have a reduction ratio which needs to be considered.

We know the maximum electrical power and let us assume an $80\%$ efficiency (conversion rate to mechanical power). That gives you about $800W$ of mechanical power. Now we know that your target velocity is $18.43 rad/s$ and we know that:

$$ P = \tau \times \omega$$
Mechanical power is the product of torque and angular velocity. We can calculate that for the given mechanical power of $800W$, with the target velocity of $18.45 rpm$ you will have $43.4 Nm$ of Torque available at the wheel axis. Now we have assumed $150mm$ wheel diameter, so the radius of the wheel is $75mm$. $43.4 Nm$ on a distance of $75mm$ produces $578.6 N$ of force. This is the maximum force that you can get at your wheels.

$40\%$ slope translates to $21.8 \deg$. From this we can deduce, that the sin component of your gravitational force cannot exceed $578 N$. At the limit

$$F= m * g * sin(21.8 \deg)$$

$ 578.6 N = m  kg \times 9.81 m/s^2 \times 0.189$. So your absolute maximum weight from a static perspective is about $312 kg$.

Please note that this is the max weight you can move at $5km/h$ with a $1kW$ motor at a 40% slope on a perfectly flat terrain and it does not mean that with this weight your system can actually accelerate to this velocity. The more you are under this weight limit the more dynamic performance your system will have! Furthermore there might be some fictions forces to consider from the 140 cc engine that will further reduce the dynamic performance.

For the power capacity, if you put the two batteries in series, you get 24V and 18 Ah. This will mean that your batteries will be able to power your system under full load, for $18Ah / 43A =  0.41h$. So about 24 minutes of fun.

Title: Choosing the right dc motor


问:
Assigning Frames and Deriving Link Parameters
The textbook I'm using doesn't have the answers to the practice questions, so I'm not sure how I'm doing. Are the following DH parameters correct given the frames I assigned?

The original question is as follows:
The arm with 3DOF shown below is like the one in example 3.3, except that joint 1's axis is not parallel to the other two. Instead, there is a twist of 90 degrees in magnitude between axes 1 and 2. Derive link parameters and the kinematic equations for $^bT_w$ (where b means base frame and w means wrist frame).

Link Parameters:

$$\begin{array}{ccc}
  & const.        & and      &     &               \\
  & angle         & distance &     &               \\
i & \alpha_{i-1}  & a_{i-1}  & d_i & \bf{\theta_i} \\
\\
1 & 0             & 0        & 0   & \theta_1      \\
2 & -90           & L_1      & 0   & \theta_2      \\
3 & 0             & L_2      & 0   & \theta_3      \\
\end{array}$$

Frame Assignments:

答: For the i =2, with alpha(i-1), it should be 0 if Z1 and Z2 are parallel </p>

ContentLicense: CC BY-SA 3.0


问:
Where to buy cheap motors and mechanical parts for hobby robotics applications?
My kids want to get into robotics, where Online can I buy the motors (dc,ac,stepper...) and mechanical assembly (nuts, bolts, sheet metal, springs, rods...) for a very affordable price, you know I just want to get a big box of all this stuff and let them tinker around with it. Even where to find cheap microcontrollers, leds, wheel bearings, wheels and tracks, sensors...</p>

答: Go to ebay and type things like arduino robot into the search bar and use search descriptions. Anything you click on will if you scroll down have a section labeled people who bought this also bought. Its a way to do a tour of  what people have been buying.</p>

ContentLicense: CC BY-SA 4.0


问:
roslaunch not found
Running ROS melodic on Ubuntu 18.04 and I keep getting this error.

```<code>$ roslaunch arm robot_spawn.launch

Command 'roslaunch' not found, but can be installed with:

sudo apt install python-roslaunch
</code>```

I was able to launch prior to this but decided to add another node though. When I try to reinstall it, I get a recursive errors like in this question. Anyway, I checked the solution provided in the question link that says that I need ros-melodic-launch and not python-roslaunch so I go ahead and try to source melodic's setup.bash but I already had

source /opt/ros/kinetic/setup.bash in my .bashrc.

Upon further digging I find another question.  I have checked the logs of apt in /var/log/apt/history.log and can confirm that I have it.

答:
You may also just try running

```<code>sudo apt install ros-<your-distribution>-roslaunch
</code>```

I was repeatedly running

```<code>sudo apt install python3-roslaunch
</code>```

Which was installing roslaunch into python installation path and thus on running roscore, roslaunch was missing in

```<code>/opt/ros/<ros-distro>/share/ros
/opt/ros/<ros-distro>/share
</code>```

Title: roslaunch not found

Body:
You may also just try running

sudo apt install ros-<your-distribution>-roslaunch

I was repeatedly running

sudo apt install python3-roslaunch

Which was installing roslaunch into python installation path and thus on running roscore, roslaunch was missing in

/opt/ros/<ros-distro>/share/ros
/opt/ros/<ros-distro>/share


问:
Calculate the Vehicle corner points given localization
I am trying to calculate the corner points of the vehicle given the details below:

Localization data from rear axle of the vehicle

Length and Width of the vehicle

Heading of the vehicle ENU

Could anybody help me calculate the 4 corner points of the vehicle?

答:
Your vehicle heading is some angle, and the corner is some distance from the rear axle. The best coordinate system to use for angles and distances is polar. So, start with a vehicle layout like:

I'm only going to go over one corner (the rest is your work to do :-)), but consider the front passenger-side corner (top view), from the center of the rear axle. You could measure the corner to be some $<x_0, y_0>$ from the center, but the easier way to handle it would be as an $<r, \theta>$ from the rear center.

Unless the car body is deforming, your $r$ and $\theta$ remain constant. Now, whenever you get a new heading $\phi$, you can add that to your "default" angle $\theta$. That is, $\theta$ is driven by your vehicle's geometry, $\phi$ is driven by your vehicle's heading. You wind up with:

$$
x = r\cos\left(\theta + \phi\right) \\
y = r\sin\left(\theta + \phi\right)
$$

You need to be careful with the combination of $\theta$ and $\phi$ - how the angles are added will depend on how you choose to define your angles. If you were lay a compass rose over the vehicle image I've drawn, my definition of $\theta$ would be from the "East" and increasing positive counter-clockwise. Compass angles, though, start with zero at the North and increase positive in the clockwise direction.

The easiest way to handle this would be to define your own $\theta$ using the same angular conventions as your heading, because then you really can just sum the two angles and get your corners. You may (will probably) need to redefine which trig functions are used if you're changing the angular definitions.

Please give this a shot and, if you've still got questions, please feel free to ask them here! Small follow-ups can be handled in comments, but if there's a big change in scope then please start a new question (you can link to this question in the new post).


问:
2D point cloud registration success probability
I am trying to implement localisation by storing images from a camera and their accompanying point clouds from a 2D lidar during mapping. During localisation I then use image matching to suggest the closest image and perform ICP on the point cloud to find the relative pose to where the original image was taken. My problem is that the best matching image is not always correct so I want to use the top say 5 images and see which one's point cloud aligns the best and if any of the alignments are any good at all. Unfortunately I have not been able to find any way of evaluating how good a point cloud registration is. Currently I am using the LibPointMatcher library and the best that it can provide is a residual error after alignment but some rough testing seems to suggest that this does not really help to determine if an alignment failed or not and does not seem to help with comparing the relative quality of different alignments either.

It is my understanding that there are algorithms such as Monte Carlo localisation/particle filter that can localise quite well using just 2D lidar scans. From my understanding of how they work they, they need to rank how likely each particle is after obtaining a lidar scan based on how well the current scan aligns with what would be expected given said particle's pose and the current map. Now obviously I am not actually building a point cloud map from which I am then calculating the expected points from each particle, but I think that my approach still amounts to something similar in that I also need to rank how likely it is that the current point cloud correlates with what was expected. Unfortunately, I have not been able to figure out how these Monte Carlo systems derive such an alignment probability.

Any suggestions on how I can rank the relative success of 2D point clouds registrations and preferably also identify alignments that were complete failures would be greatly appreciated.

答:
Matching point clouds can be very tricky.  It is kind of a needle-in-a-haystack type of problem when you don't have an initial guess at the correspondence.  As you found, if the point clouds are very different there really isn't a great way to quantify the similarity.  This holds even if the two scans are similar (or even the same!) but have very different orientations.

Algorithms like Iterative Closest Point (ICP) (which LibPointMatcher uses) requires the two point clouds to be closely aligned to begin with.  Only then can it find the correspondence between the two and also produce a reasonable residual.

Particle filter localization with 2D lidar scans works quite well because you are simply matching one scan with the next in time so they are very similar to begin with.  The robot hasn't moved very much between scans, and you have other sensors like wheel odometry and an Inertial Measurement Unit (IMU) so you have a good guess for the alignment.  Furthermore, particle filters use a large population of points surrounding the hypothesized new location of the robot.  Each point has its own translation and rotation from the original point, and does its own scan matching to determine a score.

I don't think a particle filter will work for you in this situation, but you can use some of its methods.  If you have two different scans and no prior knowledge of the correspondence between them, you will need to try many different random translations and rotations.  I can't say how many, that is entirely dependent on your application, time constraints, etc.  But hopefully you will have some heuristics to be able to narrow the search space.


问:
Universities offering courses about Drones (Unmanned Aerial Vehicles [UAVs])
I am trying to gather some information about university courses offered about drones (or Unmanned Aerial Vehicles).

At the beginning, I thought that I would have found way more than what I found and instead I could notice that most of the universities do not offer courses directly about drones (or aerial robotics) but more about mobile robotics (which of course include drones).
Some of the universities I have seen that are offering these kinds of courses are listed below (it might be useful for someone else in the same situation). However, I would like to know if you know of more of these courses.

Upenn (Coursera) - Aerial Robotics

University of Nevada Reno - Introduction to Aerial Robotics

Swiss Federal Institue of Technology (EPFL) - Aerial Robotics

Somehow related:

Politecnico di Milano - Aeronautical Systems-Guidance and Control

If I will find more of them I will try to add them to this post.
Thanks in advance.

答:
You should probably check out the universities below. I have read about some great research work done in the field of Aerial Robotics in these universities. They might not have a separate program as such for Aerial Robotics but there's extensive amount of projects that several students and researchers take part in under this area, be it through the Mechanical or Electrical side.

Queensland University of Technology

National University of Singapore

ETH Zurich

Hope this helps you out!


问:
Motor driver damage and jerking behavior when load driving in same direction as motor
I'm having trouble with a gear motor being powered by a power supply, while it is being driven by the load in the same direction as the driver wants to drive it. Here is a simple diagram:

The gear motor is the prime mover in a winch system that lifts and lowers a mass of about 25 pounds.

for reference this is the motor:

https://www.superdroidrobots.com/shop/item.aspx/ig52-04-24vdc-082-rpm-gear-motor-with-encoder/1181/

This is the motor driver:

https://www.basicmicro.com/RoboClaw-2x15A-Motor-Controller_p_10.html

And this is the power supply:

https://www.amazon.com/gp/product/B07ML2MP9Q/ref=ppx_yo_dt_b_asin_title_o00_s00?ie=UTF8&psc=1

I'm controlling the system by doing all speed/position control on a raspberry pi, sending forward/backward duty cycle commands to the roboclaw over serial. When lifting the system performs exactly as expected: the motor smoothly rotates. Initially, when lowering, if I sent the motor driver a duty cycle to lower it would jerk, then freewheel, then lower very slowly, and that channel of the driver would no longer function, and would produce a 'damaged' error code. I solved this by simply sending it a duty cycle of 0 and allowing it to lower slowly. This is turning out to be inconvenient for my project timeline, so I'm trying to solve it. I found this video:

https://www.youtube.com/watch?v=-5YFYhBU-jQ

So I'm guessing the H bridge on the driver opening up when lowering, allowed the motor to try to sink current into the power supply, the power supply didn't like that and would get fussy and temporarily shut off, thereby shutting off the motor driver, but then the motor driver would get powered on by the back drive voltage of the motor, freak out, freewheel, get damaged, and shut everything down. This was somewhat confirmed by added a forward bias schottky diode to the positive pole of the power supply, as I can now give the system a lowering duty cycle and it will run without destroying itself. HOWEVER now when it is lowering, the behavior is very jerky, like the driving of the motor is producing a current that want's to run into the power supply, but it has nowhere to go. The shuddering vibration is not great for the mechanical aspects of the system, so I'd like to try and solve this. Is there anything I can add to the circuit to help smooth out this behavior? or anything I can add to give the driven motor current somewhere to go?

答:
What you need is something that is generally referred to as a "brake resistor."

It looks like there's an application note specifically for your product. It says, in part:

A voltage clamp is a circuit used with a motor controller to divert excess energy away from the control circuitry and power supply to prevent damage. In the two applications discussed below a MOSFET is used as switch to divert excess power through a power resistor where it’s dissipated as a heat.

...

The primary purpose of a voltage clamp is to protect a motor
controller’s power supply from the regenerative power created when a
motor is stopped or slowed down quickly. A motor spins when power is
applied to it however it also acts as a generator when slowing down or
suddenly stooped. This energy opposes the flow of current to the motor
and is directed back to the motor controller and power source.
Switching power supplies are not designed to handle this power surge
and some batteries will also not tolerate the sudden inrush of
current. A voltage clamp detects this power surge as a rise in voltage
and sends the excess current to a resistor where it’s turned in to
heat.

Basically exactly as it says - you can generate power by back-driving a motor; this is how regenerative braking works. The catch is that there needs to be somewhere for the power to go, and switching power supplies can't put the power back in the battery.

The solution, then, is to just dump that excess power. Burn it off as heat. The easiest way to determine a power rating on your brake resistor would be to use your power electronics as a guide, with the assumption that your regenerative power generation capacity is going to be approximately the same as your applied power generation capacity.

That is, if you've got a system where you could provide 5 amps at 12 volts, then you can apply (5*12) = 60 watts of power. If you can provide 60 watts winching up, then the winched load could back-drive at 60 watts lowering.

The application note states:

Select the resistor for the voltage clamp based on the table below. Note that the Digikey parts listed below are all 50 watt resistors which are sufficient for most use cases. However if the voltage clamp is expected to be on for long periods of time or the motor is driving a large load a resistor with a higher power rating may be necessary.

This is the board manufacturer's suggestion, so you could start with this, but again I would (personally) look at your continuous power draw on the hoist operation and rate at 1.5 to 2x the power draw for the brake resistor's power rating.

As a final note - the manufacturer is suggesting you use FIFTY WATT power resistors. Imagine touching a 60 watt incandescent light bulb - the brake resistors are going to get about that hot. Don't put this near any wire harnesses, combustibles, etc. It's going to get very hot - the brake resistor alone is what's controlling your load's descent.


问:
GNSS Dead Reckoning -- Sensor Fusion Filter not necessary?
My question might be a bit off-topic but I don't know where else to ask.

When I do have a GNSS Module with integrated IMU and Dead Reckoning, for example, the Ublox ZED - F9K.

Does it still make sense to use a Filter for fusing GNSS and IMU / Odometry data or is that not necessary as the Module is handling that for me?

Or will the Dead Reckoning just enhance the precision after applying a Filter even more?

答:
Question

Is it necessary to use any filter to fuse Ublox ZED-F9K's GNSS and IMU/Odometry
data?

Answer

My answer is NO, for the following reasons:

(1)  GNSS and IMU are independently developed modules with matured software. It is unlikely for the vendor to "fuse" them at the hardware level.  A more efficient method is to let the MCU/SBC to talk to either or both guys using I2C,  UART, or SPI.

(2) uBlox uCenter uses very simple, text based NMEA0183 protocol, and update rate is of the order of 25Hz, so the user can easily pass the slow GNSS data to IMU for processing.

Discussion, Conclusion, and Recommendation

I think it is not a good idea to use the expensive US$300 ZED-F9K module to do any feasibility studies.  I would suggest to use two separate modules (1) UART NEO-8M, (2) Any I2C IMU and use Rpi python to do any system integration test.

References

(1) SparkFun GNSS Products Catalog

(2) GPS-RTK ZED-F9R Dead Reckoning Breakout - SparkFun US$300

(3) SparkFun GPS-RTK Dead Reckoning ZED-F9R Hookup Guide (Python)

(4) GPS Fixing Time Problem Q&A - tlfong01, EE SE, 2020sep03

(5) Neo-6M/7M/8M GPS Module Setup/Fix/Update Problem Q&A - tlfong01 Rpi SE 2020jun18


问:
sending and receiving parameters to ardupilot
I am interested in getting an arducopter with an ardupilot(APM). I read through the documentation and from what I understand, ardupilot is the low level hardware and firmware that directly controls the motors of the arducoptor.

I would like to know if there is a higher level programmatic interface to the ardupilot? The mission planner provides a user interface to control the ardupilot. But is there a programmatic interface to control it?

In other words, would it be possible for a user written 'linux process' to receive and send sensory data to and from the ardupilot respectively?

答:
Yes, there is programmatic interface. Like many autopilot systems the Ardupilot uses a protocol called Mavlink. It allows for both sending commands and receiving telemetry data. It also allows for managing the data that is sent to the ground control system to avoid overburdening ones communication link. There is support for Mavlink in C and python as well as a ROS library (see Mavlink Github).

Furthermore this can be used for onboard control via a single board computer (SBC) such as a Raspberry Pi, Gumstix, or PCduino by interfacing the preferred SBC with the Ardupilot XBee port which is really just a TTL level serial.


问:
Rotary Stage (high resolution controller for BLDC motor)
I am building a 3phase BLDC motor controller based on an STM32 MCU, a high resolution 26 bit optical encoder and a DRV8301 motor driver from TI.

This is a for a high precision rotary stage for an optical project.

My question is about the strategy to control the motor. I have tried multiple different techniques but at the end of the day I am not able to tune the system for the various operating modes I will need when using the rotary stage.

The modes I will be needing are: position hold, very slow speed control (from 100 to 1000 counts per second) to moderate speeds (a few degrees per seconds), up to high speed (relative to my application) or about 60 degrees per second.

In other words we are talking a wide range of speeds from 100 or counts per second to about 10 million counts per second.

Position data is polled at 8KHz and the noise is roughly 2 counts RMS which is pretty good.

I initially tried a P-PI cascade controller (a position controller feeding a velocity controller). it worked, but this technique didn't yield the best tracking accuracy mainly because the velocity measurements are derived from the position and the delta position between two periods is roughly equivalent to noise. Boosting the filtering helped but not enough.

I tried variations of this cascade controller, by adding I and D terms. Which didn't really help.

I then tried to do a simple position controller (PI, and PID) which gave me the best results in terms of tracking accuracy. The biggest challenge with this one is that there is realistically no way of tuning for one specific speed without significantly damaging the other speeds I am interested in.

Of course I could retune or create various presets, or even try to correlate PID gains to the velocity I want to track at.

My question is this: what is the best approach to this problem? Is there a better strategy than any of the ones I mentioned?

Cheers
SM

答:
Do you have a predefined trajectory to track?
Did you try adding a feed forward term to the cascaded controller based on the velocity of the trajectory?
Do you use a torque controller(cascaded pos-vel-torque controllers)?
If this is part of an optical system I assume that you can very well approximate the forces in your system so you could also add a feed forward signal to your torque controller, based on a model of your system</p>


问:
How can I allow for this code to switch from remote control to autonomous?
//ERROR: Task 'autonomous' is not defined at global scope level</p>
```<code>#pragma config(StandardModel, "RVW CLAWBOT")
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma DebuggerWindows("joystickSimple")
task autonomous();
task main()

{
while (true)

{

    // update buttons and joysticks
    getJoystickSettings(joystick);

    //Assign the left and right joystick values
    //to the left and right motors
    motor[leftMotor] = joystick.joy1_y1;
    motor[rightMotor] = joystick.joy1_y1;

task autonomous()
  {
    motor[leftMotor] = 80;
    motor[rightMotor] = 80;
    wait1Msec(2500);
    motor[leftMotor] = 0;
    motor[rightMotor] = 0;

    wait1Msec(1000);

    motor[leftMotor] = 80;
    motor[rightMotor] = -80;
    wait1Msec(2500);
    motor[leftMotor] = 0;
    motor[rightMotor] = 0;

  }
    if(motor[rightMotor] == 1) {
    startTask(autonomous);
    stopTask(main);

}
}
}
</code>```

答: The code for “task autonomous()” should be outside of the main() function.</p>


问:
How nanorobots are programmed?
I was thinking about the possible ways to program nanorobots, it can't be done with the regular tools.So I'm curious to know how professionals program nanobots?
Any complementary articles, extra readings are welcomed.</p>

答:
For anyone having the same question, I have found the answer :

The development of programmable nanobot arch-types offers an
opportunity to consider tools for programming nanobot cocktails.
Inspired by modern software development environments, which separates
high-level programming languages from specific CPU details, we aim to
allow medical professionals to directly program treatments in Athelas,
a medication programming language.  the design of Athelas is
motivated by  the  success  of  rule-based  systems  at  capturing
expert  knowledge  [6,  7].   A  compiler  (Bilbo)  translates Athelas
programs to nanobot specifications, which implement the program.  The
compiler relies on a library of generic nanobot arch-types, and
specializes them to create the specific roles needed for the swarm.

Reference: A Compiler for Programming Molecular Robots Inbal Wiesel, Noa Agmon, and Gal A. Kaminka

Title: How nanorobots are programmed?


问:
How to find visual odometry by video from cellphone camera mounted on robot?
I am writing codes in python for visual odometry from  single camera. I used cell phone camera for testing. I took video of 35 sec with camera moving. I used code below to read first image

```<code>height=4000
fx=3793
fy=3795
cx=2073
cy=1427
focal = fx
pp = (cx,cy)
R_total = np.zeros((3, 3))
t_total = np.empty(shape=(3, 1))
cur_ROT = np.eye(3)
cur_TRS = np.zeros((3,1))
kMinNumFeature= 1500

image_id = 0
frams_arr=[]
framrs= 3

# params for ShiTomasi corner detection
feature_params = dict( maxCorners = 100,
                       qualityLevel = 0.3,
                       minDistance = 7,
                       blockSize = 7 )

# Parameters for lucas kanade optical flow
lk_params = dict( winSize  = (15,15),
                  maxLevel = 2,
                  criteria = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03))
traj = np.zeros((600,600,3), dtype=np.uint8)

cap = cv2.VideoCapture('ga1.mp4')
ret,frame = cap.read()
img = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
ref_frm = img
P0 = cv2.goodFeaturesToTrack(img, mask=None, **feature_params)
while(1):
    ret,new =cap.read()
    img2= cv2.cvtColor(new, cv2.COLOR_BGR2GRAY)
    cur_frm = img2
    P1, st, err = cv2.calcOpticalFlowPyrLK(ref_frm, cur_frm,P0, None, **lk_params)
    E, mask = cv2.findEssentialMat(P1, P0, focal=focal, pp=pp, method=cv2.RANSAC,
                                   prob=0.999, threshold=1.0)
    _, R, t, mask = cv2.recoverPose(E, P1, P0, focal=focal, pp=pp)
    cur_TRS = cur_TRS + cur_ROT.dot(t)
    cur_ROT = R.dot(cur_ROT)
    if (P1.shape[0] < kMinNumFeature):
        P1 = cv2.goodFeaturesToTrack(cur_frm, mask = None, **feature_params)
    ref_frm =cur_frm
    P0 = P1.copy()
cv2.imshow('img', img2)
k = cv2.waitKey(30) & 0xff

cv2.destroyAllWindow
cap.release()
</code>```

I want to know whether rotation and translation values are correct or not. When I executed python code I am getting this error.

```<code>    Traceback (most recent call last):
  File "C:/Users/HP/PycharmProjects/untitled3/venv/Lalsab.py", line 48, in <module>
    _, R, t, mask = cv2.recoverPose(E, P1, P0, focal=focal, pp=pp)
cv2.error: OpenCV(4.2.0) C:\projects\opencv-python\opencv\modules\core\src\matrix.cpp:832: error: (-13:Image step is wrong) The matrix is not continuous, thus its number of rows can not be changed in function 'cv::Mat::reshape'
</code>```

Please let me know what is the problem in this code.

答: When working with odometry, you need to consider that the resulting calculation may not be valid when comparing frames. Therefore I’d suggest you add try and except statements.


问:
How do robots move parallel to a wall/line?
I have been studying robotics by myself, lately.

I noticed that there are various mapping, localization methods (popularly under SLAM), but are there concepts relating to the robot moving in a straight trajectory, like strictly parallel to a wall or a line ?

Common Scenario:

The robots (especially the differential tank drive robots) find it difficult to strictly follow a particular line and displacing itself to a point parallel to its location.

What are the methods used previously and today ?

答:
The simplest method line following robots use is 3 sensor system:
Consider the 2 block sensors, let's assume they sense color. The middle one senses white, the remaining two senses grey. This robot follows the white line in the image.

```<code>    X
        XXXXX
            XXXXX
    XX          XXXXX
     XXXXXX         XX
          XXX        XX
            XXX       X
              X       X
               X      XX
               X       XX
               X       X
               X       X
               X       X
 ^             X       X
 |       +---+ X +---+ X +---+
 |       |   | X |   | X |   |
 |       +---+ X +---+ X +---+
 |             X       X
 |             X       X
 +             X       X
               X       X
</code>```

As the robot advances, towards the turn, the left sensor evetually will sens white color. This is the trigger which makes the robot turn to the left. If the middle sensor senses grey intead of white, the robot knows that it has to turn at a sharper angle. As the left one senses grey again, and the middle one does not sens white, the robot knows that it is off track and has to turn even more sharply to the left. So there are 3 stages of triggers for turning even sharper curves to the left. If you consider the sensors not binary, but being able to sense the "closenes" of the white stripe (as it starts to reflect more light if the sensors on the side approach it) you can derive a signal which is more contous and you can create a PID line following controller based on "how much white does the side sensor sense).

Special logic is required to get back to the track once the robot left the stripe completely. In this case the sensor which last sensed white gives the direction of the line search behaviour. Once the line is found, the line following controller can be re-activated.

This is similar to wall fallowing. In that case distance to a wall gives a similar signal to which a wall following controller (e.g. a PID controller) can be tied to.

Body:
The simplest method line following robots use is 3 sensor system:
Consider the 2 block sensors, let's assume they sense color. The middle one senses white, the remaining two senses grey. This robot follows the white line in the image.

    X
        XXXXX
            XXXXX
    XX          XXXXX
     XXXXXX         XX
          XXX        XX
            XXX       X
              X       X
               X      XX
               X       XX
               X       X
               X       X
               X       X
 ^             X       X
 |       +---+ X +---+ X +---+
 |       |   | X |   | X |   |
 |       +---+ X +---+ X +---+
 |             X       X
 |             X       X
 +             X       X
               X       X

As the robot advances, towards the turn, the left sensor evetually will sens white color. This is the trigger which makes the robot turn to the left. If the middle sensor senses grey intead of white, the robot knows that it has to turn at a sharper angle. As the left one senses grey again, and the middle one does not sens white, the robot knows that it is off track and has to turn even more sharply to the left. So there are 3 stages of triggers for turning even sharper curves to the left. If you consider the sensors not binary, but being able to sense the "closenes" of the white stripe (as it starts to reflect more light if the sensors on the side approach it) you can derive a signal which is more contous and you can create a PID line following controller based on "how much white does the side sensor sense).

Special logic is required to get back to the track once the robot left the stripe completely. In this case the sensor which last sensed white gives the direction of the line search behaviour. Once the line is found, the line following controller can be re-activated.

This is similar to wall fallowing. In that case distance to a wall gives a similar signal to which a wall following controller (e.g. a PID controller) can be tied to.


问:
Time Mapping for Robot Arm Teleoperation
I have a 6DOF robotic arm that I wish to teleoperate by tracking my own hands. For the sake of simplicity, let us assume a 3DOF arm where only position is fed, and the orientation is constant.  Suppose I get my wrist pose in mm (which is my end effector) at 100 Hz. (slightly noisy in the order of a couple of mm). There are no network latency issues.

The IK solver of my robotic arm is fast, and more or less realtime (order of a millisecond). The speed for every step of the motion has to be fed in, i.e. for every position command, the speed at which to execute the motion has to be specified.

Here is my problem: The teleoperation mapping does not seem to be realtime nor smooth. If the hand moves from point A to B, the arm takes a while to get there, probably a couple of seconds delay, however, when the hand is still, then it is realtime, (since there is no movement to make, thus it is real-time). Fast motions are not mapped very well in realtime, thus the teleoperation experience is poor.

I'm sure these are general problems faced when someone gets started with teleoperation.

What is the ideal way to approach this, from a general concept of teleoperation.
There are various things to consider such as

Time taken for IK solver.

How often to the end effector (wrist) position.

Speed to feed for each motion.

Buffer size of the receiver on the robotic arm side, to process.

While I understand that one solution/method will not fit all types, a general detailed explanation of a methodology on how to approach hand teleoperation, and the various things to consider would be very helpful. What is the established method to ensure both smoothness of motion, and realtime time/latency mapping for teleoperation? While most papers deal with accuracy, and how to get the hand position, not a lot of papers discuss this issue of time and speed for operation.

Currently, this is how I approach the issue. I collect position values, and find the distance between the previous hand position (A), and the current reading. If the distance is less than some threshold (eg. 5mm), the current reading is discarded. Once the distance exceeds the threshold, it is sent to the robotic arm. The time is calculated as the distance by the time between previous position (A) and current reading. Though this works well for most cases, it fails fast random motions.
However, I am not sure if this is a general practice for teleoperation.

Finally, is solving IK for each pose required? If not, how can this be done?

答:
You have to decide what are your teleoperation priorities:

Do you want to replicate exactly the trajectory of the hand

Do you want to synchronize the current position of the hand as best as possible with the robot?

Trajectory fidelity:
This might introduce a lag between the current position of the hand and the robot, if your robot is slower than the hand motion you are trying to copy. You can refine the sampling of the hand trajectory, say to a 100ms range, but instead of sending the coordinates directly to the robot, and changing the setpoint of the robot controller every 100ms, regardless of the fact that the robot reached the position or not, you should create a motion buffet with the waypoints your robot needs to follow. You should connect these waypoint with a spline and/or use a multi-point trajectory planner to generate and execute this trajectory, by taking into account the dynamic capabilities of your robot.

Position/pose fidelity:
If you do not care about the trajectory of your hand, just he final point, you should use a PTP trajectory planner, which always brings your robot from its current point to the current position of the hand, while taking into account its dynamic capabilities.

In both cases, there is a need to plan a trajectory, not just dump setpoints on the controller. If your controller is good enough, it might be able to cope with them, but this is not guaranteed. Planning a trajectory shapes the way the setpoints are generated and makes sure that the robot can actually follow the intended motion. Yes, you might loose some dynamic performance in some cases, but the benefits outweigh this.

As for IK, if you have Cartesian position, you have 2 choices. Plan a path in Cartesian space and convert to joint angles afterwards, or convert to joint space and plan a path in joint space. Every time you transition from Cartesian to joint space you need the IK transformation, there is no way around this. If you want to follow a straight line, you have to plan in cartesian space. If you do not care about the shape of the trajectory between the waypoints, you can plan in joint space.

If you want to evaluate, where your robot is currently in relation to the intended Cartesian path, you need the forward kinematic transformation to transform your the current joint angles of your robot to Cartesian space and see where you are in relation to the intended Cartesian path.


问:
Not getting expected performance from kalman filter+mahalanobis distance
I am using a 1D lidar in one of my projects and it returns the distance it measures, in millimeters (mm). At some point in time, it gives garbage values that go as high as 10,000 or higher, when the value is expected to be under 200.

To mitigate such such outliers, I though of using a Kalman filter. Although it is something new to me, I used this MEDIUM blog as a start and tried modifying his code to suit my case.

I have the following data
. The visible peaks are the noise I added.

I create a live kalman filter as:

```<code>import numpy as np
import math
from numpy import  dot
from numpy.linalg import inv

class kf():
    def __init__(self):
        ### Defining filter
        #Transition Matrix
        self.F = np.array([[1.0]])
        #Observation Matrix
        self.H = np.array([[1.0]])
        #Process Noise Covariance
        self.Q = np.array([[0.09]])  # how much I trust the model
        #Measurement Noise Covariance
        self.R = np.array([[0.2*0.2]])
        # Control action Matrix
        self.B = np.array([1])
        #Control input
        self.U = np.array([1])

        #Covariance Matrix
        self.P = 1.0

        self.x_old = 100.0

    def filter(self, y):
        x_old = self.x_old
        F = self.F
        B = self.B
        U = self.U
        P = self.P
        Q = self.Q
        H = self.H
        R = self.R
        I = np.identity(1)
        """
        Prediction
        """
        x = dot(F, x_old) + dot(B, U)
        P = dot(F, dot(P, F.T)) + Q

        """
        Innovation
        """
        e = y - H*x
        S = dot(H, dot(P, H.T)) + R

        """
        Mahalanobis distance approximation
        """
        MD = math.sqrt(e*e)/S
        #Weighted MD
        MDw = 1/(1+(math.exp(-MD) + 0.9))
        #New Measurement Noise Covariance
        R = np.array([[4*MDw]]) # bigger the R, the lesser the trust the system will have on the sensor measurement.
        #Kalman gain
        K = dot(P, dot(H.T, inv(S)))
        """
        Update
        """
        x_new = x + dot(K, dot(e,K))
        self.x_old = x_new
        self.P = dot(I - dot(K,H), P)

        return x_new, x
</code>```

And run as instance of this class each time I read the lidar value. The results are, (ignore the blue plot)

I expected the filter to not be so severly affected by the outliers, I am new to this field and hence tried playing with the tunable parameters but to no gain :/

Where am I going wrong ? Is there any other non-computation-intense method for filtering data in real time ?

答:
You are calculating the new R, but you're not using it. You just replace the new R with the line R = self.R.

You are not removing the outliers, because you are ditching that result!


问:
Simulation of Robotic arm
I want to simulate my 5 DOF robotic arm, in such a way that the calculated Torque and mass values of each joints (Servos) defined to be tested that are they capable of physically bear all the weight and are these defined torque would be perfect or not, is it all possible to simulate in MATLAB simmechanics</p>

答:
SimMechanics has two ways to drive a joint - by angle or by torque. You can give it a position or speed and SimMechanics will auto-calculate the torque required to achieve that setpoint, or you can provide the torque and SimMechanics will auto-calculate what the resulting position change will be.

You can simulate the system first by passing in the joint position references you'd like to have, and you can trend the joint torques and forces to see what size actuator you'd like to specify.

If, for example, you wind up with some torque profile that has a really short-duration peak that is much higher than you'd like to specify the motors for, then you can do a simple motor simulation by passing a torque reference and clamping that to whatever your motor's rated torque is. Performance will degrade some, as you then can't drive to the same peak torque, but the performance hit might not be noticeable.

Title: Simulation of Robotic arm


问:
How can I automatically adjust PID parameters on the fly?
I have a simple servo system that uses a PID controller implemented in an MCU to perform the feedback. However, the properties of the system change dynamically, and so the PID parameters can never be tuned for all circumstances.

My robot is a light weight arm with back-drivable electric motors, similar to this one:

The arm performs several tasks, including picking up heavy weights, pushing and pulling objects across the desk. Each of these tasks requires different PID tuning parameters which I cannot easily predict.

What I would really like is for some higher level function which can carefully adjust the parameters in response to the arm's behaviour. For example, if it notices that the arm is oscillating, it might reduce P and increase D. Or if it noticed that the arm wasn't reaching its target, it might increase I.

Do such algorithms exist? I would be happy even if the algorithm didn't perfect the parameters immediately. E.G. the arm could oscillate a few times before the parameters were adjusted to their new values.

答:
The process you are describing is known as adaptive PID.

It seems kind of overkill though.  I have found PID to be quite robust when dealing with external disturbances and the tasks you describe do not seem beyond the capabilities of a single set of gains.


问:
Deburring Robot (Plastic Box)
For a university course I have been asked to design a rough "specification" for a system that will deburr a plastic box that appears in a workspace. Due to irregularities in the boxes edges I cannot use simple position control and must use force control.

I have so far decided on;

Using an IR sensor to detect the box has appeared in the workspace.

Use an Epson 2 axis robot to move around the work piece

Use an ATI 6 axis force sensor to maintain a constant force against the edge of the box as the deburrer/robot moves around it.

Is there a simple means of detecting the end of each side of the box ?
A 0N force value would indicate reaching the edge of a box but it could also mean a breakage in the box which was also specified. How can I distinguish between the two ?

Also does my work so far sound sensible ?

Thanks for any help

答:
Supposing all the boxes (or all the boxes being run in a batch) are the same size, and that they have rectangular cross-sections, consider fastening an L-shaped fixture to the workspace surface, at a location somewhat aside from the point where the box usually appears.  When the box does appear, push it against the L stop via a moving L-shaped fixture that is spring-loaded and has a fair amount of travel.  Upon detecting that the springs have compressed enough (or upon activation of limit switches, etc.) latch the moving L into place.  Now the box's  orientation (that is, which of four possible rotations has occurred) can be tested using force sensors, limit switches, or optical sensors.

Even with the box in a known location and orientation, it appears (from your mention of box-edge irregularities) that use of force sensors still is necessary, but presumably you will know where the end-product edges should be, so can keep deburring until the force sensors indicate close-enough results.

Title: Deburring Robot (Plastic Box)


问:
What is the state-of-the-art approach to detect moving objects from mobile robot with 2D Lidar only?
Having a setup of a mobile robot with a 360° 2D lidar (1 plane). What is the current easiest approach to detect moving object with a high reliability from a moving robot? To make it simple, we consider an indoor application.

My current considerations:

Creating a SLAM occupancy map (e.g. Hector SLAM)

conducting  a projection of the measurement points to the created 2D occupancy map from t-2. Points which are projected to the free space can be clustered as moving objects.

The issues of the above approach are the chicken-egg-problem of the new measurement points and reliability of the detection.

I've read the DATMO approach (detection and tracking moving object). However, the papers about DATMO are too complicated and too abstract for me.

Is there any reliable approach without creating a global map through SLAM? Comparing points from t and t-1 directly without estimating the robot movement/odometry is not a possible task.

答:
Obviously this problem can get very complicated when you just consider a 2D lidar. So I'm gonna try to keep my answer simple. You will want to estimate the position of the robot (odometry) as a minimum in order to project everything into a global space, rather than the local vehicle space. You can technically do it locally, but I personally don't think it is clean and you will run into more issues.

If you wanted to just track a moving object, you can use something simple like k-means clustering to estimate the center of the moving objects based on N scan point. Anything that doesn't move in the global space between t and t-1 (within a reasonable margin of error) could be filtered out. Anything that does move can be estimated, flagged, and tracked with something like a kalman filter to cover potential misdetections or if the obstacle stops moving. Obviously this isn't a flawless idea, I'm just trying to give something to go off of based on your sensing capabilities.


问:
Relying on odometry for 100 meters
I am using 4 mecanum wheeled robot and have recently tested its wheel odometry. It was accurate to about 5-10 cm.

How sensible is it to use the wheel odometry for 100 meters after which I have some external sensors that can correct the pose and orientation?

P.S. For orientation estimation I am using IMU, wheel odometry is only used for translational calculation

答: 5-10cm isn't too bad after 100m. Obviously this is kind of subjective because I don't know what hardware you are using. You could be using the cheapest encoders on the market for all I know. That being said, it's pretty good. I'd expect to see around 0.1% error in odometry without anything fancy like SLAM.</p>

Title: Relying on odometry for 100 meters


问:
Is the covariance matrix in the extended Kalman filter guaranteed to be positive definite (ignoring numerical errors)?
I understand that due to numerical errors (e.g., round off error and machine precision) that the covariance matrix may not be positive definite, but if computers had infinite precision, is the covariance positive definite?

Along this line, are the noise matrices required to be positive definite as well if this is the case? This seems like a requirement just to be able to compute the residual covariance, but if I search EKF, the definiteness isn't mentioned for any of the matrices (except for the case of numerical errors).

答:
It is always guaranteed to be positive semi definite.

That being said you have to somewhat deliberately set up your system to be that way. So essentially yes it is always positive definite.

Reasoning:

Covariance matrix by definition is always positive semidefinite.

$A^TA$ is always positive semidefinite (Takes care of the propagation matrix and the information matrix)

Addition and subtraction of positive semidefinite matrices are also positive semi definite. (Also applies to positive definite)

Since we are ignoring machine precision the only way for a matrix to be positive semidefinite(PSD) is if all of your matrices end up being positive semidefinite. This includes your covariances, Propogation matrix, and information matrix. If one of them ends up being definite(PD) then the result will be positive definite.

$PSD + PD = PD$

A semidefinite matrix occurs when you have problems with your observability. Essentially you are trying to measure something that is impossible for you to observe. Crazy example would be you are tracking a robot position with IMU+GPS and additionally are trying to estimate temperature. Your IMU+GPS combination can not observe the temperature so it is unobservable.

Along this line, are the noise matrices required to be positive
definite as well if this is the case? This seems like a requirement
just to be able to compute the residual covariance, but if I search
EKF, the definiteness isn't mentioned for any of the matrices (except
for the case of numerical errors).

Again you could potentially make one of your noise matrices positive semidefinite, but then you are probably making some sort of modeling mistake. A covariance matrix is only positive semidefinite if one of the variables is a linear combination of the others.


问:
error with dimensions in Simulink
I am trying to simulate a cartesian regulator for a unicycle with Simulink and I am having some problems.

My simulink model is the following:

and when I run it I get the following error message:

so it seems I am doing something wrong with the dimensions.

I have added also some ToWorkspace blocks since I want to use the results to do some plots.

I have not very good at using Simulink, so I am having a lot of problem and I don't know how to solve the error message I get.

Cab somebody please help me?

答:
Split the output of the regulator into to seperate signals.

```<code>function [v_out, omega_out] = C_REG(q)
...
v_out = v;
omega_out = omega;
</code>```

You are probably compounding the signals in a different way then Simulink is trying to demux it. You can use a Mux block if you really want just one signal path.

There is also a change that simply changing the last line to a row vector instead of a column vector could also help

```<code>inputs=[v,omega];
</code>```

Title: error with dimensions in Simulink

Body:
Split the output of the regulator into to seperate signals.

function [v_out, omega_out] = C_REG(q)
...
v_out = v;
omega_out = omega;

You are probably compounding the signals in a different way then Simulink is trying to demux it. You can use a Mux block if you really want just one signal path.

There is also a change that simply changing the last line to a row vector instead of a column vector could also help

inputs=[v,omega];


问:
How do I call Matlab code into Simulink for doing a simulation for doing plots?
I have built a simulink model for performing cartesian regulation on a unicycle:

and plot the results with a Matlab code

but, I am not very confident with Simulink and so I don't know how to call these codes into my Simulink model.

Can somebody please help me?

答:
There's a Matlab function block. Follow the example and your inputs and outputs will create source/sink connections on the Simulink block to wire in values.

I would only put the controller in there, and don't use the clear all command - if it's not persistent then a variable inside a function ceases to exist between function calls. Every call to the function wipes all the internal variables inside that function.

If you're wanting your data to be plotted at the end then you can use a To Workspace block and send the data out that way and plot from the console or script after the Simulink project finishes.


问:
How can I find a robot's distance from an object given FOV, bounding box measure, and image width and height in pixels?
The robot is standing in the middle of the arena. I am confused on how to convert the pixels into a real life distance and how to use the bounding box. Starting help is appreciated.</p>

答:
The best (and cheapest) method would be to use AprilTags. I have added the python bindings for it here.

For a great starting point, refer to this blog that explains the setup in C++.

It can not only tell you the distance but also the orientation of the object

I hope it helps.


问:
LiDAR Feature Extraction
I'd like to extract "features" of an environment scanned with a 2D LiDAR.

I tried to create a temporary occupancy map to extract "corners" with the Harris Corner Detector. However LiDAR data is noisy, it creates false corners even after applying a Gaussian filter on the map. I wonder if there are more reliable algorithms for this.

Another method I tried is to use raw data output of the LiDAR, i.e. the $(r,\theta)$ vector, and calculate its first and second derivatives and choose "abrupt changes" as landmarks. This has a major drawback, it is harder to track landmarks when LiDAR position changes since $(r,\theta)$ vectors are totally different.

I appreciate any help. Thanks.

答:
In general you have to assign some probability distribution to your map or cells individually and update them iteratively.

In the very simple case you can just apply a low-pass filter to each of your cells. Like that: $p[t] = a*p[t-1] + (1-a)*I(occupied)$
where I is an indicator function which is set to 1 if current lidar measurement shows that cell is occupied and to 0 otherwise.

Better way is to look at SLAM methods like FastSLAM. Which use particles and Kalman filters underneath. Comparing to the case I described above, Kalman filter will produce better estimates, because  it is optimal in the sense that it minimizes variances thus giving you as precise estimate of a mean as possible.

Title: LiDAR Feature Extraction


问:
How does sensor fusion help in robot localization
Good day...
Sorry for bigner quesion, but really I need some friendly explanation rather than long reference
I have 9 dof mpu9250 imu and gps. I found some source that calculate euler angle in ardunio
How much euler angle help me in localization?
Can I use the mpu9250 raw data for localization?
If i fuse gps and imu what is the out put of filter?
Can i determine x y z position using imu and gps?</p>

答:
I think another good place to jump right in is to examine the robot_localization package that is used in the ROS community. It implements both EKF and UKF sensor fusion estimators. The package is widely used, supported, and documented which makes it ideal for someone looking to start understanding how robot localization with multiple sensors works at a practical level. The robot_localization ROS Answers tag is very very active and is a good place to field questions about he package if you run in to them while experimenting with it.

For the particular case of implementing GPS and imu fusion look at robot_localization Integrating GPS Data.

I know this may not be a narrow and specific answer, but I think it could lead any one interested in the area onto a very useful path of discovery and has helped me build an understanding of the subject over the last year.


问:
With a 6-axis robot, given end-effector position and range of orientations, how to find optimal joint values
Given a six-axis articulated robot arm holding a tool at its end-effector,  if I have a desired tool position and tool orientation,  there will be exactly 1 solution to the inverse kinematics equation for the robot to reach that position.

(or rather up to 16 different solutions, depending on range of the joints)

But if the robot is holding something like a pen, and I want the robot to mark a specific point with that pen on the target, then I do not care how the pen is oriented, as long as it is perpendicular to the marked surface.

So the inverse-kinematics equation will have infinitely many solutions.

How can I pick among these solutions the joint configuration that is closest to the current configuration: the one that will require the least amount of movement to reach?

(or the joint configuration that is optimal according to some other similar criterion, such as that all joint angles are furthest from their maximum and minimum?)

答:
There is a nice closed form to this.  Let's say that we don't care what $r_z$ is (i.e. we don't care how we change it).

$$
\mathbf{J}^{-1}\dot{\mathbf{X}}=
\dot{\mathbf{\Theta}}=
\begin{bmatrix}
\vec{j_{1}} & \vec{j_{2}} & \vec{j_{3}} & \vec{j_{4}} & \vec{j_{5}} & \vec{j_{6}}
\end{bmatrix}
\begin{bmatrix}
\dot{x}\\
\dot{y}\\
\dot{z}\\
\dot{r_x}\\
\dot{r_y}\\
\dot{r_z}
\end{bmatrix}
=
\begin{bmatrix}
\dot{\theta_1}\\
\dot{\theta_2}\\
\dot{\theta_3}\\
\dot{\theta_4}\\
\dot{\theta_5}\\
\dot{\theta_6}\\
\end{bmatrix}
$$
Where $\vec{j_{i}}$ is the $i^{th}$ column of $\mathbf{J}^{-1}$.
We can break up $\dot{\mathbf{\Theta}}$ into the portion that depends on $\dot{r_z}$ and the portion that doesn't.
$$
\dot{\mathbf{\Theta}}=
\dot{\mathbf{\Theta}}_{\dot{x}\dots \dot{r_y}} + \dot{\mathbf{\Theta}}_{\dot{r_z}}
\\
\dot{\mathbf{\Theta}}_{\dot{r_z}}=
\vec{j_{6}}
\dot{r_z}
$$
So now the game has become, let's minimize
$$
(\dot{\mathbf{\Theta}}_{\dot{x}\dots \dot{r_y}} + \dot{\mathbf{\Theta}}_{\dot{r_z}})^T
D(\dot{\mathbf{\Theta}}_{\dot{x}\dots \dot{r_y}} + \dot{\mathbf{\Theta}}_{\dot{r_z}})
$$
for some diagonal matrix $D$ like ronalchn said above.  I'm going to use $A=\dot{\mathbf{\Theta}}_{\dot{x}\dots \dot{r_y}}$ and $B=\dot{\mathbf{\Theta}}_{\dot{r_z}^*}$ for easier viewing.

We can expand this out to
$$
A^TDA+2B^TDA+B^TDB\\
\text{or}\\
A^TDA+2\dot{r_z}\vec{j_{6}}^TDA+\dot{r_z}^2\vec{j_{6}}^TD\vec{j_{6}}
$$

Now we have an easy equation to differentiate on $\dot{r_z}$.  We find the derivative with respect to $\dot{r_z}$ and set it to $0$.
$$
2\vec{j_{6}}^TDA+2\dot{r_z}\vec{j_{6}}^TD\vec{j_{6}}=0\\
\dot{r_z} = \frac{-\vec{j_6}^TDA}{\vec{j_6}^TD\vec{j_6}}
$$
This minimizes the joint angle "distance" between your two poses.


问:
Understanding across gravity compensation, demonstration (Kinesthetic Teaching vs. Teleoperation) and inverse dynamics
I first state my humble probably wrong opinion on the connection of these concepts: In gravity compensation mode, each joint has equilibrium as its set point, a human demonstrator can effortlessly move the robot arm to conduct a task like pick up a ball, during this process, the orientation, angular speed and acceleration of the joint is recorded, as well as the Torque (Gravity Compensation torque plus the anti-torque the human exerted).   An inverse dynamic model, mapping from (orientation, angular speed, acceleration) to Torque can be built.  At reproduction phase,  the path is planned by a motion encoder trying to reproduce the demonstrated trajectory, then the inverse dynamics learned is used to exert torque to achieve each point in the planned path.  Is the statement correct?

Question1 : If one sees a multi-joint robot (the industry robot for production line) sitting there without power input, there is mechanical constraint which will govern the relative orientation of each joint. In gravity compensation mode, it is each separate joint can rest in arbitrary position, right?

But the human demonstrator can not ensure the individual joint always in the equilibrium state, there must be some acceleration, in this case, how the extra torque from the Gravity Compensation mechanism is executed?  Is there a book about gravity compensation or the mechanics analysis of Kinesthetic Teaching?

Question2: haptic control is advantageous for gripping than simply use a joystick which might break the object or not executing enough force, is there any sources explaining technical details about how that works?

答:
No, your statement is not entirely correct. During the teaching of the robot the trajectory in geometrical terms (positions velocities, maybe accelerations) are recorded, not torque. The trajectory is then replayed (although in many cases only some way points are recorded not in entire trajectory)

The inverse dynamics is not learned. All parameters of the robot required for a dynamic model and inverse dynamic model are available for robot manufacturers about their robot. However, they do not make these parameters public.

At the "reporoduction phase" as you call it, the inverse dynamics model does not play an important role. It is simply a geometrical path, a trajectory  (velocities) is planned for the path and the motion controller executes this motion, by setting the set-point of the position controller, the feed forward signal of the velocity controller and here the inverse dynamics might be used to set the feed forward signal of the torque controller, but this only improves the jerkiness of the motion, it is not essential for obtaining movement.

Your Question 1 is not quite clear. An industrial robot, without power is held in place by many factors. First holding brakes at each joint, used to block the axis mechanically. If the breaks are released, there is still a good probability that the robot will not fall due to friction in many axes (in the gearbox) and due to a hydro pneumatic spring used in the second axis (most subjected to gravitational forces). Robots which have this hydropnenumatic spring set up to a high pressure (exerted force) can lift up instead of falling down on releasing the brakes without power to the motors.

For large industrial robots it is no customary to have a "free-joint" mode.

Haptic feedback is has multiple variants, vibration in a joystick is also a haptic feedback. I assume you refer to force feedback. I do not think force feedback is extensively used in the way you describe. Having the gripper force translate to a joystick force feedback would be beneficial if someone wants to learn to grip with a joystick, but the breaking limit is then learned by the human after many trials intuitively and is not advantageous to make a process dependent on the intuitive knowledge of on human operator. Instead, the breaking force can be determined and the grasping force can be set to a certain upper limit. This would not necessarily work for different grasping positions, but that is also true for a force feedback solution. It is not similar to how a human would feel the force while grasping something, since our fingers feel both force and slippage, such a feedback is not possible with technical systems.


问:
How to make my robot move parallel to edge of the table?
I have an autonomous differential drive robot that moves on the floor and its purpose is to always move parallel to the longer edge of the table (just assume that the table never ends). It looks something like :

Now, the robot will be using this same method to align itself to the table edge for the first time. Also, when it travels forward (along the edge), it is prone to deviate from its path due to various reasons. Again, this method will guide it to stay on the right track.

I can't use any line-following method.

答:
I can't use any line-following method.

Actually, you are quite wrong, the way I understand the problem. It is a line-following. It is just that the line is not painted (like on the road). The line is the edge of the table.

The way I see it, your robot needs to look up, and detect the line "painted" by the edge of the table on the ceiling. Once the line is detected, just follow it.

I have in mind 2 ways to detect the edge of the table.

Contrast based: using an imaging sensor (e.g., camera), detect the edge of the table by detecting the contrast / color change. Based on this, adjust the trajectory.

Distance measurement: an IR sensor (practically, more) can be used to measure the distance to the first obstacle (table or ceiling), vertically. It should be easy to do, considering that the ceiling is significantly higher than the table.


答:
The problem you are describing is quite similar to line following from a controls perspective, there is a difference in how the line is detected.

Line following robots use a wide variety of ways to detect line. Simplest is probably photo-resistors or phototransistors, however magnetic stripes (lines) and hall effect sensors have been quite popular in the industrial world some years ago.

Industrial applications of mobile robots have now switched to camera (and laserscanner) based systems. In many cases the camera is downward facing, however upward facing cameras are also not uncommon (not exactly industrial, but roomba vacuums use also partially upward facing cameras)

If you mount a camera on your robot, facing upwards, and the edge of the table is visible, you can use edge detection algorithms to detect the edge of the table. If you are lucky and there is a high contrast between the area under the table and above the table edge detection will perform quite well.

The edge is your line, but you still have to map this to the line you want to follow. The line being in the middle of the camera image is not necessarily what you want, but you can apply simple transformations to make this line your path to follow. If you can deduce a signed distance function (negative distance one side of the line, positive distance on the other side of the line, zero on the line) you can use this signed distance function as an input to your line following controller.

The only difference to a more common line following approach is how you detect the line and com up with the current position relative to the line. Everything else is like a classical line following problem.

Title: How to make my robot move parallel to edge of the table?

Body:
I have an autonomous differential drive robot that moves on the floor and its purpose is to always move parallel to the longer edge of the table (just assume that the table never ends). It looks something like :

Now, the robot will be using this same method to align itself to the table edge for the first time. Also, when it travels forward (along the edge), it is prone to deviate from its path due to various reasons. Again, this method will guide it to stay on the right track.

I can't use any line-following method.


问:
Sensor fusion of GNSS and IMU using UKF
I do have a land-based robot with an IMU and a GNSS receiver.

From the IMU, I get the velocity and acceleration in both $x$ and $y$ directions.

From the GNSS receiver, I get the latitude and longitude.

I want to fuse these data to get my exact position on the field. To get the most accurate position I want to use an Unscented Kalman Filter (UKF).

However, I don't really understand the concept of fusing these data. Most of the books I found just fused the IMU data and used it together with the GNSS data but by my understanding, I should get a more precise position when I fuse IMU and GNSS.

Can someone explain to me the concept of doing so or has a good source/tutorial?

This is for my bachelor thesis. Help would be much appreciated. Thanks!

答:
I cannot recommend the robot_localization package in ROS enough. Please see my response to another post I made today How does sensor fusion help in robot localization. The documentation for the package is superb and I think, assuming you have ROS avaliable, you can have a EKF or UKF up and running in a week.

And as it regards the authors claim in a comment to another answer that the UKF provides "a better way of linearizing the non linear functions" I think it should be clear that is not true. A UKF does not linearize anything. It just picks some well placed samples (called sigma points) and passes them through the nonlinear functions, calculating weighted approximations to the expected values and covariances as it goes along. It is not calculating a Jacobian and linearizing about the mean like the EKF does.

Title: Sensor fusion of GNSS and IMU using UKF


问:
Res-Scaling method beats PID controller. How?

Scenario:

I have a robotic arm that has a (1D) lidar on its end, to orient itself with respect to the slant table in front of it. The lidar returns pretty accurate distance values in mm. The motor controlling the arm's motion towards the table (or away from it), uses time-duration values in seconds provided by the user and moves the arm fast or slow, at a pre-defined velocity.

e.g., arm_up(0.5) pulls the arm up at some velocity, for 0.5 seconds.

What am I trying to do:

I am trying to control the the position of the arm before it comes to a certain distance from the table (200 mm). That is, it moves more when far and less when near it.

Attempts:

I planned to use a controller to achieve this. First attempt included using a P-controller (P: Proportional) which involved fixing a gain Kp that gets multiplied with the error. I wanted the output of this controller to be in the range 0.1 to 1.0. I tried multiple values of Kp but did not get factor values proportional to the distance to be covered. When I set Kp=0.008, It works well when the error = 500 (mm), and the factor comes out to be = error x Kp, or 500 x 0.008=0.4, but when the error=10mm, factor=10 x 0.008=0.8 which is too below for the actuator to do anything, owing to the fact that all values must be in the range 0.1 to 1.0. Alternatively, setting higher values for Kp (as 0.01), requires me to clamp the output to a fixed value.

```<code>Kp = 0.003
if(LIDAR_1 > 200):
                # P Controller
                error = abs(200 - LIDAR_1)
                duration = error * Kp
                # To keep everything < 1.0
                factor = duration%1.0
                arm_down(factor)
</code>```

Then, I used the feature scaler to scale the range (given by the lidar) to the desired range (0.1 to 1.0). To filter very high values, I used a mod operator a.k.a % in Python.

```<code>if(LIDAR_1 > 200):
                # Using Feature Scaling
                duration = ((LIDAR_1-200)*0.8/800) + 0.2
                # To keep everything < 1.0
                factor = duration%1.0
                arm_down(factor)
</code>```

How is the (P) controller not standing up to its task ? Is it not meant to provide a value of Kp that satisfy all the cases (arm's distant or close encounters)

答:
How is the (P) controller not standing up to its task ?

Well, just like you said - how is it not standing up to its task? What is it doing that makes you think it's not working? You said,

I tried multiple values of Kp but could not succeed

Nobody here knows what that means. "Could not succeed" could be a lot of problems. My guess is that you're trying to regulate something with only a proportional controller. Integral gains are the only way to drive steady-state error to zero.

What are some other things you could be doing wrong? Well, for starters, you said:

I am trying to control the the position of the arm [so]... it moves more when far and less when near [the table].

This sounds like you want a signed error, to drive your arm in two directions, yet your code you provided has error calculated as:

```<code>error = abs(200 - LIDAR_1)
</code>```

If your error only has one sign (positive) then your arm only moves in one direction. Also, your error is calculated only with LIDAR_1, even though your entry into that calculation depends on LIDAR_1 and LIDAR_2. Are you sure that you don't want something like

```<code>error = LIDAR_2 - LIDAR1
</code>```

or something similar? You haven't provided any diagrams showing what your values are, what the significance of 200 is, what angle it is you're trying to control, or how you're calculating that angle.

Next, following your code all the way through:

```<code>error = abs(200 - LIDAR_1)
duration = error * Kp
# To keep everything < 1.0
factor = factor%1.0
arm_down(factor)
</code>```

Let's look at your code line-by-line:

Calculate the (absolute value of an) error,

Modify the error with a proportional gain to make a "duration,"

Modify a "factor,"

Use the factor to run arm_down.

Note: At no point in there did you use the duration. Maybe you meant factor = duration % 1.0?

But, finally, modulus is not your friend. This is definitely NOT what you want for it to be doing.

On the topic of the modulus, you said

to scale the range (given by the lidar) to the desired range (0.1 to 5.0). To filter very high values, I used a mod operator a.k.a % in Python.

First, you say that you want the desired range to be 0.1 to 5.0, but you're doing value % 1.0, so you're only going to get numbers between 0 and 0.999.

Second, you're making your control signal "wrap." Assuming you actually did value % 5.0, which I think is what you were intending given your stated desired range, then you get the following:

```<code>4.8 % 5.0 = 4.8
4.9 % 5.0 = 4.9
5.0 % 5.0 = 0
5.1 % 5.0 = 0.1
5.2 % 5.0 = 0.2
</code>```

If you think of this like a cruise-control system, a very fast acceleration would have you push the pedal all the way to the floor, but if you wanted to accelerate even faster then your modulus operation actually has the controller take its foot completely off the pedal.

What you need instead is to clamp the value, like

```<code>if(duration > 5.0)
    duration = 5.0
</code>```

The best thing you can do for us, and yourself, is to make a drawing that shows where your lidar units are, what measurement you're expecting from them, and how those measurements relate to an angle you're trying to control.

You should then calculate that angle, using the diagram as a guide for developing the equation, and use the angle instead of raw lidar readings in your code.

Finally, structurally speaking, your PID/controller code should accept the angle/error and use that to calculate a raw control signal. If you have some downstream process that needs it to be in a particular range, then you can modify/saturate/clamp that signal inside that downstream process. This keeps your code more modular and easier to troubleshoot.

Body:
How is the (P) controller not standing up to its task ?

Well, just like you said - how is it not standing up to its task? What is it doing that makes you think it's not working? You said,

I tried multiple values of Kp but could not succeed

Nobody here knows what that means. "Could not succeed" could be a lot of problems. My guess is that you're trying to regulate something with only a proportional controller. Integral gains are the only way to drive steady-state error to zero.

What are some other things you could be doing wrong? Well, for starters, you said:

I am trying to control the the position of the arm [so]... it moves more when far and less when near [the table].

This sounds like you want a signed error, to drive your arm in two directions, yet your code you provided has error calculated as:

error = abs(200 - LIDAR_1)

If your error only has one sign (positive) then your arm only moves in one direction. Also, your error is calculated only with LIDAR_1, even though your entry into that calculation depends on LIDAR_1 and LIDAR_2. Are you sure that you don't want something like

error = LIDAR_2 - LIDAR1

or something similar? You haven't provided any diagrams showing what your values are, what the significance of 200 is, what angle it is you're trying to control, or how you're calculating that angle.

Next, following your code all the way through:

error = abs(200 - LIDAR_1)
duration = error * Kp
# To keep everything < 1.0
factor = factor%1.0
arm_down(factor)

Let's look at your code line-by-line:

Calculate the (absolute value of an) error,

Modify the error with a proportional gain to make a "duration,"

Modify a "factor,"

Use the factor to run arm_down.

Note: At no point in there did you use the duration. Maybe you meant factor = duration % 1.0?

But, finally, modulus is not your friend. This is definitely NOT what you want for it to be doing.

On the topic of the modulus, you said

to scale the range (given by the lidar) to the desired range (0.1 to 5.0). To filter very high values, I used a mod operator a.k.a % in Python.

First, you say that you want the desired range to be 0.1 to 5.0, but you're doing value % 1.0, so you're only going to get numbers between 0 and 0.999.

Second, you're making your control signal "wrap." Assuming you actually did value % 5.0, which I think is what you were intending given your stated desired range, then you get the following:

4.8 % 5.0 = 4.8
4.9 % 5.0 = 4.9
5.0 % 5.0 = 0
5.1 % 5.0 = 0.1
5.2 % 5.0 = 0.2

If you think of this like a cruise-control system, a very fast acceleration would have you push the pedal all the way to the floor, but if you wanted to accelerate even faster then your modulus operation actually has the controller take its foot completely off the pedal.

What you need instead is to clamp the value, like

if(duration > 5.0)
    duration = 5.0

The best thing you can do for us, and yourself, is to make a drawing that shows where your lidar units are, what measurement you're expecting from them, and how those measurements relate to an angle you're trying to control.

You should then calculate that angle, using the diagram as a guide for developing the equation, and use the angle instead of raw lidar readings in your code.

Finally, structurally speaking, your PID/controller code should accept the angle/error and use that to calculate a raw control signal. If you have some downstream process that needs it to be in a particular range, then you can modify/saturate/clamp that signal inside that downstream process. This keeps your code more modular and easier to troubleshoot.


问:
How do Particle Filters give estimates of uncertainty?
In the Kalman Filter the final covariance matrix is the estimate of the filter's uncertainty.
How does one do so in Particle filters? Is it just the variance among the particles for each state?

If so, then how do you deal with crosscorrelation?

答:
For a simple case, given a particle set, calculate the weighted sum of the particles by iterating over each particle $i$ in the set $N$:
$$\mu_{x} = \Sigma_{I=1}^{N} w_{i} x_{i}$$
And then calculate the weighted covariance for each particle $i$ in the set $N$ and sum them up:
$$P_{xx} = \Sigma_{I=1}^{N} w_{i} (x_{i}-\mu_{x})(x_{i}-\mu_{x})^{T}$$
Where $w_{i}$ is the weight of the i(th) particle and $x_{i}$ is the state vector (column vector in this case) represented by that particle.


问:
Defining frames for 5DOF robotics arm
For examples if i have this robotic arm:
Example, for the base rotation (5th DOF in the clip at 0:58), we know that the Z axis for that joint will be the same as the Z axis for the base frame{0}, but I don't know about Y and Z axises of the base rotation respects to the base frame, should they be the same or not ?

And one more thing, for defining the frame of the base rotation (at 0:58 in the clip), the vertical arm pitch (at 0.47 in the clip) and the horizontal arm pitch (at 0:46 in the clip), it's pretty easy, but I don't know how to continue for defining the frame of wrist roll (at o.12 in the clip) and wrist pitch (0.23 in the clip) since the angle between the Z axis of wrist roll and the wrist pitch is now 90o.

 Thank you very much.

答:
Follow the Denavit-Hartenburg parameters, and you'll get easy equations for the transformation matrices between each frame.

The $z_n$-axis is in the direction of the $(n+1)^{th}$ joint axis

The $x_n$-axis is collinear to the common normal: $x_n = z_n × z_{n-1}$ If there is no unique common normal (parallel $z$ axes), the direction of $x_n$ is from $z_{n-1}$ to $z_n$.

The $y$-axis follows from the $x$- and $z$-axis by choosing it to be a right-handed coordinate system.

Title: Defining frames for 5DOF robotics arm


问:
My Pick and Place Program Not Working
hope you are all well.

I am trying to finish a capstone project on Coursera. It asks me to carry a cube object from an initial position to goal. This video shows the movement.

Project website: http://hades.mech.northwestern.edu/index.php/Mobile_Manipulation_Capstone

On the simulation software, simulation commands are sent as 13 element row vectors. The vector information looks like this:

```<code>chassis phi, chassis x, chassis y, J1, J2, J3, J4, J5, W1, W2, W3, W4, gripper state
</code>```

J1 to J5 are robot arm joints and the W1 to W4 are mobile wheel angels. Gripper state is 0 if the gripper is open and 1 if the gripper is closed.

In the project page, it tells me to create 3 separate functions.

First function (NextState) takes 3 inputs:

A twelve-vector containing robot configuration (without gripper state)

A 9 vector containing joint and wheel speeds (1 to 5 for joint speeds and 6 to 9 are wheel speeds)

A timestep t

A maximum value for the arm joints and wheels (can be set as a very high number if desired)

It produces 1 input:

12 vector containing the robot configuration after timestep t.

Second function (TrajectoryGenerator) takes 6 inputs:

Initial end-effector configuration, Tse

Initial cube configuration, Tsc_initial

Desired cube configuration, Tsc_final

End effector related to cube configuration while grasping, Tce_grasp

End effector related to cube configuration before and after grasping, Tce_standoff

Trajectory reference configurations per second, k.

Second function produces 1 output:

1 13 by m matrix containing reference robot configurations in each row (13 th row contains gripper state).

Third function takes 5 inputs:

Actual end effector configuration, X or Tse

Reference end effector configuration, Xd or Tse_d

Next reference end effector configuration, Xd_next or Tse_d_next

PI gain Kp and Ki

Timestep t

Actual joint configuration, thetaList

Third function produces 4 outputs:

Feed-forward reference twist, Vd

End-effector twist, in end-effector frame, V.

Je to calculate controls vector

controls vector, u or u_theta_dot

Xerr to inspect the reference error.

Here are my function implementations:

```<code>function newConfiguration = NextState(currentConfiguration, controls, delta_t, limit)

%% Assigning the properties of the robot:
l = 0.47/2;
w = 0.30/2;
r = 0.0475;

%% Assigning current and new configurations and speeds:
currentJoints = currentConfiguration(4:8)';
currentWheels = currentConfiguration(9:12)';
armJointSpeeds = controls(1:5)';
wheelSpeeds = controls(6:9)';
newJoints = currentJoints + (armJointSpeeds .* delta_t);
newWheels = currentWheels + (wheelSpeeds .* delta_t);
Tsbk = [cos(currentConfiguration(1)), -sin(currentConfiguration(1)), 0, currentConfiguration(2); sin(currentConfiguration(1)), cos(currentConfiguration(1)), 0, currentConfiguration(3); 0, 0, 1, 0.0963; 0, 0, 0, 1];

%% Calculating deltaTheta to initiate the odometry process:
deltaTheta = newWheels - currentWheels;
thetaDot = deltaTheta/delta_t;

Vb = (r/4) * [-1 / (l + w), 1 / (l + w), 1 / (l + w), -1 / (l + w); 1, 1, 1, 1; -1, 1, -1, 1] * thetaDot;
Vb6 = [0; 0; Vb; 0];
newChassisRelative = expm(VecTose3(Vb6));
Tsbk1 = Tsbk * newChassisRelative;

phi = acos(Tsbk1(1,1));
x = Tsbk1(1, 4);
y = Tsbk1(2, 4);

chass = [phi x y];

newConfiguration = [chass'; newJoints; newWheels]';

end
</code>```

................

```<code>function movement = TrajectoryGenerator(Tse_initial, Tsc_initial, Tsc_final, Tce_grasp, Tce_standoff, k)

%% Sequence:
%  1. Arm moves above the cube.
%  2. Arm reaches the cube.
%  3. Arm grasps the cube.
%  4. Arm returns to the position 1
%  5. Arm goes to final position.

Tse_standoff = Tsc_initial * Tce_standoff;

Tse_grasp = Tsc_initial * Tce_grasp;

Tse_final = Tsc_final * [cosd(135), 0, sind(135) 0; 0, 1, 0  0; -sind(135), 0, cosd(135), 0; 0 0 0 1];

% Standing on top of the cube tracejtory:
traj1 = CartesianTrajectory(Tse_initial, Tse_standoff, 8, (8*k) / 0.01, 5)

traj2 = CartesianTrajectory(Tse_standoff, Tse_grasp, 8, (8*k) / 0.01, 5);

traj3 = CartesianTrajectory(Tse_grasp, Tse_standoff, 8, (8*k) / 0.01, 5);

traj4 = CartesianTrajectory(Tse_standoff, Tse_final, 8, (8*k) / 0.01, 5);

for i = 1 : length(traj1)
    trajectory1(i, :) = [traj1{i}(1,1) traj1{i}(1,2) traj1{i}(1,3) traj1{i}(2,1) traj1{i}(2,2) traj1{i}(2,3) traj1{i}(3,1) traj1{i}(3,2) traj1{i}(3,3) traj1{i}(1, 4) traj1{i}(2, 4) traj1{i}(3, 4) 0];
end

for i = 1 : length(traj2)
    trajectory2(i, :) = [traj2{i}(1,1) traj2{i}(1,2) traj2{i}(1,3) traj2{i}(2,1) traj2{i}(2,2) traj2{i}(2,3) traj2{i}(3,1) traj2{i}(3,2) traj2{i}(3,3) traj2{i}(1, 4) traj2{i}(2, 4) traj2{i}(3, 4) 0];
end

index = size(trajectory2);

for i = 1 : 15
    trajectory3(i, :) = [traj3{i}(1,1) traj3{i}(1,2) traj3{i}(1,3) traj3{i}(2,1) traj3{i}(2,2) traj3{i}(2,3) traj3{i}(3,1) traj3{i}(3,2) traj3{i}(3,3) traj3{i}(1, 4) traj3{i}(2, 4) traj3{i}(3, 4) 1];
end

for i = 16 : length(traj3)
    trajectory3(i, :) = [traj3{i}(1,1) traj3{i}(1,2) traj3{i}(1,3) traj3{i}(2,1) traj3{i}(2,2) traj3{i}(2,3) traj3{i}(3,1) traj3{i}(3,2) traj3{i}(3,3) traj3{i}(1, 4) traj3{i}(2, 4) traj3{i}(3, 4) 1];
end

trajectory2(:, 13) = 0;
trajectory2(1:35, 13) = 0;

for i = 1 : length(traj4)
    trajectory4(i, :) = [traj4{i}(1,1) traj4{i}(1,2) traj4{i}(1,3) traj4{i}(2,1) traj4{i}(2,2) traj4{i}(2,3) traj4{i}(3,1) traj4{i}(3,2) traj4{i}(3,3) traj4{i}(1, 4) traj4{i}(2, 4) traj4{i}(3, 4) 1];
end
index = size(trajectory4);
trajectory4(index(1)-20:index(1), 13) = 0;

%% traj2_5 is used for the arm to wait for few seconds before grasping.
for i = 1 : 10
traj2_5{i} = traj2{length(traj2)};
end

%% traj4_5 is used for the arm to wait for few seconds before releasing the cube.
for i = 1 : 50
traj4_5{i} = traj4{length(traj4)};
end

for i = 1 : length(traj4_5)
trajectory4_5(i, :) = [traj4_5{i}(1,1) traj4_5{i}(1,2) traj4_5{i}(1,3) traj4_5{i}(2,1) traj4_5{i}(2,2) traj4_5{i}(2,3) traj4_5{i}(3,1) traj4_5{i}(3,2) traj4_5{i}(3,3) traj4_5{i}(1, 4) traj4_5{i}(2, 4) traj4_5{i}(3, 4) 0];
end

for i = 1 : length(traj2_5)
trajectory2_5(i, :) = [traj2_5{i}(1,1) traj2_5{i}(1,2) traj2_5{i}(1,3) traj2_5{i}(2,1) traj2_5{i}(2,2) traj2_5{i}(2,3) traj2_5{i}(3,1) traj2_5{i}(3,2) traj2_5{i}(3,3) traj2_5{i}(1, 4) traj2_5{i}(2, 4) traj2_5{i}(3, 4) 1];
end

movement = [trajectory1; trajectory2; trajectory2_5; trajectory3; trajectory4; trajectory4_5];
</code>```

.........

```<code>function [Vd, V, Je, u_theta_dot, Xerr] = FeedbackControl(X, Xd, Xd_next, Kp, Ki, delta_t, thetaList)

%% Arm properties.
Blist = [0 0 1 0 0.033 0; 0 -1 0 -0.5076 0 0; 0 -1 0 -0.3526 0 0; 0 -1 0 -0.2176 0 0; 0 0 1 0 0 0]';
l = 0.47/2;
w = 0.30/2;
r = 0.0475;
F = (r/4) * [-1/(l + w), 1/(l + w), 1/(l + w), -1/(l + w); 1 1 1 1; -1 1 -1 1];
sizee = size(F);
m = sizee(2);
zeross = zeros(1, m);
F6 = [zeross; zeross; F; zeross];

Tb0 = [1 0 0 0.1662; 0 1 0 0; 0 0 1 0.0026; 0 0 0 1];
M0e = [1 0 0 0.033; 0 1 0 0; 0 0 1 0.6546; 0 0 0 1];
T0e = FKinBody(M0e, Blist, thetaList);
Tbe = Tb0 * T0e;
Teb = inv(Tbe);
Jbase = Adjoint(Teb) * F6;
Jarm = JacobianBody(Blist, thetaList);
Je = [Jbase, Jarm];

psInv = pinv(Je, 1e-02);

Xerr_bracket = MatrixLog6(inv(X) * Xd);
Xerr = se3ToVec(Xerr_bracket);
Vd_bracket = (1/delta_t) * MatrixLog6(inv(Xd) * Xd_next);
Vd = se3ToVec(Vd_bracket);

V = (Adjoint(inv(X) * Xd) * Vd) + (Kp * Xerr) + (Ki*(Xerr + Xerr * delta_t));

u_theta_dot = psInv * V;
</code>```

end

.....

I tested my 3 functions in order to check for any possible errors; however function outputs were exactly like the expected ones on the project website. However, I wrote a final program to put together everything, but the results are nowhere close to the solution. I believe that I did something wrong in the program.

This was my approach:

1. Cube's final and initial configurations are given.

2. End effector initial reference configuration is given.

3. I created a reference trajectory with Trajectory Generator function

4. I chose a 12 vector robot configuration at random.

5. I calculated the 4x4 Tse_actual with that 12 vector

6. I choose the kth and k+1th steps as the Xd and Xd_next

7. I calculated the Feed-forward twist V and controls with Kp and Ki = 0 with FeedbackControl function

8. I calculated the new robot configuration with the current actual robot configuration(12 vector) and controls with NextState function.

9. Went to step 4

10. Loop as many times number of trajectory points.

And this is my program to find the robot configurations to accomplish the goal:

```<code>    clc
clear

Blist = [0 0 1 0 0.033 0; 0 -1 0 -0.5076 0 0; 0 -1 0 -0.3526 0 0; 0 -1 0 -0.2176 0 0; 0 0 1 0 0 0]';
Tb0 = [1 0 0 0.1662; 0 1 0 0; 0 0 1 0.0026; 0 0 0 1];
M0e = [1 0 0 0.033; 0 1 0 0; 0 0 1 0.6546; 0 0 0 1];

l = 0.47/2;
w = 0.30/2;
r = 0.0475;
F = (r/4) * [-1/(l + w), 1/(l + w), 1/(l + w), -1/(l + w); 1 1 1 1; -1 1 -1 1];

sizee = size(F);
m = sizee(2);
zeross = zeros(1, m);
F6 = [zeross; zeross; F; zeross];

Kp = 0.005;
Ki = 0.005 * 0.1;
delta_t = 0.1;

%%
Tse_initial = [0 0 1 0; 0 1 0 0; -1 0 0 0.25; 0 0 0 1];
Tsc_initial = [1 0 0 1; 0 1 0 0; 0 0 1 0.025; 0 0 0 1];
Tsc_final = [0 1 0 0; -1 0 0 -1; 0 0 1 0.025; 0 0 0 1];
Tce_standoff = [1 0 0 0; 0 1 0 0; 0 0 1 0.1; 0 0 0 1] * [cosd(135), 0, sind(135) 0; 0, 1, 0  0; -sind(135), 0, cosd(135), 0; 0 0 0 1];
Tce_grasp = [1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1] * [cosd(135), 0, sind(135) 0; 0, 1, 0  0; -sind(135), 0, cosd(135), 0; 0 0 0 1];

movement = TrajectoryGenerator(Tse_initial, Tsc_initial, Tsc_final, Tce_grasp, Tce_standoff, 1);

conf1(1, :) = [0.5, -0.9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0];
o = conf1(1);
x = conf1(2);
y = conf1(3);
z = 0.0963;
Tsb = [cos(o), -sin(o), 0 x; sin(o), cos(o), 0, y; 0, 0, 1, z; 0, 0, 0, 1];
Tb0 = [1, 0, 0, 0.1662; 0, 1, 0, 0; 0, 0, 1, 0.0026; 0, 0, 0, 1];
thetalist = conf1(4 : 8);
T0e = FKinBody(M0e, Blist, thetalist);
%%
X = Tsb * Tb0 * T0e;
%%
refconf1 = movement(1,:);
refconf2 = movement(1,:);

oconf1 = refconf1(1);
xconf1 = refconf1(2);
yconf1 = refconf1(3);

oconf2 = refconf2(1);
xconf2 = refconf2(2);
yconf2 = refconf2(3);

Tsb1 = [cos(oconf1), -sin(oconf1), 0 xconf1; sin(oconf1), cos(oconf1), 0, yconf1; 0, 0, 1, z; 0, 0, 0, 1];
thetalist1 = refconf1(4 : 8);
T0e1 = FKinBody(M0e, Blist, thetalist1);
Xd1 = Tsb1 * Tb0 * T0e1;

Tsb2 = [cos(oconf2), -sin(oconf2), 0 xconf2; sin(oconf2), cos(oconf2), 0, yconf2; 0, 0, 1, z; 0, 0, 0, 1];
thetalist2 = refconf2(4 : 8);
T0e2 = FKinBody(M0e, Blist, thetalist2);
Xd2 = Tsb2 * Tb0 * T0e2;
[Vd, V, Je, controls, Xerr] = FeedbackControl(X, Xd1, Xd2, Kp, Ki, delta_t, thetalist);

%%
for i = 2 : length(movement) -1
conf1(i, :) = NextState(conf1(i-1, :), controls', delta_t, 5000);
o = conf1(1);
x = conf1(2);
y = conf1(3);
z = 0.0963;
Tsb = [cos(o), -sin(o), 0 x; sin(o), cos(o), 0, y; 0, 0, 1, z; 0, 0, 0, 1];
Tb0 = [1, 0, 0, 0.1662; 0, 1, 0, 0; 0, 0, 1, 0.0026; 0, 0, 0, 1];
thetalist = conf1(4 : 8);
T0e = FKinBody(M0e, Blist, thetalist);
X = Tsb * Tb0 * T0e;
%
refconf1 = movement(i,:);
refconf2 = movement(i+1,:);

oconf1 = refconf1(1);
xconf1 = refconf1(2);
yconf1 = refconf1(3);

oconf2 = refconf2(1);
xconf2 = refconf2(2);
yconf2 = refconf2(3);

Tsb1 = [cos(oconf1), -sin(oconf1), 0 xconf1; sin(oconf1), cos(oconf1), 0, yconf1; 0, 0, 1, z; 0, 0, 0, 1];
thetalist1 = refconf1(4 : 8);
T0e1 = FKinBody(M0e, Blist, thetalist1);
Xd1 = Tsb1 * Tb0 * T0e1;

Tsb2 = [cos(oconf2), -sin(oconf2), 0 xconf2; sin(oconf2), cos(oconf2), 0, yconf2; 0, 0, 1, z; 0, 0, 0, 1];
thetalist2 = refconf2(4 : 8);
T0e2 = FKinBody(M0e, Blist, thetalist2);
Xd2 = Tsb2 * Tb0 * T0e2;
[Vd, V, Je, controls, Xerr] = FeedbackControl(X, Xd1, Xd2, Kp, Ki, delta_t, thetalist);
%
end
sizee = size(conf1);

for i = 1 : sizee(1)
    conf1(i, 13) = movement(i, 13);
end
</code>```

I know that this is a lot to look for; however I am trying for almost 2 days and I could never figure my mistake out. Can you help me?

答:
The best way to debug geometry related applications is by using plots.

First of all, plot the initial configuration, with robot TCP, cube initial position, cube final position. As these are all input values all should be ok.

After this, plot the generated trajectory. Is this what you are expecting it to be? does it link the start end end points exactly? You can use small arrows (quiver plot) to add orientations to each point to make sure that those are also as expected. Visually it is easy to judge if something goes wrong.

After this, you can better identify where the problem is, before trajectory generation, during or after. This is the goal of debugging (a kind of meta divide et impera) to slowly close in on the problematic part by dividing and subdividing the code and checking if it works ok.

For each timestep after trajectory generation you can plot the robot itself. You can check if it follows the trajectory or not, or where does it go off track.

Title: My Pick and Place Program Not Working


问:
What is the idea behind calling configuration space a metric space?
In my robotics textbook  1 , the configuration space has been defined configuration space the following way:

A configuration space is a metric space compromising of all given configuration of a system.

Then he goes one about how we can define a metric on a configuration space of a system and some other ideas on based on it... I can't understand this at all.

What exactly is the concept of configuration space and secondly what is the idea behind defining distances behind it?

After some quick googling, I understood that a metric space is just a set endowed with a notion of distance but I can't understand how it connects to robotics. What exactly is the idea behind what the author is trying to say here?

 1  Mechanics of Robot Manipualtion, Matthew T. Mason,page-11

答:
I did not read the book. But one of possible thinks that came to my mind is this:

There is big field in mathematica dedicated to preciselly define some terms (set, metric, metric space, ...) and prove a lot of thinks about them. Once you can prove, that something is "metric space" then you can use any result from this part about "metric space" and safely apply it to your problem, knowing, that it fits.

So many authors use this "reduction on known problem" to be able use all knowledge collected about the "known problem" and apply it on the problem they have (and maybe then add something new).

Say I want win a contest on running thru maze in less time (and I have 2 tries).

Then I would just prove, that the maze can be described as graph where crossing are nodes and ways between them are weighted edges, where the weight is travel time.

Then I can first run just "map" all the maze and in second run use https://en.wikipedia.org/wiki/Shortest_path_problem known graph theory to find "shortest path", in my case fastest (as I maesure the time, not distance).

And then I would folow the fastest way and the rest just dependes on how good motor and navigatition I have, knowing I cannot get better result any other way (I can get the same result at the best).

So I think, that the author of the book took the same approach - he "proved", that configurations can be described as some already known mathematic constructs and then he use proven knowledges about those mathematical constructs to apply them on configurations and get desired results "fast, easy and proven way".