问:
Number of inverse kinematics closed form solutions for ABB IRB 140
I've read that 6 dof manipulators give eight closed form solutions, according to a paper. What is the number of iK solutions for the ABB IRB 140? (last three axes intersect but there is a offset between joint 1-2. Does it affect the number of solutions?)
Thanks

https://search-ext.abb.com/library/Download.aspx?DocumentID=3HAC041346-001&LanguageCode=en&DocumentPartId=&Action=Launch&fbclid=IwAR2atlGRywE3gEGu7Or-LGl5_xKxuD6r_MAAIS5EQxEb7MJwwd2hJRmoi7Q

答:
Actually, in certain configurations you can have an infinite number of IK solutions.  For example if joints 4, 5, and 6 are straight and inline as in your image.  You can rotate joint 4 by $X$ and 6 by $-X$ to get the same end-effector pose.

This is a great video on arm singularities which also shows this phenomenon nicely:
https://www.youtube.com/watch?v=zlGCurgsqg8

You don't typically have these joints inline like this, but in general you can have 2 solutions at the wrist.  In one solution, joints 4, 5, and 6 will have values: $X$, $Y$, and $Z$.  And in the other solution, they will have values: $180-X$, $-Y$, and $180-Z$.  (I think, please verify).  I am not sure if there is a standard term for this but I have always called it a "wrist-flip".

You can have another flip with joints 2, 3, and 4 ("elbow-flip").  And another with joints 1, 2 and 3 ("shoulder-flip").  These flips can be combined, creating 2*2*2 = 8 possible permutations of IK solutions.

The presence of an offset joint will not affect the number of IK solutions.  You will still be able to do a shoulder-flip.  Although I think this means the joint angles won't simply be 180 degrees off from each other.  And maybe other joints will need to compensate too.

Note that you will not always get the same number of solutions.  Joint limits,  self collisions, and construction details can reduce the number of valid solutions.  Here is where the offset between joints 1 and 2 comes into play.  For example, near the limits of the workspace on one side, you won't be able to get that close if joint 1 is rotated 180 degrees.  Eliminating the possibility of a shoulder-flip.


问:
Rotation Matrices Using DH Parameters
I need to find rotation matrices between the frames of references of the vertices of the cube, and the DH parameters. For the rotation between the first to second frame of reference, I got alpha as pi/2 and theta as -pi/2, which makes the rotation matrix as: [0 0 -1; -1 0 0; 0 1 0]. This doesnt match any of the given options.

答:
I'm not sure where you went wrong, but that's because you haven't explained how you arrived at your DH values or how you converted from DH to rotation matrix.

This question doesn't really need all that, though, because the axes are swapped but otherwise remain colinear. If you were trying to get to/from O3 it'd be a bit harder.

The matrix, any matrix but especially here for rotations, just explains how you recombine elements of an existing vector to get a new vector. In your case, you have:

$$
\left[\begin{matrix}
x_1 \\
y_1 \\
z_1
\end{matrix}\right] =
\left[\mbox{something}\right]
\left[\begin{matrix}
x_2 \\
y_2 \\
z_2
\end{matrix}\right]
$$

And you want the [something] that converts from O2 to O1. Just look at the axes!

In O2, the x-axis points the same way as the z-axis does in O1. None of the other axes point in that direction, so you want all of the $x_2$ and none of the $y_2$ or $z_2$ entries. That is:

$$
\left[\begin{matrix}
x_1 \\
y_1 \\
z_1
\end{matrix}\right] =
\left[\begin{matrix}
&\mbox{something} \\
&\mbox{something} \\
1 & 0 & 0
\end{matrix}\right]
\left[\begin{matrix}
x_2 \\
y_2 \\
z_2
\end{matrix}\right]
$$

If you look at just the multiplication on that bottom row, you get:

$$
z_1 = 1*x_2 + 0*y_2 + 0*z_2 \\
$$

or, as stated above, $z_1 = x_2$. You can see that from the diagram: $z_1$ and $x_2$ point in the same direction.

By the same way, you can see that $z_2$ points in the same direction as $x_1$. Since it's for $x_1$, you want it to be the first row, and again you want all of $z_2$ but none of $y_2$ or $x_2$, so you get:

$$
\left[\begin{matrix}
x_1 \\
y_1 \\
z_1
\end{matrix}\right] =
\left[\begin{matrix}
0 & 0 & 1 \\
&\mbox{something} \\
1 & 0 & 0
\end{matrix}\right]
\left[\begin{matrix}
x_2 \\
y_2 \\
z_2
\end{matrix}\right]
$$

And you can again verify this by multiplying the top row out to get:

$$
x_1 = 0*x_2 + 0*y_2 + 1*z_2 \\
$$

Or $x_1 = z_2$. Finally, the y-axes just point in the opposite directions, so you want to invert all of the y-values, again taking none of the x- or z-axis values:

$$
\left[\begin{matrix}
x_1 \\
y_1 \\
z_1
\end{matrix}\right] =
\left[\begin{matrix}
0 & 0 & 1 \\
0 & -1 & 0 \\
1 & 0 & 0
\end{matrix}\right]
\left[\begin{matrix}
x_2 \\
y_2 \\
z_2
\end{matrix}\right]
$$

So that's the easy way to solve the problem. If you want help figuring out where you went wrong with the DH route (which is still totally valid but just more work) then please post more information including how you arrived at the DH parameters and how you converted them to a rotation matrix.

ContentLicense: CC BY-SA 4.0

Title: Rotation Matrices Using DH Parameters


问:
Robot moving sideways in RVIZ ROS

Hi,

I have a wheeled-legged robot as you can see in the picture. When I simulate it in ROS-Gazebo it works fine, however at the same time in Rviz it moves sideways instead of forward or backwards. I have no idea why it is behaving like this. Would greatly appreciate if someone can give me some clue here.

答: I have found the problem after some long search on the internet. The problem was that in Rviz, X-axis is defined as front of the robot and Y-axis is defined as the sideways. However, in my URDF I defined the robot in a way where Y-axis was its front and X-axis was its sideways. After fixing this, my robot moves normally in Rviz and Gazebo both. </p>

Title: Robot moving sideways in RVIZ ROS


问:
Not able to communicate with Roomba with serial port
I have an IRobot Roomba model number 565 and I am trying to control it with a Wemos D1 mini with Roomba's OI.

I connected the TX and RX pins from my FTDI adapter to the Roombas RX and TX and I can see information (battery firmware and etc) pop up in Termite (a serial terminal) but if I send the commands 128,131,135 in sequence The Roomba doesn't go to clean mode.

I saw somewhere that sending commands to the Roomba with the serial terminal won't work because it will send 128 as three bytes instead of one. So I used a sketch from someone that publishes information about the Roomba into an MQTT server. So I uploaded it to my Wemos D1 mini and connected the RX and TX accordingly, but it sends "no data" and when I do a power reset it gives me a two-tone beep.

I am really confused about what does that beep mean and why isn't it working correctly. I did try two different sketches but neither of them worked.

How can I command my Roomba to go into clean mode using my Wemos D1 mini?

EDIT: sorry for forgetting to add the links of the sketches I tried. The MQTT one is from a Youtuber called The Hook Up here is his GitHub The second one I saw while searching on why it wasn't working, so here is that one. Also, I was trying to upload this one but I was getting a compiling error so I didn't test it.
Also, I didn't find anything about the error beep the Roomba is giving me. The closest thing I get while searching for it is the list of error codes and beeps but it doesn't say anything for only 2 beeps and no voice message. I also totally forgot to mention that when I connect my Roomba with a serial adapter, open Termite(a serial terminal) and then press the clean button I get the following response

this tells me that there is a high chance that there is OI support in my Roomba.

I also tried using Python as Austin suggested but still, no luck
here is my code if you can find some sort of mistake I did

```<code>import serial
ser = serial.Serial("COM7", 115200) # open serial port
print(ser.name) # check which port was really used
ser.write(bytearray([128, 131, 135]))
print ("commands sent")
</code>```

答:
Your code looks good, the device is probably sending you back bytes so you need to read them on the port. Adding to what you have, it would look something like this:

```<code>import serial
import time

ser = serial.Serial("COM7", 115200) # open serial port
print(ser.name) # check which port was really used
ser.write(bytearray([128, 131, 135]))
print ("commands sent")

start_time = time.time()
while time.time()-start_time < 10:
    if ser.inWaiting() > 0:
        incomming_byte = ser.read()
        print(incomming_byte),      #Prints bytes interpreted as ascii/char values
        #print(ord(incomming_byte)) #Prints bytes interpreted as decimal values

ser.close()
</code>```

This will loop for 10 seconds checking to see if any bytes are in the serial buffer, and if there is, then it will read the bytes. The comma after the print statement should print all the bytes on the same line as opposed to on multiple new lines. The second print statement is commented out so it wont run, but I put it there in case you wanted to get decimal values back.

Body:
Your code looks good, the device is probably sending you back bytes so you need to read them on the port. Adding to what you have, it would look something like this:

import serial
import time

ser = serial.Serial("COM7", 115200) # open serial port
print(ser.name) # check which port was really used
ser.write(bytearray([128, 131, 135]))
print ("commands sent")

start_time = time.time()
while time.time()-start_time < 10:
    if ser.inWaiting() > 0:
        incomming_byte = ser.read()
        print(incomming_byte),      #Prints bytes interpreted as ascii/char values
        #print(ord(incomming_byte)) #Prints bytes interpreted as decimal values

ser.close()

This will loop for 10 seconds checking to see if any bytes are in the serial buffer, and if there is, then it will read the bytes. The comma after the print statement should print all the bytes on the same line as opposed to on multiple new lines. The second print statement is commented out so it wont run, but I put it there in case you wanted to get decimal values back.


问:
How exactly does sensor fusion work in Kalman filters?
I've been looking into implementations of Extended Kalman filters over the past few days and I'm struggling with the concept of "sensor fusion".

Take the fusion of a GPS/IMU combination for example, If I applied a kalman filter to both sensors, Which of these will I be doing?

Convert both sensors to give similar measurements (eg. x, y, z), apply a kalman filter to both sensors and return an average of the estimates

Convert both sensors to give similar measurements (eg. x, y, z), apply a kalman filter, and return an estimate of the sensor I trust more based on a certain parameter (measurement noise covariance)?

答:
How to, article!

Sensor Fusion, Published on June 5, 2019 by Jose R.F Junior (Machine Learning Engineer)


问:
Where I can learn algorithms or and find examples of code for controlling a rover?
I am a programmer by profession and new to Robotics. I have studied ECE, so know electronics, but not very familiar with mechanical aspects of robotics. I am working on a learning project with Dagu Rover 5 platform.

I am trying to control the 4 DC motors with PWM and want to use the optical encoders for feedback. I am looking for some algorithms, example code in C to effectively control the rover. I know how to control the GPIO, PWM and interrupts from the processor. I am more interested in learning the algorithm that controls the motors based on this. For now, i am working on a manual robot, controlled with up/down/left/right keys. In future, I would like to add sensors, camera etc and work on autonomous aspects. Any pointers would be helpful.

For reference, I am working on the Raspberry Pi platform to control the rover.

答:
Specifically about control of motors by PWM, you should learn about the PID controller.

The sky's the limit for what you want to learn about.  Find a robotics simulator to start playing around with algorithms -- otherwise you will find that you will spend all year building the robot instead.

For a mobile robot, motion planning or mapping algorithms would be a good start.

Real sensors are noisy and ambiguous, so machine learning (i.e. statistical, fuzzy learning) is a whole field you can look at if you're interested, with applications in control, motion planning, and vision processing.  How much time do you have?

ContentLicense: CC BY-SA 3.0


问:
Quadcopter motors vibration problem
I built an Arduino Due based quadcopter, with MPU-6050 for auto levelling and BMP-085 for Altitude.

I am using Kalman filter code from here for the MPU-6050 to take the Roll and Pitch angels from the sensor.

The Roll and Pitch angels are correctly read while the quadcopter motors are off, but when the motor speed increased the angels start to be more inaccurate.

As shown in the following photos (where I took different speed motors to show the effects of the motor vibration on the MPU readings). These tests were taken while the quadcopter on the ground.

Motor speeds vary from 0 to 180 so I choose (50,100, and 150).

My sensors are fixed on a breadboard over a plastic surface with anti-vibrator rubber balls as shown in my quadcopter pictures.

My problem is that the controller I designed is very sensitive to these vibrations, is there any way to reduce these vibrations as minimum as possible.

Thank you

答: In addition to previous answers which I agree with, try a FOC ESC, expensive yes, but less noise and vibration.</p>

Title: Quadcopter motors vibration problem


问:
When are non-diagonal matrices advantageous for linear quadratic regulators?
When setting up an optimal control problem using a linear quadratic regulator, one often chooses semi-definite matrices Q and R such that the objective function J has the form

$$J=\int_{t_0}^{t_f}(x^TQx+u^TRu)dt$$

I can see how choosing Q and R as diagonal matrices is advantageous because they make the drive the state x and control values to be as small as possible (close to zero).  I also understand that a positive definite or semi definite matrix can also (at least in principle) be used because it would make the objective function essentially still a convex parabola whose minimum is still a single point, which also helps to drive the state and controls to be as small as possible.

In practice, I can’t think of a situation where a non-diagonal semi definite matrix would be better than a diagonal one for an LQR control problem.  What kinds of problems/ situations make it more advantageous to choose off-diagonal entries of Q and R to be non-zero? I want to be able to identify situations that may come up in my work where this might be necessary, so I’m more interested in guiding principles rather than individual situations or exceptions.

答:
In general it not meaning full to say that a diagonal $Q$ is advantageous, since one can always perform a similarity transformation to the state space model ($x_\text{new}=T\,x$) which would make the effective $Q$ ($x^\top Q\,x = x_\text{new}^\top T^{-\top} Q\,T^{-1} x_\text{new}$, so the effective $Q$ would be $T^{-\top} Q\,T^{-1}$) for the new state coordinates non-diagonal. In other words given any non-diagonal semi-positive definite $Q$ there is always a similarity transformation which would make the effective $Q$ for the new state coordinates diagonal.

As for $R$ it would depend on what you would like to minimize. For example if $u$ represents two parallel actuators and you want to minimize their combined force. This can be written as

$$
(w_1\,u_1 + w_2\,u_2)^2 = u^\top
\begin{bmatrix}
w_1^2 & w_1\,w_2 \\ w_1\,w_2 & w_2^2
\end{bmatrix} u,
$$

with $w_1,w_2$ some weights. Though normally $R$ should be positive definite (not just semi-positive definite), so one would have to add some additional term to ensure this, but this should illustrate how one could get to an non-diagonal $R$.


问:
Matlab Code for Lynx Robot
In this section, you will need to write a function lynx_fk which returns the positions of 10 points along the robot arm as defined later, given the joint angles of the robot.

For this question, please solve the forward kinematics of the robot using the frames as given below. The figure below shows the robot in its 'zero pose', i.e., making all joint angles 0 places the robot in this pose. Consider frame 0 to be the world frame. Also,  are facing into the plane and away from you.

Input Format are the joint angles in radian, as shown in the figure

Output Format:

pos is a 10x3 matrix where each row contains the x,y,z coordinates represented as [x y z] in matrix form.

Each row is the x,y,z coordinates of a point in world frame (frame 0)

The first 5 rows must contain:

Position of frame 0 in world frame

Position of frame 1 in world frame

Position of frame 2 in world frame

Position of frame 3 in world frame

Position of frame 4 in world frame

The next 5 rows are used to describe the gripper (green dots shown in the figure above)

[0 0 -e] of frame 5 in world frame

[g/2 0 -e] of frame 5 in world frame

[-g/2 0 -e] of frame 5 in world frame

[g/2 0 0] of frame 5 in world frame

[-g/2 0 0] of frame 5 in world frame

My Code:

```<code>function [pos] = lynx_fk(theta1, theta2, theta3, theta4, theta5, g)

    pos = zeros(10, 3);

    A01 = compute_dh_matrix(0,     -pi/2, 3,     theta1);
    A12 = compute_dh_matrix(5.75,   0,    0,     theta2);
    A23 = compute_dh_matrix(7.375,  0,    0,     theta3);
    A34 = compute_dh_matrix(0,     -pi/2, 3,     theta4);
    A45 = compute_dh_matrix(0,      0,    4.125, theta5);

    L0 = [0;     0; 0;    1];
    L1 = [0;     0; 3;    1];
    L2 = [0;     0; 8.75; 1];
    L3 = [7.375; 0; 8.75; 1];
    L4 = [7.375; 0; 8.75; 1];
    L5 = [11.5;  0; 8.75; 1];

    pos(1,:) = [0,0,0];
    posi2 = (A01 * L1).';
    posi3 = ((A01*A12)* L2).';
    posi4 = ((A01*A12*A23) * L3).';
    posi5 = ((A01*A12*A23*A34) * L4).';

    pos(2,:) = posi2(:,1:3);
    pos(3,:) = posi3(:,1:3);
    pos(4,:) = posi4(:,1:3);
    pos(5,:) = posi5(:,1:3);

    posi5 = ((A01*A12*A23*A34*A45) * L5).';
    pos5 = posi5(:,1:3);

    pos(6,:)  = pos5 - [ 1.125, 0, 0];
    pos(7,:)  = pos5 + [-1.125, 0, g/2];
    pos(8,:)  = pos5 - [ 1.125, 0, g/2];
    pos(9,:)  = pos5 + [ 0,     0, g/2];
    pos(10,:) = pos5 - [ 0,     0, g/2];

    pos

end
</code>```

The DH matrix is returned with the following function:

```<code>function A = compute_dh_matrix(r, alpha, d, theta)

    A = eye(4);

    dh_ct = cos(theta);
    dh_st = sin(theta);
    dh_ca = cos(alpha);
    dh_sa = sin(alpha);
    dh_r = r;
    dh_d = d;

    A(1) =  dh_ct;
    A(5) = -dh_st*dh_ca;
    A(9) =  dh_st*dh_sa;
    A(13) = dh_r*dh_ct;

    A(2) =   dh_st;
    A(6) =   dh_ct*dh_ca;
    A(10) = -dh_ct*dh_sa;
    A(14) =  dh_r*dh_st;

    A(3) =  0;
    A(7) =  dh_sa;
    A(11) = dh_ca;
    A(15) = dh_d;

    A(4) =  0;
    A(8) =  0;
    A(12) = 0;
    A(16) = 1;

end
</code>```

The code for computation of DH matrix is correct, as it was checked previously. I suspect there might be a mistake in my DH values I found (which I found using right hand rule). Or there might be a problem in the rest of the code. Please help me debug or find my error.

答:
I edited your post to clean up some of the formatting. I'll go over some glaring issues I see in the post, but if you're looking for really precise troubleshooting then you need to edit your question to post why you think the code you have isn't working correctly - i.e., you have an input, your code gives you an output, and you were expecting some other answer. Seeing what portions are correct and what aren't, etc. can help to narrow down the suspect code.

That said, I think a lot of bugs are now apparent after having cleaned up the code. Neatness and readability counts for a lot! I it's hard to look at your code, it's hard to find the bug.

For instance:

Problem 1.

```<code>A01 = compute_dh_matrix(0,     -pi/2, 3,     theta1);
A12 = compute_dh_matrix(5.75,   0,    0,     theta2);
A23 = compute_dh_matrix(7.375,  0,    0,     theta3);
A34 = compute_dh_matrix(0,     -pi/2, 3,     theta4);
A45 = compute_dh_matrix(0,      0,    4.125, theta5);
</code>```

If you read down the columns now, you can quickly compare parameters with your drawing. Notice the A34 line?

```<code>A34 = compute_dh_matrix(0,     -pi/2, 3,     theta4);
</code>```

But it looks like frames 3 and 4 are coincident! Why is there a distance of 3 between them?

Problem 2.

You seem to be misunderstanding how to use the frame transforms. You have the starting positions hard-coded in your lynx_fk function. This shouldn't be necessary. If you want to know the position of a point in Frame 1 with respect to the world (Frame 0), then you transform that point to world coordinates with the A01 transform:

```<code>pt_world = A01 * pt_frame1;
</code>```

This chains - if you want to know how a point in Frame 2 is described in Frame 1, then you do the following:

```<code>pt_frame1 = A12 * pt_frame2;
</code>```

And you can see then that, to express that point in world coordinates, you go:

```<code>pt_world = A01 * pt_frame1;
pt_world = A01 * (A12 * pt_frame2);
</code>```

You can keep going on:

```<code>pt_world = A01 * A12 * ... * A_N_Nplus1 * pt_frameNplus1;
</code>```

If you want to know where the frame origins are located in world coordinates, then you would do something like:

```<code>frame1_worldCoordinates = A01 * [0; 0; 0; 1];
frame2_worldCoordinates = A01 * A12 * [0; 0; 0; 1];
</code>```

and so on. You can take those outputs and compare against your drawing and see where you're going wrong.

Problem 3.

You're overwriting pos5. I'm not going to say which way is correct, but your code has:

```<code>posi5 = ((A01*A12*A23*A34) * L4).';
<...>
pos(5,:) = posi5(:,1:3);

posi5 = ((A01*A12*A23*A34*A45) * L5).';
pos5 = posi5(:,1:3);
</code>```

As stated above, neither will give you the location of the Frame 5 origin in world coordinates; that would need (list of transforms) * [0; 0; 0; 1].

Problem 4.

When you try to find the end effector locations, you don't have the positions in the correct columns. You have:

```<code>pos(6,:)  = pos5 - [ 1.125, 0, 0];
pos(7,:)  = pos5 + [-1.125, 0, g/2];
pos(8,:)  = pos5 - [ 1.125, 0, g/2];
pos(9,:)  = pos5 + [ 0,     0, g/2];
pos(10,:) = pos5 - [ 0,     0, g/2];
</code>```

Which, when you distribute the negatives, is equivalent to:

```<code>pos(6,:)  = pos5 + [-1.125, 0,  0];
pos(7,:)  = pos5 + [-1.125, 0,  g/2];
pos(8,:)  = pos5 + [-1.125, 0, -g/2];
pos(9,:)  = pos5 + [ 0,     0,  g/2];
pos(10,:) = pos5 + [ 0,     0, -g/2];
</code>```

But you're supposed to have:

```<code>[ 0   0 -e] of frame 5 in world frame
[ g/2 0 -e] of frame 5 in world frame
[-g/2 0 -e] of frame 5 in world frame
[ g/2 0  0] of frame 5 in world frame
[-g/2 0  0] of frame 5 in world frame
</code>```

This is probably because you're compensating for:

Problem 5.

You're not converting the end effector locations to world coordinates. Just like Problem 2 says, if you want to convert a point from a particular frame to world coordinates, step through the transforms from the local expression of that point to each subsequent parent frame until you've arrived at the world coordinate frame.

So, for example, [0 0 -e] of frame 5 in world frame. To find the origin of Frame 5, you take a Frame 5 point that is located at the origin ([0; 0; 0]) and pass it through the sequence of transforms to get back to world coordinates. You do the same thing here, but with [0; 0; -e], etc., to get the expression for those points in world coordinates.

If you can fix all of these problems and you're still having issues, please edit your question to provide your updated code, a sample input, the output of your function for the sample input, and the output you expected to get for your sample input.

The best hint I can give you is that, if you have your DH values correct and everything is working correctly, then passing all zeros for the thetaN values should give you the general arrangement drawing you've provided in your original post. You shouldn't have to hard-code anything except the DH parameters in the lynx_fk code.

Title: Matlab Code for Lynx Robot

Body:
I edited your post to clean up some of the formatting. I'll go over some glaring issues I see in the post, but if you're looking for really precise troubleshooting then you need to edit your question to post why you think the code you have isn't working correctly - i.e., you have an input, your code gives you an output, and you were expecting some other answer. Seeing what portions are correct and what aren't, etc. can help to narrow down the suspect code.

That said, I think a lot of bugs are now apparent after having cleaned up the code. Neatness and readability counts for a lot! I it's hard to look at your code, it's hard to find the bug.

For instance:

Problem 1.

A01 = compute_dh_matrix(0,     -pi/2, 3,     theta1);
A12 = compute_dh_matrix(5.75,   0,    0,     theta2);
A23 = compute_dh_matrix(7.375,  0,    0,     theta3);
A34 = compute_dh_matrix(0,     -pi/2, 3,     theta4);
A45 = compute_dh_matrix(0,      0,    4.125, theta5);

If you read down the columns now, you can quickly compare parameters with your drawing. Notice the A34 line?

A34 = compute_dh_matrix(0,     -pi/2, 3,     theta4);

But it looks like frames 3 and 4 are coincident! Why is there a distance of 3 between them?

Problem 2.

You seem to be misunderstanding how to use the frame transforms. You have the starting positions hard-coded in your lynx_fk function. This shouldn't be necessary. If you want to know the position of a point in Frame 1 with respect to the world (Frame 0), then you transform that point to world coordinates with the A01 transform:

pt_world = A01 * pt_frame1;

This chains - if you want to know how a point in Frame 2 is described in Frame 1, then you do the following:

pt_frame1 = A12 * pt_frame2;

And you can see then that, to express that point in world coordinates, you go:

pt_world = A01 * pt_frame1;
pt_world = A01 * (A12 * pt_frame2);

You can keep going on:

pt_world = A01 * A12 * ... * A_N_Nplus1 * pt_frameNplus1;

If you want to know where the frame origins are located in world coordinates, then you would do something like:

frame1_worldCoordinates = A01 * [0; 0; 0; 1];
frame2_worldCoordinates = A01 * A12 * [0; 0; 0; 1];

and so on. You can take those outputs and compare against your drawing and see where you're going wrong.

Problem 3.

You're overwriting pos5. I'm not going to say which way is correct, but your code has:

posi5 = ((A01*A12*A23*A34) * L4).';
<...>
pos(5,:) = posi5(:,1:3);

posi5 = ((A01*A12*A23*A34*A45) * L5).';
pos5 = posi5(:,1:3);

As stated above, neither will give you the location of the Frame 5 origin in world coordinates; that would need (list of transforms) * [0; 0; 0; 1].

Problem 4.

When you try to find the end effector locations, you don't have the positions in the correct columns. You have:

pos(6,:)  = pos5 - [ 1.125, 0, 0];
pos(7,:)  = pos5 + [-1.125, 0, g/2];
pos(8,:)  = pos5 - [ 1.125, 0, g/2];
pos(9,:)  = pos5 + [ 0,     0, g/2];
pos(10,:) = pos5 - [ 0,     0, g/2];

Which, when you distribute the negatives, is equivalent to:

pos(6,:)  = pos5 + [-1.125, 0,  0];
pos(7,:)  = pos5 + [-1.125, 0,  g/2];
pos(8,:)  = pos5 + [-1.125, 0, -g/2];
pos(9,:)  = pos5 + [ 0,     0,  g/2];
pos(10,:) = pos5 + [ 0,     0, -g/2];

But you're supposed to have:

[ 0   0 -e] of frame 5 in world frame
[ g/2 0 -e] of frame 5 in world frame
[-g/2 0 -e] of frame 5 in world frame
[ g/2 0  0] of frame 5 in world frame
[-g/2 0  0] of frame 5 in world frame

This is probably because you're compensating for:

Problem 5.

You're not converting the end effector locations to world coordinates. Just like Problem 2 says, if you want to convert a point from a particular frame to world coordinates, step through the transforms from the local expression of that point to each subsequent parent frame until you've arrived at the world coordinate frame.

So, for example, [0 0 -e] of frame 5 in world frame. To find the origin of Frame 5, you take a Frame 5 point that is located at the origin ([0; 0; 0]) and pass it through the sequence of transforms to get back to world coordinates. You do the same thing here, but with [0; 0; -e], etc., to get the expression for those points in world coordinates.

If you can fix all of these problems and you're still having issues, please edit your question to provide your updated code, a sample input, the output of your function for the sample input, and the output you expected to get for your sample input.

The best hint I can give you is that, if you have your DH values correct and everything is working correctly, then passing all zeros for the thetaN values should give you the general arrangement drawing you've provided in your original post. You shouldn't have to hard-code anything except the DH parameters in the lynx_fk code.


问:
PID control loop and feedback acquisition synchronization
In many real-time PID control loop application I have seen that the control loop is triggered exactly as the ADC interrupts arrives in order to achieve maximum synchronization between feedback conversion and control loop execution.

I am in a situation where it would be better to have the control loop triggered by a timer interrupt so that it can be indipendent from how the feedback is acquired, for example:

Capacitive encoder read through ADC -> interrupts;

Hall absolute encoder read through I2C -> no interrupts;

In the first case I have a sample available each 4ms. How fast the control loop should be triggered? What are the drawbacks of this implementation? Is it possible to take this issue into account when modeling the digital control system?

Thank you for your time.

Edit 1:

Yes I am controlling a motor that can use different kind of encoders (one at time). Each encoder implements the same software interface so that I can keep motor logic indipendent from encoder internals.

Edit 2:

The rising edges in the image are the instants the feedback is sampled and thus made available by ADC interrupts. The sampling period is 4ms.
Assuming the control loop executes its calculation in the same time as the sampling period, what happens if the control loop interrupt is triggered late in respect to the ADC interrupt?

答: This kind of control situation in general Nyquist method is used, In your first if your sampling period is 4 ms, your sampling frequency is 250 Hz, so you can control event maximum 125 Hz or another saying 8 ms of sampling period. But it is only for starting, if your sensor values very volatile you would make your period much longer. But I can say 8 ms is the minimum.</p>


问:
Why are brushless servos more common than steppers in Cobots?
Essentially every cobot I've worked with uses BLDCs instead of stepper motors in the joints.  When searching for answers as to why I see a lot of posts that suggest one or the other, but nothing explains the consistent trend in the industry.  The closest I've come to an answer is that stepper motors are worse with dynamic loads, but I don't understand why.  Steppers have much higher precision due to their detant torque, so it seems like they should win out.

If anyone could help point me in the right direction I would greatly appreciate it.

答:
Size for size, steppers are wimpy compared to brushless (feel free to look at some catalogs and check; be sure to compare both torque and maximum RPM, because a motor can always be geared).

Without fancy electronics, steppers are run open-loop.  When they get lost, their average torque goes to zero.  I would not want that close to my face if it happened.

With fancy electronics, a stepper may as well be a brushless.  A wimpy brushless.

As long as position feedback is used and it's not limited by sticktion, a brushless can attain precision proportional to the accuracy of the feedback.


问:
Should you learn assembly language for robotics?
I ask this since assembly language is really close to the micro-controller hardware, and what micro-controller would you reccomend.

The bot has to search for object that I show it before and then I 'lose' the object. Note, I do not know anything about micro-controllers.

答:
What kind of problems are you interested in?  Mechanical design?  Electronics?  Low-level control and algorithms?  High-level planning and state estimation?

Pick your battles.  Assembly is good to know for some projects, but use bigger/easier building blocks depending on what your interest is.


问:
Action client waiting forever for server to start
I'm in the process of writing an action cpp file and the client side seems to be stuck waiting for the server to start and I'm not quite sure why.

Here's the code:

```<code>#include <stdio.h>
#include <std_msgs/String.h>

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <actionlib/client/simple_action_client.h>

#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/CommandBool.h>

#include <rovexp/ModeAction.h>

typedef actionlib::SimpleActionServer<rovexp::ModeAction> Server;

class ModeActionServer {

protected:
ros::NodeHandle nh_;

ros::ServiceClient arming_client_;
ros::ServiceClient set_mode_client_;

Server mode_server_;

rovexp::ModeFeedback feedback_;
rovexp::ModeResult result_;
std::string action_name_;
public:
ModeActionServer(std::string name) :
    mode_server_(nh_, name, boost::bind(&ModeActionServer::executeCB, this, _1), false),
    action_name_(name)
{
    mode_server_.start();
    ROS_INFO("Mode action server started...");
}
~ModeActionServer(void) { }

void executeCB(const rovexp::ModeGoalConstPtr &goal)
{
    ROS_INFO_STREAM("mode experiment started...");

    /*
    set_mode_client_ = nh_.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = goal->mode_num;

    set_mode_client_.call(offb_set_mode);
    */

    arming_client_ = nh_.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
    ros::Rate rate(20.0);
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;
    arming_client_.call(arm_cmd);
}
};

typedef actionlib::SimpleActionClient<rovexp::ModeAction> Client;

void modeActionClient(uint8_t mode_num)

{
Client client("mode_client", true);

ROS_INFO("Waiting for action server to start");
client.waitForServer();

rovexp::ModeGoal goal;
goal.mode_num = mode_num;
printf("%d\t4\n",goal.mode_num);
client.sendGoal(goal);

client.waitForResult();
}

int main(int argc, char**argv)
{
if(std::string(argv[1])=="-server")
{
  ros::init(argc, argv, "mode_server");
  ModeActionServer server("mode_server");
  //server.start();
  ros::spin();

}
else if(std::string(argv[1])=="-client")
{
    if ( std::string(argv[2])=="-h" ) {
        printf("Use: rosrun rovexp mode -client @mode\n");
        exit(-1);
    }
    ros::init(argc, argv, "mode_action_client");
    uint8_t mode_num = atoi(argv[2]);
    modeActionClient(mode_num);
}
printf("done\n");
return 0;
}
</code>```

The terminal will output "Mode action server started..." and "Waiting for action server to start" but never gets to "mode experiment started...".

So it would seem that the client gets stuck on

```<code>client.waitForServer();
</code>```

A lot of what I've done was inspired by a ROS tutorial (link) and there's works fine on my pc. Having a hard time seeing the difference.

I'm running ubuntu 16.04LTS and ROS kinetic.

答:
You need to use the same topics for the master and the client.

```<code>Client client("mode_client", true);
</code>```

starts a client that looks for a server that listens on the ~mode_client/goal topic, but you started the server on ~mode_server/goal.
Could you run both nodes and add the output of

```<code>rostopic list
</code>```

to your question?

Body:
You need to use the same topics for the master and the client.

Client client("mode_client", true);

starts a client that looks for a server that listens on the ~mode_client/goal topic, but you started the server on ~mode_server/goal.
Could you run both nodes and add the output of

rostopic list

to your question?


问:
Yaw drift when implementing AHRS filter fusion
I am using the Matlab AHRS filter fusion algorithm with an InvenSense ICM-20948 to determine object orientation. I seem to be obtaining reasonable results however I am getting what appears to be substantial yaw/heading drift (please see attached plot).

The Matlab AHRS filter fusion algorithm requires the following hardware/scenario specific parameters to be set (which I think is where my problem is stemming from):

Accelerometer noise - variance of accelerometer signal noise
$(\frac{m}{s^2})^2$

Magnetometer noise - variance of magnetometer signal noise $T^2$

Gyroscope noise - variance of gyroscope signal noise $(\frac{rad}{s})^2$

Gyroscope drift noise - variance of gyroscope offset drift $(\frac{rad}{s})^2$

Linear acceleration noise - variance of linear acceleration noise

$(\frac{m}{s^2})^2$

Linear acceleration decay factor (this appears to be dependent on
the application/movement velocity)

Magnetic disturbance noise - variance of magnetic disturbance noise
$T^2$

Magnetic disturbance decay factor - decay factor for magnetic
disturbance

Expected magnetic field strength (I understand this as being
location dependant)

Initial process noise - covariance matrix for process noise

(https://au.mathworks.com/help/fusion/ref/ahrsfilter-system-object.html)

I have calibrated my magnetometer to account for hard/soft iron effects. Additionally, I have found and am now using what I believe to be appropriate values for the Accelerometer noise and Gyroscope noise from the ICM-20948 data sheet.

Does anyone know where I may be going wrong or how I may obtain/set/measure any more of the above parameters to perhaps reduce this drift?

答:
You haven't provided any sample data or your implementation, etc., so it's hard to say for sure, but I would imagine that the signal coming in on your magnetometer is small enough that it's insufficient to set the angle. If your magnetometer readings went to zero, you would effectively have no magnetometer and you would get the same drift you're seeing.

How are you scaling your readings?

You said the magnetometer noise should be in units of $T$, but the Matlab documentation says otherwise:

What units are you passing to the filter? $T$ or $\mu T$? Per the documentation, it should be $\mu T$. The expected local strength should also be $\mu T$.


问:
Calculate Roll Pitch Yaw from Modified Denavit Hartenberg
I'm using modified denavit hartenberg to represent my robot end-effector. But i couldnt find equation to calculate rpy if im using mdh. Below my MDH.

I try use rpy equation for dh paramater(from Getting pitch, yaw and roll from Rotation Matrix in DH Parameter), but when i apply it doesnt seem correct. Does anyone know the correct equations? I'd be very thankful for any help.

答:
The orientation of the end-effector is obtained by multiplying together the MDH homogeneous transformation matrices for each of the joints.  There are well known algorithms and code implementations to extract roll, pitch, yaw angles from such a homogeneous transformation matrix.  You need to be careful about how you define roll, pitch and yaw angles, there are multiple conventions which is frustrating and confusing to those entering the field.

It doesn't seem particularly useful to go directly from MDH parameters to RPY angles, since in general a robot has more than one joint.

However, if you did want to go this path, then just looking at the expression we see that the orientation is a rotation about the x-axis of $\alpha$ followed by a rotation about the z-axis of $\theta$.  The XYZ RPY convention, often used for problems with robot manipulator arms, is defined as an SO(3) rotation $R = R_x(y) R_y( p) R_z( r)$ so it is clear that $\alpha$ is the yaw angle, $\theta$ is the roll angle, and pitch angle is always 0.


问:
Position Controller for a Quadrotor
I have a question regarding the implementation of a quadrotor's position controller.
In my Matlab model the quadrotor takes 4 inputs: a desired altitude ($Z_{des}$) and desired attitude angles($\Phi_{des}$, $\Theta_{des}$, $\Psi_{des}$) which reflects the motion described by the differential equations of the model (see last picture).

Here an insight into the implemented Matlab dynamic model. As you can see it has a structure like an inner loop controler:

Anyway...it "hovers" perfectly on the starting point. (perfect graphs :) )
Now I just need to go over and implement a sort of position controller to let the quadrotor to get from a start to a goal point, defined as usual through 3 coordinates $[X_d, Y_d, Z_d]$.

That's tricky because I don't have the same space state variables as input and output of the system. So the controller must take a vector of three coordinates and be able to output 3 different angles to get there. The only exception is the height because it will be simply bypassed by the controller and doesn't need another calculation loop. A different story is for the three angles...

My first idea was to simply create a feedback between the position given at the output of the simulated system and the desired position as in the figure above.
But that rises another question: my quadrotor model solves the following equation system:

$$
\large \cases{
 \ddot X = ( \sin{\psi} \sin{\phi} + \cos{\psi} \sin{\theta} \cos{\phi}) \frac{U_1}{m}  \cr
 \ddot Y = (-\cos{\psi} \sin{\phi} + \sin{\psi} \sin{\theta} \cos{\phi}) \frac{U_1}{m} \cr
 \ddot Z = (-g + (\cos{\theta} \cos{\phi}) \frac{U_1}{m} \cr
 \dot p  = \frac{I_{YY} - I_{ZZ}}{I_{XX}}qr - \frac{J_{TP}}{I_{XX}} q \Omega + \frac{U_2}{I_{XX}} \cr
 \dot q  = \frac{I_{ZZ} - I_{XX}}{I_{YY}}pr - \frac{J_{TP}}{I_{YY}} p \Omega + \frac{U_3}{I_{YY}} \cr
 \dot r  = \frac{I_{XX} - I_{YY}}{I_{ZZ}}pq - \frac{U_4}{I_{ZZ}}
}
$$

that means that they expect (as in the matlab model above) the desired angles and height.
But now I need right the inverse: given a desired position calculate the right angles!!!
For the direction is the solution really simple, since I can write something like:

```<code>Psi = atan2( (yd - yactual), (xd - xactual) );
</code>```

where y and x lies on the horizontal plane. This is not so simple for the other two angles. So what can I do at this point? Just "invert" the given equations to get the desired angles?

Another idea could be to implement a simple PD or PID controller. This is much more easier given the fact that I can experiment very quickly using Simulink and get very good results. But the problem is here again: how get I the desired angles from a desired position?

答: I have a suggestion. I have worked some time on px4 controller. For position control, it takes position as input and by feedback (PID) generates desired acceleration. Since quadcopter can generate acce. in Body Z axis , from this preliminary idea u can convert the desired thrust (vector) to attitude command and use the inner loop for attitude controller.</p>

Title: Position Controller for a Quadrotor


问:
Which controller (PI or PD) is generally more robust to noisy sensor measurements and why?
Assuming no filter is used to measure error (or rate of change in error) with respect to a desired state, would a PI controller more/less/about equally robust to signal input noise compared to a PD controller? And why?

My intuition tells me that they would be about equally bad/good because wild changes in the measured state would generate equally bad spikes in either the I or D terms.  I have, however, read some (probably not reliable) sources alluding to the possibility that PI controllers are generally more robust to noisy measurements than PD controllers.  Is this true? And if it is true, why would a PI controller be more robust to noise than a PD?

答:
One of those multiplies by the sampling time and one of those divides by the sampling time.

If your measurements are varying by, say, +/- 2%, and your control loop is running at 100 Hz, then an integral term gives you output swings of +/- (2% * 0.01) = +/- 0.02%, where a derivative term gives you output swings of +/- (2%/0.01) = +/- 200%.

As you can see, they do not create equally bad spikes.


答:
Proportional control amplifies equally across all frequencies.

Integral action amplifies low frequencies more, and high frequencies less (in fact, its gain at DC is infinite, and at infinite frequency its gain is zero).

Differential action amplifies low frequencies less, and high frequencies more (it has zero DC gain and unless its bandlimited, infinite gain at high frequencies).

Noise tends to be equally distributed across the frequency band.

So a PD controller will tend to be more sensitive to noise.  Whether it's more robust depends on your situation: there are some plants that can't be satisfactorily controlled without differential action.

Title: Which controller (PI or PD) is generally more robust to noisy sensor measurements and why?

Body:
Assuming no filter is used to measure error (or rate of change in error) with respect to a desired state, would a PI controller more/less/about equally robust to signal input noise compared to a PD controller? And why?

My intuition tells me that they would be about equally bad/good because wild changes in the measured state would generate equally bad spikes in either the I or D terms.  I have, however, read some (probably not reliable) sources alluding to the possibility that PI controllers are generally more robust to noisy measurements than PD controllers.  Is this true? And if it is true, why would a PI controller be more robust to noise than a PD?


问:
Multiple robots performing a set of tasks (Optimization)
Consider a situation where you have a 'm' number of robots and 'n' number of tasks. Each task is at a different location. What optimization algorithm should I use in order to assign a set of tasks to the robot. The constraints I am taking are battery percentage and distance.
How should I assign the tasks to the robots taking into account the constraints.</p>

答:
This seems like a mTSP (multi traveling salesman problem).

Summary: The Multiple Traveling Salesman Problem (mTSP) is a generalization of the Traveling Salesman Problem (TSP) in which more than one salesman is allowed. Given a set of cities, one depot where m salesmen are located, and a cost metric, the objective of the mTSP is to determine a tour for each salesman such that the total tour cost is minimized and that each city is visited exactly once by only one salesman.


答:
Well, there are a few reasons to that.
Servos  have a closed-loop mechanism, with an encoder or another device giving accurate feedback on the actual motor path. Steppers provide no feedback; control is through movement in incremental steps.
Servos normally boast consistent torque output over a wide speed range; copes well with short-term power bursts (e.g., due to rapid acceleration).

At low RPM, steppers performs better, while, closer to the maximum limit, torque is degrading; can provide holding torque in the power-off state.
There are also some other reasons for that trend. You can better idea of the difference in that article Steppers for a cobot

Title: Why are brushless servos more common than steppers in Cobots?

Body:
Essentially every cobot I've worked with uses BLDCs instead of stepper motors in the joints.  When searching for answers as to why I see a lot of posts that suggest one or the other, but nothing explains the consistent trend in the industry.  The closest I've come to an answer is that stepper motors are worse with dynamic loads, but I don't understand why.  Steppers have much higher precision due to their detant torque, so it seems like they should win out.

If anyone could help point me in the right direction I would greatly appreciate it.


问:
Buying DC motors with fitting gears
I want to try a little project with an arduino controlled crossbow turret. Therefore I need some motors to move the turret and gears. Since this is my first try on such a project I am very unsure on how to find the needed DC motors and gears that are usable with those motors. I need a little guidance for what motors I am actually looking for. The motors should only move the rotation of the crossbow (horizontal and vertical). I already found some motors with a torque stall of 8.5 kg/cm which I think is enough to move the turrent. But I still do not know what gears I can buy for that. I could not read anything up that tells me that gears are universally usable for every kind of motor or how to attach gears to motors actually.</p>

答: When selecting a servo you need to know the torque and the load profile. Not all types of actuators are equally suitable to become a robot component. Steppers and servo motors seem to have the qualities essential to build a robot. Some of the servos come with a integrated gera and controller. That could make the whole RD a bit easier and MPV production seriously faster. You can get some insights from this How to select a motor for a robot</p>

Title: Buying DC motors with fitting gears


问:
For robot wheel control : Brushless DC motor or Servo Motor?
Simply, when to use brushless dc motor and when to use Servo Motor ? </p>

<p>what are the differences , specially when adding an encoder to the dc motor you can have the position and it will be similar to Servo Motor ? </p>

答: It really depends on the economic details of the project you are working on. If it's a one time project with limited budget I would say a DC brush motor with  or without position feedback would do the job. Sure you will need a controller. If you aim to get a consistently high mechanical work output, precision positioning, and compact size you might draw your attention to servo motors. Some of them already come with a gear and a controller integrated. If you still feel embarassed about making the right choice, you could find some hints in here.What you need to know about robot servos before purchase</p>


问:
UR5 Visual servoing(image based, eye in hand)
I am implementing IBVS on a UR5 using ROS. The algorithm works very well in linear X, Y and Z and omega Y. When I try to add a fifth axis(omega X or Omega Z), the robot jerks heavily and goes into protective stop if the velocity is high, or it takes forever to converge if i reduce the velocity. How can I solve this problem?

The jacobian I'm using is generated from Peter Corke's Robotics Toolbox.

答: im facing the same problem as yours. And im  sorry if put this in the answer section(because i cant comment yet). I have try so many equation of orientation trying to fit which one able to work with RTB Jacobian(note that im using MDH notation), but none of them work. But recently i find a clue that in order to control the rotational part, we need to understand theory about Two-Vector Representation of end-effector and Rodrigues Matrix (lie group). But until now i didnt really know the correct relationship between that theory and inverse jacobian. I hope we can solve this together. </p>


问:
Mathematical prerequisites for beginning graduate student in robotics
A beginning graduate student in robotics asked me the areas of mathematics that he should brush up on (prerequisites) to begin a masters research program in robotics. What are some good materials/books that are indispensable for a research student? Which ones should we suggest in order that the student develops a solid foundation in robotics?</p>

答:
I'll add my $0.02. As other answers have said, it matters what you're studying. The more comfortable you are with the mathematics, the easier it will be to understand the concepts so I'd bias heavily on the math as an undergrad, expecially IF you're expecting to go to grad school and do research in robotics.

Linear algebra, 2 semesters

Differential equations.

Calculus, 3 semesters -- certainly through vector calculus, ideally with exposure to high dimensions (generalized Stokes' Theorem, differential forms, exterior algebra, etc)

Real analysis. Good intro to proof-based math and provides a basis for high-level math

Probability theory, 2 semesters. Ideally with some exposure to modern measure theory.

Differential geometry (if you want to do path planning, geometric control, etc)

Group theory, especially on Lie groups

You'll also want to take grad-level math courses in matrix analysis and maybe probability theory. My very favorite math class and book is Horn and Johnson's Matrix Analysis.

This list excludes non-math classes with a heavy math component like Lagrangian mechanics, data structures, algorithms, signals and systems, state-space control theory, etc.

In my humble opinion, a double major in CS and physics with classes in mechatronics and controls the ME/EE department would probably serve you best. You'll have a good grasp of the theory and then you can spend your summer internships getting practical experience.


问:
Can neural network probability directly be used as inverse sensor model?
For my robot i'm using semantic segmentation neural network that assigns to every pixel probability of being "road" (not occupied). By using homography matrix i'm re-projecting image to top-down view. The final goal is to build a map (poses are known).

I'm going to use simple algorithm "occupancy grid mapping" described in "Probabilistic Robotics" chapter 9.2. Core component of algorithm is "inverse sensor model" $p(m_i |z_t,x_t)$ - probability for cell $m_i$ being occupied given measurement $z_t$ and robot pose $x_t$

Will it be correct if i directly use probabilities from neural network as "inverse sensor model"?

```<code>if cell is outside current "top-down":
  return probability_0.5
else
  return probability_from_network
</code>```

答:
Sure, it's possible to assign the conditional probability of a pixel to the map, but there is an easier way for occupancy mapping. A normal neural network can learn the probabilities internally so the programmer doesn't has to care about the value by it's own. This will make the development process lighter and modern frameworks like Tensorflow have the natural ability in model building.

So the answer to the question is, that the neural network will learn directly the grid map and this allows the programmer to focus on other tasks like pathplanning and steering control. Probabilistic robotics is overestimated because the weights in the neural network are able to adapt to new situation by it's own.

[1] Zhang, Jingwei, et al. "Neural SLAM: Learning to explore with external memory." arXiv preprint arXiv:1706.09520 (2017).


问:
EKF SLAM : Missing older landmarks in new observation
I am a beginner to SLAM and robotics in general and I have been trying to implement SLAM on my GoPiGo3 robot car kit using primarily Chapter 10 from Probabilistic Robotics by Sebastian Thrun as reference.

I am using a laser distance sensor attached to a servo that turns only 180°. The sensor itself has a range of 2m. I am able to get the odometry data from the wheel encoders.

I am planning to use the RANSAC algorithm for line extraction and landmark detection.

Suppose I detect a landmark and add the global coordinates of the landmark into the state vector. Then, I move some distance and the newly added landmark into the state vector is no longer observable.

Is this a problem that is accounted for in the EKF SLAM algorithm from the book? Specifically I wish to know how missing landmarks in the new observation affect the transformation function that maps the predicted state to the predicted observation and thus the Jacobian. How will this affect the computation of the Kalman Gain and the measurement update of the state?

If it is not feasible to use this algorithm in such a situation, I would appreciate it if someone could point me to the right resources for my implementation.

Thank you!

答: EKF SLAM will have larger covariances for the missing landmarks. </p>


问:
how to implement autonomous navigation using lidar and hector?
I want to make an autonomous mobile robot, which can localise itself and navigate to the goal in an unknown environment.
As sensor, i just have a RPLidar to get laser scan, that's it. I used Hector SLAM to make a map of the room. But how do I implement navigation for the robot?
Move_Base, (specifically base_local_planner) requires odometry data. while hector gives pose update and slam out pose, they have a different message type compared to odom.

Please help.

答:
http://wiki.ros.org/amcl
You should try this and take a look at http://wiki.ros.org/turtlebot_navigation/Tutorials/Autonomously%20navigate%20in%20a%20known%20map
It provides the code to use amcl and RVIZ to navigate. Now you just substitute the fastslam here with hector slam, and write your own controller to replace the one in the package.</p>


问:
Robot mechanism parts names
I'm working on my first "robotic" project that involves a square platform that should drop one of the four sides at a time.

```<code>     side 1
  A----------B
  |          |
4 |          | 2
  |          |
  D----------C
     side 3
</code>```

My idea is to mount {something} in each corner [A, B, C, D] and for example to drop to side 1 I would pull down corners A and B. Then back up again and drop another side. The platform will be circa 150 x 150 mm and has to tilt to circa 45° therefore that {something} has to go down by around 150 mm as well.

What is that {something} that I should use? I was googling for a while but I don't know the right names for the parts, that makes googling quite challenging. Is it a gear rack? Or Linear actuator? Or some kind of electric piston?

Is there any site somewhere for aspiring makers that lists the common parts and mechanism items? And ideally where to buy them cheap too (this is a little school project on a very limited budget).

答: This is called a Stewart platform. You can use any linear actuator type, hydraulic, pneumatic or electric. </p>

Title: Robot mechanism parts names


问:
Integreation Method In Dr with 3 Sources
I'm searching a navigation method to find my location while dead reckogning (gps signals jammed) with an imu and gsm based navigation results.
In short i have 3 sources:

Gsm Based Navigation ( gives lat lon data at @5hz, around 100m accuracy)

Imu(gyro+accelerometer)

GPS Receiver

There are filters like kalman, complimentary etc. but many use this methods when they have 2 source (gps/imu)in my case i have 3. What is simplest way to integrate gsm based pos and imu data in dr and should i use a filter or not.
Thanks.

答: You should definitely use a filter. You need to adjust to the noises. If you can design or know the state space models of the agent and the sensors then KF is the easiest to use.</p>


问:
Robot arm with steppers - speed
I am using a 5 axis Robot arm (Robolink robot from Igus) to perform some 3D printing routines.
By now I am trying to use them and have him have some good position (and speed) control.
The robot is atuated by stepper motors (encoders included), but, generally, it lacks stiffnnes. When I tech him some consecutive reference positions its movement is not very soft.
I believe I can improve its behaviour by tunning speed references (maximum speed, minimum speed, reference speed) of each joint.
Can anybody give me some hint on how to set good speed references?</p>

答: I think the softness you describe has to do with stepper motor step sizes, try micro stepping to increase smoothness. And devise a motion profile with 3rd order spline to have acceleration equal to zero at turning points.</p>

Title: Robot arm with steppers - speed


问:
Python PID tuning - Parrot Bebop 2 - Target follower - ROS
I'm developing a PID controller for making the drone follow a detected object.

I used as reference these topics:

Ttracking-objects-from-camera-pid-controlling-parrot-ar-drone

Tracking-objects-from-camera-pid-controlling-parrot-ar-drone

I wrote a Python code that interacts with ROS bebop_autonomy package.

For the detection, I used YOLO.

The problem is that I'm not sure if the PID that I wrote is correct and I don't know how to tune the gain parameters. I have read about the theory of PID tuning like What are good strategies for tuning PID loops?

I also know that with an input step you can graph the response and then based on it tune the gains until you obtain the desired characteristics but here I don't know how to do the graphs as the input is a pixel value of the bounding box.

Do you know how should I tune the gain parameters? Also, let me know if how I developed the PID is correct or if there is a better way to do it.

Code:

```<code>import rospy
from darknet_ros_msgs.msg import BoundingBoxes
from std_msgs.msg import Empty
from geometry_msgs.msg import Twist

def set_state(input):   #possible states: "pd", "np " person detected, no person
    global state
    state = input
def get_state():
    return(state)

def callback(data):

Cy_center = 340 #x center position of the image
Cz_center = 240 #y center position of the image
Area_id = 640 * 480 * 0.30 # 30% of pixel area (for x)
K_x = 1 #vel_x paramete
sum_error_x = 0
sum_error_y = 0
sum_error_z = 0

#parameters to tune
Kp_fb = 0.2 #proportional gain FORWARD/BACKWARD
Kd_fb = 1.5 #derivative gain FORWARD/BACKWARD
Ki_fb = 1 #integral gain FORWARD/BACKWARD
Kp_lr = 0.2 #proportional gain LEFT/RIGHT
Kd_lr = 1.5 #derivative gain LEFT/RIGHT
Ki_lr = 1 #integral gain LEFT/RIGHT
Kp_ud= 0.2 #proportional gain UP/DOWN
Kd_ud = 1.5 #derivative gain UP/DOWN
Ki_ud = 1 #integral gain UP/DOWN

state = get_state()

pub = rospy.Publisher('bebop/cmd_vel', Twist, queue_size = 1)
pub_land = rospy.Publisher('bebop/land', Empty, queue_size = 1)
pub_takeoff = rospy.Publisher('bebop/takeoff', Empty, queue_size = 1)
empty_msg = Empty()
twist = Twist()

if state == "np":
    twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0;
    twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = -0.3;
    pub.publish(twist)
    print("Looking for person")

for box in data.bounding_boxes:

    if box.Class == "person":

        set_state("pd")
        Cy = box.xmin+(box.xmax-box.xmin)/2;
        Cz = box.ymin+(box.ymax-box.ymin)/2;

        Area_ref = ((box.xmax - box.xmin) * (box.ymax - box.ymin))
        error_y = Cy_center - Cy # LEFT/RIGHT
        #error_z = Cz_center - Cz # UP/DOWN
        error_z = Cz_center - box.ymin #so drone will fly higher
        error_x = Area_id - Area_ref #FORWARD/BACKWARD
        vel_y = error_y/320
        if vel_y >= 1:
            vel_y = 1
        elif vel_y <= -1:
            vel_y = -1
        vel_z = error_z/320
        if vel_z >= 1:
            vel_z = 1
        elif vel_z <= -1:
            vel_z = -1
        vel_x = (error_x*K_x)/Area_id
        if vel_x >= 1:
            vel_x = 1
        elif vel_x <= -1:
            vel_x = -1

        #PID X AXES
        sum_error_x = sum_error_x + vel_x
        last_error_x = vel_x
        output_x = Kp_fb * vel_x + Kd_fb * (vel_x - last_error_x) + Ki_fb * sum_error_x

        #PID Y AXES
        sum_error_y = sum_error_y + vel_y
        last_error_y = vel_y
        output_y = Kp_lr * vel_y + Kd_lr * (vel_y - last_error_y) + Ki_lr * sum_error_y

        #PID Z AXES
        sum_error_z = sum_error_z + vel_z
        last_error_z = vel_z
        output_z = Kp_lr * vel_z + Kd_lr * (vel_z - last_error_z) + Ki_lr * sum_error_z

    #SEND COMMANDS
    twist.linear.x = 0.5*output_x; twist.linear.y = 0.5*output_y; twist.linear.z = 0.5*output_z;
    twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0;
    pub.publish(twist)

    rospy.loginfo(
        "vel_x: {}, vel_y: {}, vel_z: {}, Classe: {}".format(round(output_x,2), round(output_y,2), round(output_z,2), box.Class)
    )

def main():
    set_state("np")
    while not rospy.is_shutdown():

    rospy.init_node('listener', anonymous=True)
    rate = rospy.Rate(1)
    rospy.Subscriber('/darknet_ros/bounding_boxes', BoundingBoxes , callback )
    set_state("np") #reset state to no person in case detector fails to detect
    rospy.spin() #blocks until ros node is shutdown

if __name__ == '__main__':
    try :
        main()
    except rospy.ROSInterruptException:
        pass
</code>```

答:
```<code>    #PID X AXES
    sum_error_x = sum_error_x + vel_x
    last_error_x = vel_x
    output_x = Kp_fb * vel_x + Kd_fb * (vel_x - last_error_x) + Ki_fb * sum_error_x

    #PID Y AXES
    sum_error_y = sum_error_y + vel_y
    last_error_y = vel_y
    output_y = Kp_lr * vel_y + Kd_lr * (vel_y - last_error_y) + Ki_lr * sum_error_y

    #PID Z AXES
    sum_error_z = sum_error_z + vel_z
    last_error_z = vel_z
    output_z = Kp_lr * vel_z + Kd_lr * (vel_z - last_error_z) + Ki_lr * sum_error_z
</code>```

Your error for the derivative terms are all wrong. vel_x - last_error_x is always zero because you assign the last error variables as vel variable.

Body:
    #PID X AXES
    sum_error_x = sum_error_x + vel_x
    last_error_x = vel_x
    output_x = Kp_fb * vel_x + Kd_fb * (vel_x - last_error_x) + Ki_fb * sum_error_x

    #PID Y AXES
    sum_error_y = sum_error_y + vel_y
    last_error_y = vel_y
    output_y = Kp_lr * vel_y + Kd_lr * (vel_y - last_error_y) + Ki_lr * sum_error_y

    #PID Z AXES
    sum_error_z = sum_error_z + vel_z
    last_error_z = vel_z
    output_z = Kp_lr * vel_z + Kd_lr * (vel_z - last_error_z) + Ki_lr * sum_error_z

Your error for the derivative terms are all wrong. vel_x - last_error_x is always zero because you assign the last error variables as vel variable.


问:
Helium Balloon Altitude and Velocity Control System
I am designing a helium balloon which implements a control system that can expand and contract the balloon using a motor, thereby increasing or decreasing buoyancy and, subsequently, acceleration. I want to be able to send the balloon to a target altitude without overshooting. I initially thought of using two PID controllers:

First PID compares actual and target altitude and outputs a target velocity

Second PID compares actual and target velocity and drives the motor

This approach has been problematic, because the rate at which acceleration changes as the motor expands or contracts is about $\frac{1}{6} \frac{m}{s^2 \cdot s}$ at most. Tuning the system typically results in wild overshooting and oscillation, even when using Zeiger-Nichols tuning heuristics.

My question, then, is what control system would be best to ensure I don't overshoot my target altitude? I still want to approach my target quickly, but need to make sure I can reduce speed before oveshooting. I have researched LQRs and Full-State Feedback controllers, but I don't have a mechatronics background and found the material to be quite confusing. Before researching further, I want to make sure I spend my time researching the best-fit solution for this problem.

Thank you very much in advance!

答:
Two thoughts occurred to me when reading your question:

Although I haven't analyzed your specific problem, I wonder if it is beneficial to have two full PID loops.  In motion control applications, it is common to have a position controller whose output feeds into a velocity controller, but typically one of these controllers will be PI (or PID) and the other will only have a proportional term.

Because you have a physical system (and not just a model on paper), you have physical limits.  Classical control theory assumes linear systems, but as you've noted, you can only change your system at a certain rate - this is called saturation, and it is a non-linear effect.  To know if your system is in saturation, you can check your acceleration rate (or rate of change of acceleration, in your case?) and see if it's at its limit.  When you're asking your system to change faster than its capable of changing, you will almost certainly introduce overshoot.

One way to avoid this is to limit your reference to values and rates which you know your system can achieve.  For example, don't ask your balloon to climb to 1000 ft instantaneously.  Instead, ask your balloon to climb to 1000 ft over a period of many minutes.  Depending on your system dynamics, you many need to apply acceleration and jerk limits as well.


问:
How does 3D Lidar work?
It is a beginner question for most of you, I had Googled this question but I didn't find any satisfying answer. What is the returned raw data from lidar, the difference between 2d and 3d lidar? How we map with lidar? I need mathematical relations of this process.</p>

答:
Lidar, sonar, and radar all work generally the same:

Emit a pulse. For radar, this means briefly energizing an antenna. For sonar, it means briefly energizing a sound transducer/speaker. For lider, this is means briefly flashing a light (typically a laser).

As you emit the pulse, start a timer.

Wait for a reflection to return. For radar, this means an electrical pulse in your antenna. For sonar, it's an acoustic pulse at a microphone. For lidar, it's a light flash at a photosensor.

When you detect the reflection, stop the timer.

Calculate the distance traveled by your reflected pulse by dividing the speed of transmission by the elapsed time. For radar and lidar it's the speed of light, for sonar it's the speed of sound. These speeds will vary depending on the medium through which your pulses are propagating. For the speed of light, the variation may not be big enough to matter, but for sonar it will definitely matter. For underwater sonar things like salinity, temperature, etc. is enough to make a big impact.

The distance calculated in step 5 above determines how far the pulse traveled. Assuming the pulse transmitter and the receiver are co-located, this would be twice the distance to the obstacle because the pulse has to travel out to the object and then back to the receiver.

Associate the distance to the obstacle with the direction the transmitter was pointed. This should now give you a distance and heading of the object with respect to your transmitter's base frame. This is how scan data from a laser scanner is typically returned - as a distance and angle (i.e., polar coordinates).

Finally, once steps 1-7 are completed, the emitter is pointed in a new direction and the process repeats. For lidar systems, typically a laser points down at a mirror that is angled at 45 degrees. The mirror spins about the vertical axis and, in doing so, continuously re-aims the output of the laser. Radar systems use motors to spin the antenna/dish. Sonar systems (especially underwater sonar) actually tend to be constantly oriented and are towed/pulled along.

These are the essentials of how all of the detection and ranging systems work. Here are some finer points that were difficult for me to understand when I was trying to learn more about radar systems. These seemed to be points that were clearly understood by most authors, but nobody laid any of these points out in any singular text that I ever read:

You don't get any information about the width of the object that causes the return. Usually, for radar and sonar systems, it's assumed that the object that causes the reflection occupies the entire beam width at whatever the calculated distance is. This can cause smearing and poor resolution if the beam isn't narrow enough. For lidar systems the returns are generally treated as a single point in the center of the beam.

Following on the above, it's possible that the object doing the reflecting only occupies a portion of the "illuminated" or pulsed area, and it is possible (read:likely) to get a subsequent return from whatever is behind the first object. For lidar systems, rain, dust, etc. will frequently cause this. Whether or not you get the data about subsequent returns is generally referred to as whether or not the device has "multi echo" capability.

Also following on points 1 and 2, it's not possible to discriminate against a portion of the pulse that weren't intending to listen for. No device has perfect edges to the transmission beam, but highly reflective objects that are outside of the design beamwidth may be reflective enough to generate a detectable echo with the low-energy portion of the pulse. This creates a star- or plus-shaped "dazzling" pattern in the return data, like you can see below.

It is entirely possible for the device to receive signals that it did not generate and to treat those as valid returning pulses. This is known as "jamming" and happens if there is some external emitter that is (coincidentally or intentionally) transmitting a similar enough signal to be accepted by the receiver.

Following kind of on point 4 above, there are generally times when the receiver won't detect anything at all. For systems like radar and sonar, the aperture that generates the pulse is the same object that will do the detection and so there is some switching time where the residual motion/charge that is in the aperture needs to be adequately dissipated until the receiver can "listen" for the return. For lidar systems there may be a need to change the optical return path to change from the emitter->lens to lens->receiver. These switching times will determine the minimum range for the system.

Finally, regarding your question about 2D versus 3D, this generally refers to whether or not the device is detecting points along a single line (a single scan plane) or along multiple lines (multiple scan "planes" which, for non-zero "plane" angles, actually create scan cones).

:EDIT:

Forgot - here's a good picture from a synthetic aperture radar system showing some highly reflective objects - they make the + patterns in the data where they're creating a detectable reflection even though the radar isn't pointed directly at it. It's kind of like a lens flare, but not exactly the same mechanism.

Title: How does 3D Lidar work?


问:
Best way to turn large "lazy suzan" bearing with stepper motor
I'm designing an automated object capture turntable (take photos -> photogrammetry) where the platform the object sits need to rotate, driven by a stepper.

The catch is the base is glass as I need to view the object from beneath and the bearing sits around the edge of the glass plate, not at it's center.

This is the bearing I'll be ordering, it has a 24" diameter:
https://www.amazon.com/gp/product/B071CPFCYS/ref=ox_sc_act_title_1?smid=A2ZKGRENV5NC9J&th=1

Now I need a way to mechanically couple a stepper motor to the inside or outside of the bearing.  I'd like to use something more precise like timing belt as opposed to rollers.

Is there an existing product/technique for interfacing large wheels with stepper motors?

Update: As per  jsotola's suggestion I did some searching, best thing I could find was this:
Scooter Wheel Adapter for 5mm Shaft

My origional ideas was to glue gt2 belt around the inside/outside, only just rediscovered the link to my inspiration: Make Your Own Gears Out of Wood and Timing Belts

答:
Have a machine shop make you big gear or timing belt pulley that fits over the outside of the bearing.

Note that if you a position reference on the outer edge of the glass you can use encoder feedback and a DC motor -- you should be able to achieve more precision this way.


问:
Can drone fly in predefined fixed trajectory without GPS
Is it possible to make a drone to fly in fixed altitude and move in fixed trajectory without GPS. (Drone has to move in fixed path which is already defined).
I am saying without GPS because drone has to move inside the hospital / office (moving from one floor to another floor).

I searched google and I find GMapping Slam for trajectory movement. But this does not work when moving from one floor to another floor.

I there any options I can use to achieve my requirement.

Thanks

答:
In the situation you describe, it is the easiest if you install "eyes" to the drone. Have at least 6 sensors to detect the closest obstacles (walls, ceiling, floor) on the 3 coordinates.

Additionally, you should have a "map" of the building, so the drone can navigate through it. GPS uses the same idea: some coordinates + some map.

However, you must be aware that inside a building there are many more things, than just walls. And a hospital is even more special. There are a lot of fixtures, lots of cables, conduits, lamps, perfusion "posts" (or however they are called). Even with installing good resolution cameras as "eyes", I still think that there is a significant safety risk for operating the drone within a building. What about the air flow from AC units? What about the drone accidentally crashing inside a patient under surgery? What about the drone running out of "juice" and just "crashing" wherever it happens to be?

While it is doable as a hobby / research, real-life use has limited applicability.

Did I say the sometimes ceilings are very close to the floors, and the drone will start navigating through the people's haircuts? :)


问:
EKF SLAM : SLAM specific Jacobians for new landmarks
I am currently trying to understand the books SLAM for dummies and Simulataneous localization and mapping
with the extended Kalman filter to implement slam. I have understood steps 1 and 2 SLAM for dummies. However I am having difficulty understanding Step 3: Add new landmarks to the current state (Page 40). The corresponding section in Simulataneous localization and mapping
with the extended Kalman filter is 2.3.4 Landmark initialization for full observations.

Specifically, I do not understand how the SLAM specific Jacobian $J_{xr}$ (SLAM for dummies) defined below is actually derived:

I am confused about what transformation function the Jacobian $J_{xr}$ is linearizing. I notice that $J_{xr}$ is the same as Jacobian $G_R$ that linearizes the Inverse Observation model as described in section 2.3.4 of Simulataneous localization and mapping
with the extended Kalman filter. Why does $J_{xr}$, contain $\Delta t$, which is the thrust applied to the robot? $\Delta t$ which belongs to the State Prediction model shouldn't be in the Inverse Observation model, no? Am I right to say that the Inverse Observation model is a function of the robot pose : [$x_{r}$, $y_{r}$, $\theta_{r}$] and the new landmark observation:  [$d_{l1}$, $\phi_{l1}$] and the output is the updated state vector : [$x_{r}$, $y_{r}$, $\theta_{r}$, $x_{l1}$, $y_{l1}$]?

答:
Updating the covariance matrix for a new landmark always confused me, the equation is often given but I've never seen it derived.  I had a go at deriving it here

http://petercorke.com/wordpress/ekf-covariance-matrix-update-for-a-new-landmark

I'd be interested to know if you find this useful.


问:
Mechanism for changing gears on a bicycle
I'm looking to make an automatically shifting bicycle for my senior design project (along with some additional features TBD). However, I come from an electrical/software background; not a mechanical one. So I need to figure out a decent way to shift gears. I was thinking of leaving the gear system in place as is and using some sort of motor (servo or stepper motor with worm gears) to pull and release the wire cable as needed. However, I have some concerns with this; namely the amount of torque needed to pull the wire and finding something with enough holding torque. Perhaps my best option is to use the trigger shifters on as well and perhaps use a solenoid. My other concern (namely with the worm gear) is that it'll be too slow.

So I would like to pick your brains here for a moment. Thanks

答:
Depending on your budget, for the mechanical part you could use one of the electronic derailleurs on the market and simply add send it signals from your microcontroller.

One of the fancier ones I've seen is the Shimano Di2 Dura Ace 7970 It's not cheap though, it's about \$2200 for the set or \$500 for the rear derailleur alone, \$690 for the shifters, plus another \$85 for the battery.

If you're looking for something cheaper, you might want to check out a project from some Carnegie Mellon students. Their wiki goes into more detail. It looks like they used an Arduino controller, hall effect sensors and four servos to control the shifting based on rider cadence.


问:
Calculate the inertia matrix of complex robotic mechanism
Motor, irregular rigid body of Different materials Forming a single robotic.

For example, Industrial robot, every geometric shape of the link is very complex. But we can find inertia matrix form CAD software, how these software tools calculate the inertia matrix of a single robotic arm? Is this inertia matrix accurate or equivalent?

答:
CAD software knows everything about your parts...(assuming you tell the things it doesn't know...as mentioned, material properties) After doing this, calculating multibody C.O.G or even inertias of complex bodies is 'trivial' (of course, it wasn't for the programmers).

These things can be very accurate, or can be made very accurate, however they can and usually will be off by a certain tolerance. How much one is willing to accept for an error is up to the user.

They can also be wrong simply because something as trivial as the electronics and wires within your body weren't modelled correctly, or given the correct initial data, on top of that, no material is perfectly homogenous, which CAD software always assumes. This may not even account for even 1% error and not matter for something small, but is extremely important when dealing with large heavy loads.

Because of this, it's almost always a good idea to experimentally verify such data yourself, using trifilar pendulums as an example or other equipment and methods

I generally use CAD data as a good lower boundary for initial guesses with empirical verification.

Of course, this again will only be as accurate as your measurements and is an entire branch on it's own.


问:
How to render a robotic arm in Unity?
As part of a research project, I'm creating synthetic data, using the Unity engine.
I have the final pose of a robotic arm (6 params: 3 for orientation, 3 for location/position), and also a .ply file for the mesh. Is there any way I can use this information directly to render an image in Unity?

Update:

I was thinking of the following approach (though I am not sure how well this will translate to Unity). I have the Camera transform, so I can have the coordinates of the end effector in the Camera frame. My camera and light source are the same, so I was thinking that in a renderer (say Unity), I'd fix the origin of the 3D world as the camera+light point, and then if I could somehow input the end effector coordinates (x,y,z and thetax, thetay, thetaz) in the frame, that would align the arm in the camera (final position) as I need it, and then I'd render the image.

Also as of now, my ply file is really simple and has only 3 joints (end effector, of course, being the end point of the last).
Is this approach viable in Unity?

.ply File,  Image

答:
You can convert the PLY files to OBJ fromat using MeshLab (free). You can import the OBJ files to Unity. More on how to do this can be found here.

I assume you have an obj file for all segments of the robotic arm, so you will have to recreate the chain of GameObjects that correspond to the linkages and attach the meshes to these. You have to take care of coordinate systems. You will have to have a Coordinate System (In Unity a Transfrom attached to a GameObject) at each joint (for motion) and probably, relative to the joint another one, relative to which the mesh vertex coordinates are expressed (they might coincide).

If you want to solve the inverse kinematics problem (calculate joint angles from end-effector pose) you can google for analytic solutions, iterative solutions or you can try the built-in Unity IK Solver.

If you want to render the image "as seen by a camera on the end-effector" you can just make the camera the child (or a component on) the last linkage of the robot.

Title: How to render a robotic arm in Unity?


问:
How to determine x-axis if the two z-axis are parallel in Denavit Hartenberg representation
I have problem of determining the DH table, specially finding x axis when two z axis are parallel to each other </p>

答: Make them parallel to the previous X Axis. This helps reducing the number of non-zero coordinates in the DH table which in turn means less + and * operations in the calculations.</p>


问:
Occupancy grid mapping
I want to construct 2D occupancy grids for mapping of an unknown environment. Could you guide me how can I achieve it?

What I have researched is that robot can construct its map given sensor data using recursive Bayesian estimation algorithm. Is it correct?

答:
Yes you do need some sort of sensor data such as LIDAR data.

If you are planning to perform 2D SLAM then you could start playing around with the ROS package gmapping which will help you construct a 2D occupancy grid using LIDAR data.

Here is a simple tutorial that shows you how to use gmapping for this use case. Here is another tutorial that shows you how to do this in simulation.

This youtube video is also a good guide.

Title: Occupancy grid mapping


问:
Why Euler Angle is set to be in ZYZ order?
We have 3 DOF for rotation in 3D space. So to describe an arbitrary rotation, we need to describe its 3 DOF. Euler angle does this by dividing a rotation in 3 steps, first rotate along the Z axis of the world frame ($z_0$), then rotate along Y axis of current frame ($y_1$), and finally rotate along the Z axis of the current frame($z_2$).

And of course we have other conventions like z-x-z, x-y-x, y-z-y, x-z-x, y-x-y.

And my question is, why the euler angle is define in this "ZYZ" manner? Why the first and third rotation are about the same axis (of course the first Z and the last Z are in different frames) ? Why not to use ZYX?

答:
There are many conventions for Euler angle sequence, just are there are many conventions for coordinate system axes.

Robotics generally uses right-hand coordinate frame for robots and robot parts, with x-forward, y-left, z-up.  Euler angles in robotics are generally ZYZ ; using an a-b-a sequence (ZYZ, YZY, XZX, ...) rather than a-b-c (XYZ, YZX, ZYX, ...) sequence helps avoid singularity/gimbal lock issues.

However, robotics also often uses the optical/cameras/sensor-systems coordinate frame convention of z-forward, y-down, x-right for cameras and some other pixel-wise sensors which project the 3D world to a 2D image/matrix.

For an example of making conventions explicit, see the ROS REP 103 on metrics, coordinate systems, and rotation representations, and REP 105 on coordinate system naming, chaining, and conversion to Earth-centred/global systems and maps.

There are lots of other conventions. For example, see this explanation of the NASA Standard Aeroplane and NASA Standard Aerospace coordinate systems, which are x-fore, y-up, z-right  and  x-fore, y-up, z-right.

edit:  This paper (Rotations in Three-Dimensions:
Euler Angles and Rotation Matrices) gives a clear summary of the Euler angle parameters on pages 1 & 2, gimbal lock on pages 5 & 6, and problems with averaging Euler angles on page 8.


问:
Most accurate rotation representation for small angles
Assume that I have a rigid body for which I know that it can rotate with respect to a global reference frame (which is considered fixed and already given) for only a few degrees of angle, so I can describe its rotation by using the small angle approximation. For this system, I would like to know if there is a rotation representation that offers more accuracy when compared with other representation methods.

The main representation methods that I considered are the euler angles and the pitch-yaw-roll transformation. To my perception, I think that pitch-yaw-roll representation is expected to be more accurate, since all the angles are expressed with respect to the initial coordinate frame. On the other hand, euler angles are defined on different frames, so I am not sure if the resulting angles will be really small.

To sum up, I know that the body can rotate for only a few degrees and I would like to know which coordinate representation is much probable to deliver the smallest angles, such that the small angle approximation is more valid.

It could also be the case that there is not a general answer (so it depends on the specific configuration) but still I haven't found anything about this topic on the related literature!

Edit: This question is not related to numerical issues. Therefore, it is assumed that all the possible rotation matrix descriptions (Euler, PYR etc) result in the same, exact coordinate vector. Therefore, the question is if there exists one parametrization that is composed of the smallest possible angles.

Example (no small angle approx used): Assume I have a coordinate frame which describes a point in space by the following vector

$P2=\begin{bmatrix} 4 \\ 1 \\ 0.05 \end{bmatrix}$.

Given another coordinate frame which is rotated with respect to the previous one, the description of the same point is given by

$P1=\begin{bmatrix} 3.8933 \\
    1.3566 \\
   -0.0630 \end{bmatrix}$.

Using Euler angles, I can find that the rotation matrix $R_{euler}$ is characterized by the angles $0.1,0.2,0.1$ rads, which correspond to the angle of rotation around z axis, the rotation around the resulting y axis and the rotation around the resulting z axis, respectively (these are basic stuff, it is explained in many books.). So I have that $P1=R_{euler} P2$.

Now I want to find the corresponding rotation matrix if I use the pitch-yaw-roll representation. Here I have to solve an optimization problem and the solution that I get (maximum error between P1 and the estimated P1 is $3 \times 10^{-8}$) delivers me the following angles

$\begin{bmatrix}  -0.0103   \\ 0.0257  \\  0.0902\end{bmatrix}$,

which correspond to the rotation around the x,y and z axis of the initial coordinate frame.

答:
As I understand it, quaternions have the least issues with numerical approximations or discontinuities for a sequence of mujltiple rotations.  See this explanation of Quaternions, plus all the quaternion- and rotation-related pages on that site.  You'll also get numerical errors when you convert between representations.

However, for single rotations rather than chained sequences of rotations (hence minimal numerical approx errors) most of the other answers here are correct: the representations are equivalent.  Just avoid Euler a-b-c sequences like XYZ, ZXY, etc. to avoid gimbal lock.

edit: you can see how representation conversions lead to numerical approximations, from the code on this page - lots of trigonometric functions.


问:
Arduino for simple Data-Glove. Should I go with Mega or Due?
First, I'm a beginner in MCU/Robotic world (been working with ATMega+CVavr, but that's all). so please bear with me.

I'm making a prototype data glove (Like KeyGlove, but much more simpler), it consist of:

IMU sensors (MPU 9150 9DOF, all reading is fused with built in DMP) ->
Reads hand position and orientations

Minimum of 2 flex sensors -> Reads Figer flexion

MCU (well, Arduino to be specific)

The sensors are plugged in to the Arduino and the reading will be filtered (e.g Low pass, Kalmann) in the Arduino before being transferred over serial to PC. The PC will then translates the data into virtual gripper to move an object (VR)

Initially I planned to use UNO +  Pansenti’s MPU9150 Library in my code, then I realised the flash memory size left would be so tiny (i.e MPU9150 lib code size is ~29k, Uno has 32k).
My project is still in very early stage, so a lot things are expected to be changed and added, with so little flash memory left. I can only do so much.

I immediately looking for Mega as replacement (256k flash) but I realised there is also newer Due with faster processor.

They cost effectively the same as for now.
.

My main concern here is the robustness and compatibility when:

Designing  the HW interface to Arduino (making circuits, addding
shield)

Code development (available library)

Streaming and filtering the sensor readings (would 32 bit MCus helps, or it's overkill?)

I know this question might sound as too localized, but I believe a lot of projects that utilize multiple sensor reading + filtering similarly will also benefits from this discussion.

I'll revise the question if it's needed. The main question is probably
Would 32 bit MCUs perform significantly better in multiple sensor reading and signal filtering compared to 8 bit MCUs?

Or in my case.. should I go with Mega or Due?

答:
The number of bits in the instruction set for the MCU will directly affect the precision of the math operations on that given MCU. If you're going to be doing a lot of 16/32 bit math operations, then yes, a 32 bit MCU will perform significantly better compared to an 8 bit MCU.

This article may help you understand the difference: http://www.embedded.com/electronics-blogs/industry-comment/4027627/The-8-bit-MCUs-won-t-be-going-away-anytime-soon.

The Due has an 84MHz CPU vs the Mega's 16MHz...this isn't even taking into account that a 32-bit core allows the Due to perform operations on 4 byte wide data within a single CPU clock cycle, whereas the Mega will need a clock cycle for each byte at least.


问:
Robotic arm for university student
Can anybody recommend a robotic arm to be possibly coupled with VR to be used for educational purposes for university students within a bucket of about 15000€?</p>

答: I've made a bit of a list of what's available for teaching robots.  </p>

Title: Robotic arm for university student


答: The default decision would be a UR5. It is a bit out or your range, but it could be worth to check with UR if they give you a university discount. Main advantage is the huge user base, rather good support in ROS and easy things like following waypoints can be done in python within hours of unpackaging. </p>

Title: Robotic arm for university student

Body: Can anybody recommend a robotic arm to be possibly coupled with VR to be used for educational purposes for university students within a bucket of about 15000€?</p>