问:
Kalman Filter Design
I'm new to Kalman filter design and I'm struggling to understand how to apply the Kalman filter methodology to my problem. I've read a research paper which seems to describe what I'm trying to do https://journals.sagepub.com/doi/pdf/10.5772/57516. The paper describes experiments in which the data collected consists of a noisy measurement paired with a truth measurement. In the paper the Kalman filter matrices are designated as follows:

$$\boldsymbol{X}=\begin{bmatrix} b_{x} &P_{11} &P_{12} &P_{13} \end{bmatrix}^{T}$$

$$\boldsymbol{H}=\begin{bmatrix}
        1       &r              &0          &0\\
        1       &-r             &0          &0\\
        1       &r/\sqrt{2}     &r/\sqrt{2} &0\\
        1       &r/\sqrt{2}     &0          &r/\sqrt{2}\\
\end{bmatrix}$$

$$\boldsymbol{\Phi} = \boldsymbol{I}_{4\times4}$$

The state vector describes bias and scale factor states. Whilst I'm not entirely sure how the H matrix was derived I can't understand how the filter would be used with real data i.e. the observed measurement replaces the elements r in the H matrix, but the ground truth which this is to be calibrated or compared against is a single value i.e. we know the true rate or true acceleration, we do not know the true bias and scale factor states. How then does the filter include this ground truth which I assume it uses to update its estimate of the bias and scale factor terms. Could someone shed some light on how the Kalman filter is laid out to allow the estimation to use this pairing of measurement and ground truth?

答:
The measurements don't get inserted into $H$. The $H$ matrix is the "measurement matrix" or "output matrix" such that you get an estimate of the output when you multiply $H$ by your state vector estimate $\hat{X}$. You can see this in equation (23) in the paper you linked, on document page 7:

The model can be expressed as follows:

$$ X_{i+1} = \Phi X_i + w_i \\
Z_{i+1} = HX_{i+1} + \epsilon_{i+1} \\
$$

The Kalman filter action happens in equation (25), when the Kalman gain $K_i$ is applied to the measurement error or "residual." The measurement error is the difference between what you actually saw, the measurement vector $Z_i$, and what you expected to see, which is the output matrix $H_i$ times the predicted state $\hat{X}_{i|i-1}$.

The Kalman filter takes that modified residual and adds it to the predicted state to get the "corrected" or filtered state. It all happens as one equation in (25):

$$
\hat{X}_i = \hat{X}_{i|i-1} + K_i\left(Z_i - H_i\hat{X}_{i|i-1}\right) \\
$$

Where again the $\hat{X}_{i|i-1}$ term means, "given my previous state $i-1$, what does the model say my state at $i$ should be?" Rewriting it from the earlier equation:

$$
X_{i+1} = \Phi X_i + w_i \\
$$

or

$$
X_{i|i-1} = \Phi X_{i-1} \\
$$

or

$$
X_{i|i-1} = (I + A\Delta t)X_{i-1} + B\Delta t u \\
$$

which is just a numeric integration of

$$
\dot{x} = Ax + Bu \\
$$

such that

$$
x_i = x_{i-1} + \dot{x} \Delta t\\
x_i = x_{i-1} + (Ax_{i-1} + Bu)\Delta t \\
x_i = x_{i-1} + A\Delta t x_{i-1} + B \Delta t u\\
x_i = (I + A\Delta t) x_{i-1} + B\Delta t u \\
$$

Hope this makes more sense!

Title: Kalman Filter Design


问:
Problem designing acceleration inputs controller
I am reading this paper: https://www.researchgate.net/publication/224635390_The_motion_control_problem_for_the_CyberCarpet

in which is implemented a cartesian regulator to bring the position $(x,y)$ to $(0,0)$. I have built it as in the paper, so with velocity inputs, and the output is the following:

the final velocities has to be zero, and since I can only control position and orientation, I think it is correct that I get the plot on the right.

The problem is a posture regulatoin prolem. If we consider for example a unicycle, we have that the desired behavior is the following:

so with this type of controller the unicycle is driven to the origin.

from here: https://people.kth.se/~dimos/pdfs/IET15_Zambelli.pdf

where $R$,$\gamma$ and $\delta$ are the polar coordinates for the system. $\gamma$ can be defined as a pointing error, and $\delta$ is the orientation on the final position.

I use polar coordinates, because I have read in many papers that in this way we can use Lyapunov function to find the controller.

The controller is the following(written in a Simulink block):

```<code>temp = x^2+y^2;

%polar coordinates

R = sqrt(temp);
gamma = atan(tan(y/x))-theta;
delta = gamma + theta;          %not used now

sinc = sin(gamma)*sign(cos(gamma));

%linear and angular velocity
if R == 0
    v=k_1*((R^2)*sign(cos(gamma)));

% steering velocity

    omega = k_1*R*sinc;
else
    v=k_1*R*sign(cos(gamma));
    omega = k_1*sin(gamma)*sign(cos(gamma));

end

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

which is obtained throgh feedback linearization, but it can also be obtained using a Lyapunov method.

Now, I am trying to go a little furhter, and instead of using velocity inputs, I would like to use acceleration inputs. What I have done is the following:

I have defined v and omega as states of the system, and I have built a cascade controller. So, to the controller I hve just added these lines:

```<code>a = k_2*(v-v_s);               %linear acceleration
alpha = k_2*(omega-omega_s);          %angular acceleration

inputs = [a;alpha];               %acceleration inputs
</code>```

where v_s and omega_s are now the linear and angular velocities, which are states now and v and omega are now the desired linear and angular velocity respectively. And the model of the system (written in nother Simulink block) becomes:

```<code>x_dot = -v_s*cos(theta)+y*omega_s;
y_dot = -v_s*sin(theta)-x*omega_s;
theta_dot = omega_s;
theta_w_dot = -omega_s;
v_dot = a;
omega_dot = alpha;

state_dot = [x_dot;y_dot;theta_dot;v_dot;omega_dot];    %out of the block I integrate this and send it back as [x;y;theta;v;omega], which are the states of the system
</code>```

and if I do so, now I get the following plots:

so it diverges in practically everithing!

I don't understand what I am doing wrong. For the definition of the acceleration inputs I am trying to follow this paper, which is for a uniccyle but it should be similar, since they are both non-holonomic systems: https://www.researchgate.net/publication/276184094_Stabilized_Feedback_Control_of_Unicycle_Mobile_Robots  (at pag. 4)

My final objective is actually to build a controller for this system that converges both in position and orientation, so apply the concept written in the latter paper, at pag.3, to my system of study.

But I think now that the problem lies in the definition of the acceleration law.

Can somebody please help me understand what I am doing wrong?

[EDIT]If I try to substitute gamma = atan(tan(y/x))-theta; with gamma = atan2(y,x)-theta; I get:

which I think it is a little better then before, even if the acceleration profile is a bit weird I think.

Could you give me some help understanding if up to now I have done somehting wrong? The acceleration profile that I get seems to be not really ok to me, what do you think?

[EDIT] I wrote sinc as an abbreviation for sin*cos , sorry for the confusion, when I wrote it I really wasn't thinking about the sinc function.

答:
You've been asking lots of questions along these lines, and I've been there before - I know what it is to be at your wits end trying to find a solution. I'm going to try to help, but part of my "help" is to point out that I don't understand how your equations are derived. To that end, I'm going to show why I understand how they're derived. I'll start with some equations you've given:

```<code>x_dot = -v_s*cos(theta)+y*omega_s;
y_dot = -v_s*sin(theta)-x*omega_s;
theta_dot = omega_s;
theta_w_dot = -omega_s;
v_dot = a;
omega_dot = alpha;
</code>```

Okay, working from the bottom to the top, with the help of MathJax, it looks like

$$
\dot{\omega} = \alpha \\
\dot{v} = a \\
$$

Those make sense to me. Derivative of an angular speed $\omega$ is an angular acceleration $\alpha$, and the derivative of a linear speed $v$ is a linear acceleration $a$. Got it.

Moving on:

$$
\dot{\theta} = \omega_s \\
$$

and you said in your text that (replacing code with Mathjax):

where $v_s$ and $\omega_s$ are now the linear and angular velocities, which are states now and $v$ and $\omega$ are now the desired linear and angular velocity respectively.

You've got a $\dot{\theta_w}$ that I don't know what it's doing, but otherwise I'm still on board with your definitions - they're all identity so far. Where I have trouble is with the next line. Again, in Mathjax, you're saying:

$$
\dot{x} = -v_s \cos{\theta} + y (\omega_s) \\
\dot{y} = -v_s \sin{\theta} - x (\omega_s) \\
$$

So you've got $v_s$ as the linear velocity, so I can understand the sine/cosine of an angle times the linear velocity to get the components of linear velocity, but what is this $x(\omega_s)$ and $y(\omega_s)$ business? I'm assuming that it's your x/y position, but the x/y position should have nothing to do with your speed.

What you're saying is that, by changing your reference point for zero position, you're changing your speed. If $x = 0$ then $x(\omega_s) = 0$, and if $x = 100$ then $x(\omega_s) = 100*\omega_s$.

It feels like you're trying to combine a linear speed and a tangential speed, but the tangential speed would be an angular speed times a radius of curvature, but there's a lot lacking from that. It seems almost like you're maybe trying to do something like with a Coriolis force, but that would be a force that would impact acceleration, not velocity. And there's no $r$ for radius of curvature or rotating frame distance, etc.

I would think you should omit the $x(\omega_s)$ and $y(\omega_s)$ terms completely, but again I'm not understanding how they're derived or what they mean.

Continuing up:

```<code>a = k_2*(v-v_s);               %linear acceleration
alpha = k_2*(omega-omega_s);          %angular acceleration
</code>```

it looks like you've got acceleration as a linear function of the difference in reference and state speed, but what is $k_2$? The more important problem for me is that you have the same gain $k_2$ being used to drive both your linear and angular accelerations. This might make sense if you were doing some vector operation, but if you did it longhand like that then I'd expect a $k_1$ for the linear acceleration and a $k_2$ for the angular. Since I'm not seeing where $k_2$ (or $k_1$) is defined I can't give any feedback on that.

Now finally I work my way back up to your first set of equations, which have me totally lost. I would expect that your reference would be either a constant setpoint, like v=0, or the result of some profile, like v = MySpeedProfileGenerator(t). What I'm seeing instead is that your linear and angular speeds are again functions of distance, but not the x/y positions; just the radial distance $R$ from origin to the x/y point.

That is, you have:

$$
R = \sqrt{x^2 + y^2} \\
$$

and that's fine, but then why do your targets $v$ and $\theta$ depend on whether or not $R==0$? And, IF $R$ is zero, then:

```<code>if R == 0
    v=k_1*((R^2)*sign(cos(gamma)));
    omega = k_1*R*sinc;
else
</code>```

is just a really fancy way of writing

```<code>if R == 0
    v = 0;
    omega = 0;
else
</code>```

But I'm also not understanding the rest of it - what is $\theta$? What is $\gamma$ supposed to be? Finally, the biggest thing I don't understand here, what is this sinc function you've got, and what is it doing? You have:

```<code>sinc = sin(gamma)*sign(cos(gamma));
</code>```

But the sinc function is

```<code>sinc = sin(x)/x;
</code>```

so maybe you're abbreviating some other name? But why does the sign (not sine!) of cos(gamma) keep coming up?

I would almost guess you're trying to constrain it to run in a circle or something, the way these angles keep coming up, and that you're using an $R$ to drive your speed and... angle? I'm assuming theta is an input, but theta doesn't drive your values directly, gamma does, and that's a function of position.

The crazy thing for me is that I can't tell how or even if your position ever gets back to anything. You have position driving $R$ and driving $\gamma$, which means position also drives everything else, and then you're outputting $\dot{x}$ and $\dot{y}$, but you're never doing anything with x or y.

I thought you had given some drawings previously but I can't find those now; they're not in your list of questions for this site. I think that you should go back to the problem statement for your project and do the following:

Clearly define what you're trying to control - is it x/y position? Radial position? Heading? Is this a SISO system or MIMO?

Clearly define the kinematic relationships at play - this means you need a labeled drawing of your setup.

Pick Cartesian or Polar, and stick with it. You're begging for accidents by switching back and forth mid-block.

:EDIT:

As I was writing the last line of this answer, I had a sudden flash - I think maybe you're trying to convert from Polar to Cartesian in your x_dot and y_dot equations! But you're not doing it correctly. For one, you're using the linear speed instead of the radial speed. For two, you're using the wrong equation (but you're so close!):

You have:

```<code>x_dot = -v_s*cos(theta) + y*omega_s;
</code>```

where the proper polar-to-Cartesian conversion would be:

```<code>x_dot = R_dot*cos(theta) - r*omega_s*sin(theta);
</code>```

I hope all of this leads you to some insight into your problem.

ContentLicense: CC BY-SA 4.0

Body:
You've been asking lots of questions along these lines, and I've been there before - I know what it is to be at your wits end trying to find a solution. I'm going to try to help, but part of my "help" is to point out that I don't understand how your equations are derived. To that end, I'm going to show why I understand how they're derived. I'll start with some equations you've given:

x_dot = -v_s*cos(theta)+y*omega_s;
y_dot = -v_s*sin(theta)-x*omega_s;
theta_dot = omega_s;
theta_w_dot = -omega_s;
v_dot = a;
omega_dot = alpha;

Okay, working from the bottom to the top, with the help of MathJax, it looks like

$$
\dot{\omega} = \alpha \\
\dot{v} = a \\
$$

Those make sense to me. Derivative of an angular speed $\omega$ is an angular acceleration $\alpha$, and the derivative of a linear speed $v$ is a linear acceleration $a$. Got it.

Moving on:

$$
\dot{\theta} = \omega_s \\
$$

and you said in your text that (replacing code with Mathjax):

where $v_s$ and $\omega_s$ are now the linear and angular velocities, which are states now and $v$ and $\omega$ are now the desired linear and angular velocity respectively.

You've got a $\dot{\theta_w}$ that I don't know what it's doing, but otherwise I'm still on board with your definitions - they're all identity so far. Where I have trouble is with the next line. Again, in Mathjax, you're saying:

$$
\dot{x} = -v_s \cos{\theta} + y (\omega_s) \\
\dot{y} = -v_s \sin{\theta} - x (\omega_s) \\
$$

So you've got $v_s$ as the linear velocity, so I can understand the sine/cosine of an angle times the linear velocity to get the components of linear velocity, but what is this $x(\omega_s)$ and $y(\omega_s)$ business? I'm assuming that it's your x/y position, but the x/y position should have nothing to do with your speed.

What you're saying is that, by changing your reference point for zero position, you're changing your speed. If $x = 0$ then $x(\omega_s) = 0$, and if $x = 100$ then $x(\omega_s) = 100*\omega_s$.

It feels like you're trying to combine a linear speed and a tangential speed, but the tangential speed would be an angular speed times a radius of curvature, but there's a lot lacking from that. It seems almost like you're maybe trying to do something like with a Coriolis force, but that would be a force that would impact acceleration, not velocity. And there's no $r$ for radius of curvature or rotating frame distance, etc.

I would think you should omit the $x(\omega_s)$ and $y(\omega_s)$ terms completely, but again I'm not understanding how they're derived or what they mean.

Continuing up:

a = k_2*(v-v_s);               %linear acceleration
alpha = k_2*(omega-omega_s);          %angular acceleration

it looks like you've got acceleration as a linear function of the difference in reference and state speed, but what is $k_2$? The more important problem for me is that you have the same gain $k_2$ being used to drive both your linear and angular accelerations. This might make sense if you were doing some vector operation, but if you did it longhand like that then I'd expect a $k_1$ for the linear acceleration and a $k_2$ for the angular. Since I'm not seeing where $k_2$ (or $k_1$) is defined I can't give any feedback on that.

Now finally I work my way back up to your first set of equations, which have me totally lost. I would expect that your reference would be either a constant setpoint, like v=0, or the result of some profile, like v = MySpeedProfileGenerator(t). What I'm seeing instead is that your linear and angular speeds are again functions of distance, but not the x/y positions; just the radial distance $R$ from origin to the x/y point.

That is, you have:

$$
R = \sqrt{x^2 + y^2} \\
$$

and that's fine, but then why do your targets $v$ and $\theta$ depend on whether or not $R==0$? And, IF $R$ is zero, then:

if R == 0
    v=k_1*((R^2)*sign(cos(gamma)));
    omega = k_1*R*sinc;
else

is just a really fancy way of writing

if R == 0
    v = 0;
    omega = 0;
else

But I'm also not understanding the rest of it - what is $\theta$? What is $\gamma$ supposed to be? Finally, the biggest thing I don't understand here, what is this sinc function you've got, and what is it doing? You have:

sinc = sin(gamma)*sign(cos(gamma));

But the sinc function is

sinc = sin(x)/x;

so maybe you're abbreviating some other name? But why does the sign (not sine!) of cos(gamma) keep coming up?

I would almost guess you're trying to constrain it to run in a circle or something, the way these angles keep coming up, and that you're using an $R$ to drive your speed and... angle? I'm assuming theta is an input, but theta doesn't drive your values directly, gamma does, and that's a function of position.

The crazy thing for me is that I can't tell how or even if your position ever gets back to anything. You have position driving $R$ and driving $\gamma$, which means position also drives everything else, and then you're outputting $\dot{x}$ and $\dot{y}$, but you're never doing anything with x or y.

I thought you had given some drawings previously but I can't find those now; they're not in your list of questions for this site. I think that you should go back to the problem statement for your project and do the following:

Clearly define what you're trying to control - is it x/y position? Radial position? Heading? Is this a SISO system or MIMO?

Clearly define the kinematic relationships at play - this means you need a labeled drawing of your setup.

Pick Cartesian or Polar, and stick with it. You're begging for accidents by switching back and forth mid-block.

:EDIT:

As I was writing the last line of this answer, I had a sudden flash - I think maybe you're trying to convert from Polar to Cartesian in your x_dot and y_dot equations! But you're not doing it correctly. For one, you're using the linear speed instead of the radial speed. For two, you're using the wrong equation (but you're so close!):

You have:

x_dot = -v_s*cos(theta) + y*omega_s;

where the proper polar-to-Cartesian conversion would be:

x_dot = R_dot*cos(theta) - r*omega_s*sin(theta);

I hope all of this leads you to some insight into your problem.


问:
Transform Image Using Roll-Pitch-Yaw Angles (image rectification)
UPDATE: This exact problem has been solved in StackOverflow. Please read this post there for further explanation and a working solution. Thanks!

I am working on an application where I need to rectify an image taken from a mobile camera platform. The platform measures roll, pitch and yaw angles, and I want to make it look like the image is taken from directly above, by some sort of transform from this information.

In other words, I want a perfect square lying flat on the ground, photographed from afar with some camera orientation, to be transformed, so that the square is perfectly symmetrical afterwards.

I have been trying to do this through OpenCV(C++) and Matlab, but I seem to be missing something fundamental about how this is done.

In Matlab, I have tried the following:

```<code>%% Transform perspective
img = imread('my_favourite_image.jpg');
R = R_z(yaw_angle)*R_y(pitch_angle)*R_x(roll_angle);
tform = projective2d(R);
outputImage = imwarp(img,tform);
figure(1), imshow(outputImage);
</code>```

Where R_z/y/x are the standard rotational matrices (implemented with degrees).

For some yaw-rotation, it all works just fine:

```<code>R = R_z(10)*R_y(0)*R_x(0);
</code>```

Which gives the result:

If I try to rotate the image by the same amount about the X- or Y- axes, I get results like this:

```<code>R = R_z(10)*R_y(0)*R_x(10);
</code>```

However, if I rotate by 10 degrees, divided by some huge number, it starts to look OK. But then again, this is a result that has no research value what so ever:

```<code>R = R_z(10)*R_y(0)*R_x(10/1000);
</code>```

Can someone please help me understand why rotating about the X- or Y-axes makes the transformation go wild? Is there any way of solving this without dividing by some random number and other magic tricks? Is this maybe something that can be solved using Euler parameters of some sort? Any help will be highly appreciated!

答: SOLUTION: This exact problem has been solved in StackOverflow. Please read this post there for further explanation and a working solution. Thanks!

ContentLicense: CC BY-SA 3.0


问:
How to add, additional prismatic joint to the end of Universal robot last joint inside URDF file?
Could you please tell me how can I add, additional prismatic joint to the end 6th joint in URDF file of an Universal robot as in the link below:

https://github.com/ros-industrial/universal_robot/blob/kinetic-devel/ur_description/urdf/ur5.urdf.xacro

答:
What do you mean by "end of an URDF file"?

The last two links (wrist_3_link and ee_link) are connected by a fixed joint. Change the joint type to prismatic and then update the joint properties (joint axis and joint limits).

Or do you want to add an additional link to the manipulator i.e. add a new link after ee_link? In that case, define a new link with a unique name, for eg. new_link. Then, define a prismatic joint with parent as ee_link and child as new_link.


问:
Autonomous vessel general design
I have been asking a couple of questions here and around on autonomous boats and finally I decided to post the design and gather some feedback.

The idea is to build a small vessel capable of advanced autonomous navigation (on a given path), path optimisation and obstacle avoidance, being at the same time completely independent from fuel thanks to renewable energy sources, only relying on solar panels and thermoelectric generators.
The displacement of the boat is around 2m, with a bulb at the bottom containing LiFePo batteries.

On the electronics side, I thought to split tasks into several units, made of microcontrollers running a RTOS (ChibiOs or FreeRTOS, I still have to choose) and a Single Board Computer with Arch Linux.
Up to now the units identified are:

High level navigation unit

Low level navigation unit

Power unit

Engine unit

The High level navigation unit would perform the following tasks:

Path planning: global route calculation, considering shorelines, high-traffic routes and sea bathymetry to avoid groundings;

Obstacle Avoidance: using a forward-looking sonar for underwater obstacles, AIS for boats, a thermal imaging camera and maybe a small radar for other types of objects;

Local navigation: optimise the path taking into account the general predefined path (from the path planner), use Inertial Navigation and dead reckoning in case of GPS failure.

Local System Management: put sensors in sleep mode when they're not needed or in case of power shortages.

The Low level navigation unit is responsible for:

Local navigation

Local System Management

Communication: transmit and receive messages via radio link or satellite;

The Power unit contains a MPPT solar charger and a battery monitor and is responsible for:
 - Charging and powering the units;
 - Sensing the battery and informing other units on the charge status;
 - Disabling units to go in low power mode (one after the other).

The last unit is simply an interface to the engines, basically it's an ESC connected to the other units.

The backbone of the boat would be the well-established CAN Bus running NMEA2000 since almost all sensors use this protocol, and both the SBC and the uC would share messages via CanBus.
Locally, the SBC will have a CanBus interface and a MessageBus where threads responsible for the aforementioned tasks will stream messages (inter-thread) and eventually publish frames on the Can Bus to interface with sensors (say put the Sonar in sleep mode) or the other units.
Ideally in case of failure the boat should be able to navigate with just one of the two navigation units, obviously losing some functionalities.

My main concern is to reduce failure points so that a single error or broken sensor/unit would not compromise the whole system. Ideally there could be failsafe additions such as a Draculino, an Arduino running on RF and piezoelectric harvested energy to stream QRSS and morse position messages to nearby boats, but probably it's useless.

Fun fact: this design should be COLREG compliant due to the several anti-collision features and some small details I didn't mention such as the navigation light or the manoeuvre normaliser (to make the boat steer in a human-understandable way).

答: AutoNaut Limited in the UK Chichester have been developing a vessel similar to your ideas, it might be worth getting in touch with them.</p>

Title: Autonomous vessel general design


问:
Cartesian control for $z$ vector of end effector frame to point towards a specific direction
Consider a robotic manipulator with 7-DOF (all joints are revolute) that accepts joint velocities as execution commands. What I am trying to achieve is the following:

I want to calculate the velocities of the joints needed in order for the $z$ vector of the end effector frame to point towards a ball which moves on the $xy$ plane. I am asked only to change the end effectors orientation and not its position. The position and the velocity of the ball, both expressed in the end effector frame are given by a function get_state(). The time step of the simulation is $T = 0.004ms$. So, basically the procedure is like this:

Simulate one step of the ball

Use get_state() function to obtain ball's position and velocity expressed in the end effector frame

Do what it needs to be done to calculate the appropriate joint velocities such that $z$ vector of end effector frame points towards the ball

Repeat

Considering the fact that the $z$ vector of the end effector should point towards the center of the ball, the problem breaks down to the simpler one that the $z$ vector should point towards a point in space. Consider also that the forward kinematics is solved, so the homogeneous transform from the 'world_frame' to the 'end_effector_frame' is known. My thoughts for solving it are:

At the end the $z$ vector of the "end_effector_frame" should be aligned to the unit vector $u$ in the direction from the origin of the end effector frame to the point $p$ which is the position of the ball expressed in the end effector frame and is known.

So, basically I have to come up with a rotation matrix $R$ capable of rotating the $z$ vector onto the $u$ vector. I found a possible way of deriving this rotation matrix here.

After having this rotation matrix $R$, I thought of expressing it as an $[ \text{angle} \ \text{axis} ]$ representation and then calculate $dw = angle \cdot axis$, which is the rotational movement the end effector has to perform in order to achieve the new orientation (maybe add also a proportional controller $K \cdot dw$).

So, because then end effector performs only a rotational movement and not at all any translational, its total desired movement should be: $ D = [0 \ \ 0 \ \ 0 \ \ dw(1) \ \ dw(2) \ \ dw(3)]^T $.

Final step is to compute the joint velocities by using the pseudoinverse of the manipulator's jacobian, like so: $ \dot{q} = J^{-1}(q) \cdot D $.

Basically it is cartesian control of the manipulator. First of all, my basic concern is whether this approach is correct or is it completely wrong and if it is correct, should I use the aforementioned formula (found at the link) in order to compute the proper rotation matrix or is there another way of finding this matrix that is more suitable for robotics ? Besides that would an inverse kinematics approach be more suitable ?

Finally, I am curious about the fact that the velocity of the ball is given and expressed in the end effector frame. The procedure I described above doesn't use this knowledge and I am curious if I am missing something and I should use this velocity in any certain way.

答:
By imposing that the position of the end-effector $\mathbf{x} \in \mathbb{R}^3$ has to remain fixed, you're actually limiting your IK that, as a result, might struggle to achieve the task of aligning the vector $\mathbf{z} \in \mathbb{R}^3$ to point toward the target $\mathbf{p} \in \mathbb{R}^3$ when, for instance, one of the joints $\mathbf{q} \in \mathbb{R}^n$ reaches its bounds. To sum up, we are not required to stick to the same position, but rather to point to the ball.

To this end, you could try to solve a different optimization problem. Aligning the vector $\mathbf{z}$ to point toward $\mathbf{p}$ can be written as:

$$
\max_{\mathbf{q}} \{ \mathbf{z}\left(\mathbf{q}\right) \cdot \left(\mathbf{p}-\mathbf{x}\left(\mathbf{q}\right)\right) \},
$$

where $\mathbf{z}$, $\mathbf{x}$ and $\mathbf{p}$ are all expressed in the world reference frame.

The maximization of the dot product can be easily turned into the following minimization:

$$
\min_{\mathbf{q}} \{ \mathbf{z}\left(\mathbf{q}\right) \cdot \left(\mathbf{x}\left(\mathbf{q}\right)-\mathbf{p}\right) \},
$$

At this point, you can apply standard IK algorithms such as the Jacobian transposed, the pseudoinverse, the damped least-squares, and the like.

Concerning the velocity of the ball $\dot{\mathbf{p}}$, you could recruit the well-known differential law $\dot{\mathbf{p}}=\mathbf{J} \dot{\mathbf{q}}$, invert it and plug into your IK using the null-space policy.

My suggestion is to start off considering that, instead of dealing with $\mathbf{p}$ in the minimization above, you could come up with a better estimate using a look-ahead, which is in its simplest form $\tilde{\mathbf{p}} = \mathbf{p} + \dot{\mathbf{p}} \cdot \Delta t$.

Interestingly, in the past, I implemented something similar for the iCub robot using a nonlinear constrained optimization package called Ipopt. See the relative pull request and the corresponding code.


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

I have so far decided on;

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

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

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

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

Also does my work so far sound sensible ?

Thanks for any help

答:
Use the robot to hold and index the plastic box and use flame to deburr the plastic.
I owned a machine shop and deburred plastic for a living.
No force necessary just the correct flame and speed.
Good luck</p>

Title: Deburring Robot (Plastic Box)


问:
What are the differences between sensing, perception and learning
I see people are using a lot the terms sensing, perception, and learning together in the context of artificial intelligence. It seems to me perception is a more board term that may include sensing and learning. What are their definitions and how do they interact with each other? Any paper references are highly appreciated.</p>

答:
Sense and perceive, in robotics, are synonyms. There is no practical technological step between sensing and perception.
If we take a robot for example, or in intelligent thermostat, the sensor signal is fed to the controller (or agent if we consider reinforcement learning), actions come out of controller (or agent). There is no in-between step practically which would constitute a difference between sensing and perception.

One could argue, that there is a difference between the sensor signal and the robots internal representation of state. If we consider SLAM algorithms or sensor fusion also in this case there is "no extra" step which one could call perception, differentiated from sensing.

Learning is a procedure, which uses data to create models. It is very loosely linked to sensing. Sensing can be used to gather data for learning and sensed data can be passed though a machine learning model to achieve a certain goal (e.g. classifying that signal or identifying an object if the signal is a camera).

The comments to the question are suggesting that perception would be somehow information derived from sensing...That is just signal processing, has been called that for years, the goal of signal processing is not to "create perception" but to process signals and derive other/better information from it.


问:
Sphere-bot CNC that can engrave in a high resolution?
What kind of machine would be best suited to engrave a photograph (grayscale) on a glass sphere?

I have checked out the eggbot, and openbuilds "spherebots"....but neither of these options can do something like a photograph.

Would I need a 5-axis machine? If so what type? I know nothing about them.
What types of cnc machines out there could achieve this?

答:
Laser cutters like this Epilog can engrave grayscale images as well as glass and curved surfaces.  See some sample images below.  But I am not sure how much grayscale you will be able to achieve on glass.  You might need to achieve this effect with halftone.

You will need an added rotary attachment for curved surfaces.  This will limit the size of work piece you can use.  The laser with rotary attachment is really only suited for cylindrical objects, not spheres.  As the sphere surface curves away, the laser will de-focus and loose power and resolution.  Fixing a sphere in the rotary attachment will also be a challenge.

That being said, I have engraved logos on pumpkins in the past with reasonable results.


问:
Controlling Integrated Servo motors with PLC for 5 DOFIndustrial Robotic arm
I am working on developing a 5 DOF robotic arm having 50 kg Payload using PLC, we are willing to source the integrated servo all in one (servo drives, encoder, and servo motor) with no breaks and external breaks have to be attached,
and I have some doubts before sourcing servos that would this Integrated servo motor be convenient to be controlled with PLC accurately? IS there any chance of mispositioning or any inaccuracy in the case of these integrated servos?
Is Controlling External break with the integrated servos reasonable?</p>

答:
Industrial servos with integrated driver and encoder have a limited power compared to non-integrated counterparts. Other then that, their spec sheet specifies their accuracy and if they are good/acceptable quality they will conform the spec sheet. You can extrapolate the positioning accuracy of your robot, based on your mechanism and the positioning (in)accuracy of each joint.

Depending on your country, sales representatives might be willing to work with you on finding the right motor for your application from their offering, but you have to break down the requirements to the motor level, you cannot go to them with robot requirements. It is not uncommon for industrial companies to admit, that they cannot fulfil your specs.

The Kuka iiwa 7DoF robot arm, the Schunk 6 DOF robot arm, the Pilz 6 Dof root arm are built with integrated servos. There is high quality precedent for integrated servos in robots, although payloads are lighter.

Depending on your targeted application(s), programming and motion synchronisation requirements and the chosen PLC, the PLC can be a serious limiting factor (!), although this was not part of your question, as I understand the question focuses on the integrated servos aspect. There is no precedent of a versatile industrial robot using only PLC (and servo drives, regardless of integration).  If the task is highly specific, it might fit, but unlikely. It should be called a "positioning mechanism" (or similar?) not a robot, then...

External brakes are uncommon, but not unheard of. Depending on the (safety) certification of the robot cells your robot is placed in (e.g. CE, UL, ISO13849) "diy" brake integration can be a serious problem (not necessarily, highly application dependent).


问:
Sinusoidal trajectory of each cartesian coordinate for the end effector
Consider a 7-DOF robotic manipulator. I want the end effector to follow a predefined trajectory and suppose I care only about the position and not about the orientation. So, the rotation part of the forward kinematics will always be the identity matrix. Now, here is my question. Is it possible for each of the end effector's position coordinates $ (x,y,z) $ to impose a sinusoidal trajectory of the same frequency ? I mean it like this:

$$ x(t) = A_1\sin(\omega \pi t) $$
$$ y(t) = A_2\sin(\omega \pi t) $$
$$ z(t) = A_3\sin(\omega \pi t) $$
So, at each time instant the desired cartesian position of the end effector is obtained by the above sinusoids. If this is possible, I assume that I could also find the desired trajectory of each one of the $7$ joints by solving the inverse kinematics, right ?

答:
The short answer is yes, it is possible.

The long answer is:

The all xyz coordinates have to be inside workspace boundaries of the robot

Solving IK for 7 DOF is a bit harder then any other non redundant structure, you need an additional equation.

If you are using an industrial robot arm, with its own programming language, you have to find out how to get a "time signal" or create your own by incrementing a variable in a loop.

The frequency of the sinusoids has to be very low, I would start with 0.05Hz and increase gradually to see what happens. Maybe 0.1 if the amplitudes are small.

You should avoid exiting the eigenfrequency of the robot.


问:
Collaboration of mobile robot and survaillance camera - classic localization (still) needed?
Just started with the topic of mobile robotics..
so I'm still into concept making and little programming, but have not setup everything or tested at all. I got a (differential) mobile robot (lego mindstorm) and a external camera mounted on the ceiling of my testarea, looking top down. Robot is equipped with a bumper for collision detection and ultrasonic sensor. Area has some static obstacles (3-4) in it and there should be another lego robot moving around as dynamic obstacle. No markers or colors on the ground.

I plan to do a camera self-calibration taking odometry data and tracking the position of the robot by template matching. Creating the map get's done by image processing, with edge filtering and later converting to a gridmap.
So next for localization:
While I get more and more into all the subjects I ask myself how to do the localization in the best manner and still as collaboraive system. Is it possible or even clever to use a particle filter out of robot sensor data?
Or is my localization given, just by my template matching and image position extracting? The problem I see comes with the second robot, which will get the localization very complex.

Another Question is, how can I achieve an iterative map update? - The map should react on happenings, like the moving robot (dynamic).

I was reading about layering the occupancy/grid maps and update the master map. So combining it with A* Path Planning should make it dynamic I guess?

Thanks and best regards

答:
I am also working on visual tracking using overhead imagery. What I am proposing to do is training a YOLO V3 object detector on an edge device (camera with a wide-angle lens) to first identify and classify the objects within a scene. Once that is done, the objects can then be tracked within the scene using SORT, DeepSORT, or a similar algorithm.

Now regarding navigation, I have experimented with a Genetic Algorithm but for a 2D static environment (will release the code and the graduate report soon). Maybe you could start there, rather than hardcoding the static environment, you may want to train the vision system to build the 2D static map. Once that information is passed into the robot, it may use GA to find the best path and then move across the map. You mentioned a moving obstacle, the vision system can track multiple objects so another idea could be to run the GA multiple times and reoptimize your robot's path as it traverses along a feasible path while avoiding collision with the companion robot. In my humble opinion, it would be best if you fixed the scope of your project first as in literature you would soon see, just this scenario has hundreds of different implementation each with increasing level of difficulty.

On a concluding note, have a look at these papers, I hope they come of some use to you.

Overhead View Person Detection Using YOLO -- https://doi.org/10.1109/UEMCON47517.2019.8992980

Implementation of Path Planning using Genetic Algorithms on
Mobile Robots -- https://ieeexplore.ieee.org/document/1688529

Good luck


问:
Solidworks to URDF for robot arm with complex joint's
I've tried to create a URDF model of an arm robot similar to the following links using Solidworks to URDF plugin. This plugin creates a hierarchy relation between joint and links in any robot mechanism

https://grabcad.com/library/robot-arm-assembly-1

however, I've noticed this robot arm style has a couple of interaction links & joints that is not easy to make simple child-parent tree links.
I've trying to extract the child-parent link relations according to the following image

for example, the child-parent relation  of link 1 is not clear for me, however, I made the following child-parent tree according to that image
I really appreciated any tip that helps me verify my design

答: You're not going to get a parent-child relationship with this because it's not a kinematic tree. You've got a four-bar linkage, which is closed chain linkage. The angle that Link 2 makes with Link 1 depends on the angle at AxisX1 AND the distance from AxisX1 to AxisX3 AND the length of bar1 AND the length of link1 AND the distance from Axis4 to AxisX7.</p>


问:
Do I need an accurate flight model for a UAV?
As I understand it, a Kalman filter uses a mathematical model of the robot to predict the robot's state at t+1. It then combines that prediction with information from sensors to get a better sense of the state.

If the robot is an aeroplane, how accurate/realistic does the model need to be? Can I get away with simple position and velocity, or do I therefore need an accurate flight model with computational fluid dynamics?

答:
Depends on how stable you want the UAV to be. If the air is still, probably position/velocity/acceleration added with friction and drag would be a pretty accurate model and is not too difficult.

However, air is not still, and no matter what model you make, you can't predict wind. Neither it's strength nor direction. The effect of wind especially makes the calculation of drag difficult as it depends on the aerodynamicity of the UAV in the direction of the wind.

That said, unless someone here has created before a similar UAV to the one you are building, and used it in the same application as yours, I don't think you can get a clear answer. Best way would be to try it.

My suggestion is to try building a model that doesn't ignore the basics (e.g. basic fluid dynamics), but don't go out of your way either. If it turns out that it's not good enough, then ask for help on the particular behavior.


问:
Create2: Getting strange data reading sensor data

I am new to Create2 and I hope someone can help me.

I have a problem with reading the sensor data or the data I get.

I have setup a nodeJS application running on a Raspberry PI 3B+. Sending data (e.g: drive commands) is no problem - the robot makes what I want.

At program start I send the optcodes 128 (start) and 132 (full mode). After that I send optcode 148 with the packageIds to start streaming. This works also very fine.
I have defined the sensors I need in an array:
```<code>this.readCommands = [
        {
            packetId: 21, //Charging state
            len: 1,
            name: 'Charging state',
            signed: false,
            value: 0
        },
        {
            packetId: 25, //Battery charge
            len: 2,
            name: 'Battery charge',
            signed: false,
            value: 0
        },
        {
            packetId: 26, //Battery capacity
            len: 2,
            name: 'Battery capacity',
            signed: false,
            value: 0
        },
        {
            packetId: 28, //Cliff left Signal
            len: 2,
            name: 'Cliff left Signal',
            signed: false,
            value: 0
        },
        {
            packetId: 29, //Cliff front left Signal
            len: 2,
            name: 'Cliff font left Signal',
            signed: false,
            value: 0
        },
        {
            packetId: 30, //Cliff front right Signal
            len: 2,
            name: 'Cliff front right Signal',
            signed: false,
            value: 0
        },
        {
            packetId: 31, //Cliff right Signal
            len: 2,
            name: 'Cliff right Signal',
            signed: false,
            value: 0
        },
        {
            packetId: 46, //Light Bump Left Signal
            len: 2,
            name: 'Light Bump Left Signal',
            signed: false,
            value: 0
        },
        {
            packetId: 47, //Light Bump Front Left Signal
            len: 2,
            name: 'Light Bump Front Left Signal',
            signed: false,
            value: 0
        },
        {
            packetId: 48, //Light Bump Center Left Signal
            len: 2,
            name: 'Light Bump Center Left Signal',
            signed: false,
            value: 0
        },
        {
            packetId: 49, //Light Bump Center Right Signal
            len: 2,
            name: 'Light Bump Center Right Signal',
            signed: false,
            value: 0
        },
        {
            packetId: 50, //Light Bump Front Right Signal
            len: 2,
            name: 'Light Bump Front Right Signal',
            signed: false,
            value: 0
        },
        {
            packetId: 51, //Light Bump Right Signal
            len: 2,
            name: 'Light Bump Right Signal',
            signed: false,
            value: 0
        }
];
</code>```

To fill the 148-data:

```<code>const tempArr = [148, this.readCommands.length];
this.readCommands.forEach(c => tempArr.push(c.packetId));
this.writeToPort(Uint8Array.from(tempArr)).subscribe(() => {
});
</code>```

When data arrives I collect the data and when I have a full package I try to interpret the data.
The packets I get seem to be ok (also the checksum is fine)

Here one as UINT8s:

```<code>19, 38, 21, 0, 25, 9, 253, 26, 10, 137, 28, 10, 46, 29, 10, 77, 30, 10, 86, 31, 10, 83, 46, 0, 31, 47, 0, 65, 48, 0, 59, 49, 0, 22, 50, 0, 24, 51, 0, 6, 50
</code>```

But when I take these values I have data, e.g. for cliff-signals, that are much more greater than 4095 as described in the OI spec.
Also I get a battery capacity of 35082 while battery charge is greater than 60000.

Here's my code for reading the packet:

```<code>if (!this._checkData(data))
            return;
let index = 2;
while (index < data.length-1) {
    const id = data.readUInt8(index);
    const cmd = this.readCommands.find(c => c.packetId === id);
    if (!cmd) {
       index++;
       continue;
    }
    switch (cmd.len) {
        case 1:
           cmd.value = cmd.signed? data.readInt8(index +1) : data.readUInt8(index+1);
           index+=2;
           break;
        case 2:
           cmd.value = cmd.signed ? data.readInt16LE(i+1) : data.readUInt16LE(i+1);
           index+=3;
           break;
        }
}
</code>```

What am I doing wrong?

When I reset the robot I get the message:

```<code>Soft reset!
Roomba by iRobot!

stm32 2016-06-13-1127-L

battery-current-zero
 254

b
box vars restored!
languages: english (0)
</code>```

Is the firmware too old?
Can someone help me with that problem?

Daniel

答:
I think you're swapping the bytes in your multi-byte messages. Considering just the battery charge and capacity, you said:

Here one as UINT8s:

```<code>19, 38, 21, 0, 25, 9, 253, 26, 10, 137, 28, 10, 46, 29, 10, 77, 30, 10, 86, 31, 10, 83, 46, 0, 31, 47, 0, 65, 48, 0, 59, 49, 0, 22, 50, 0, 24, 51, 0, 6, 50
</code>```

I get a battery capacity of 35082 while battery charge is greater than 60000

So, looking at battery charge:

```<code>25, 9, 253
</code>```

That's giving you packet ID 25, which is battery charge, and then two bytes, which in hex are:

```<code>9 = 0x09
253 = 0xFD
</code>```

You need to merge the two bytes to get the full value; if you put them together as 0x09FD then you get 2,557. If you swap them, then you get 0xFD09, which is 64,777.

Similarly, with the battery capacity, you have the response

```<code>26, 10, 137
</code>```

which would be the hex values 10 = 0x0A and 137 = 0x89. You can join them as 0x0A89 and get 2,697 or you can swap them and join as 0x890A and get 35,082.

A test like this, where you try interpreting the data both ways, can be an easy way to check if you're doing it correctly - with the bytes swapped, your charge is greater than your capacity, which doesn't make sense. When done correctly, though, it looks like the battery capacity is about 2.7 Ah and the current charge is about 2.6 Ah - looks good!

It took me a while to find which way it should be interpreted in the Open Interface specification (warning: direct download). I didn't see where it said anything about byte order until the "Quick Reference" section at the end of the spec, where it states

Some of the sensor data values are 16 bit values. These values are sent as two bytes, high byte first.

That's telling you that the first byte is the "most significant" byte and the second byte is the "least significant" byte, which is exactly how we're treating the data in the correct interpretation.

Hope this helps!

Body:
I think you're swapping the bytes in your multi-byte messages. Considering just the battery charge and capacity, you said:

Here one as UINT8s:

19, 38, 21, 0, 25, 9, 253, 26, 10, 137, 28, 10, 46, 29, 10, 77, 30, 10, 86, 31, 10, 83, 46, 0, 31, 47, 0, 65, 48, 0, 59, 49, 0, 22, 50, 0, 24, 51, 0, 6, 50

I get a battery capacity of 35082 while battery charge is greater than 60000

So, looking at battery charge:

25, 9, 253

That's giving you packet ID 25, which is battery charge, and then two bytes, which in hex are:

9 = 0x09
253 = 0xFD

You need to merge the two bytes to get the full value; if you put them together as 0x09FD then you get 2,557. If you swap them, then you get 0xFD09, which is 64,777.

Similarly, with the battery capacity, you have the response

26, 10, 137

which would be the hex values 10 = 0x0A and 137 = 0x89. You can join them as 0x0A89 and get 2,697 or you can swap them and join as 0x890A and get 35,082.

A test like this, where you try interpreting the data both ways, can be an easy way to check if you're doing it correctly - with the bytes swapped, your charge is greater than your capacity, which doesn't make sense. When done correctly, though, it looks like the battery capacity is about 2.7 Ah and the current charge is about 2.6 Ah - looks good!

It took me a while to find which way it should be interpreted in the Open Interface specification (warning: direct download). I didn't see where it said anything about byte order until the "Quick Reference" section at the end of the spec, where it states

Some of the sensor data values are 16 bit values. These values are sent as two bytes, high byte first.

That's telling you that the first byte is the "most significant" byte and the second byte is the "least significant" byte, which is exactly how we're treating the data in the correct interpretation.

Hope this helps!


问:
velocities transformations between frames
In my system, I have two frames.
I want to convert my velocities(x,y,z,r,p,y) in frame 1 to be represented in frame 2.
Does multiply the linear component and the angular component with the rotation matrix is correct?</p>

答: Yes it is. The rotation matrix of the coordinates system B relative to the coordinate system A or its inverse backwards, A relative to B.</p>


问:
irobot create 2: random changes in driving speed
I am trying to make the Create2 dance a specific choreography. I´m not using the sensors, I just let it drive and turn for a specific amount of time. In general it works fine.

But after working with it for a while, it suddenly doesn't do a full turn any more. It seems like something changes and than driving in a specific angle and turning in place takes a bit longer or shorter. It also happens, that Roomba doesn't drive straight any more but slightly turns to the right. And than it changes again and it´s back to the first configuration.

I tried changing the baudrate, I did manual resets, I charged Roomba and tried in full battery power and low battery power but I can´t find an explanation. The bug occurs without changing code or anything.
Does anyone have an idea, what the problem might be?

Another problem that I ran into: I let Create2 play songs. Most of the time it works. But from time to time I get some weird issues. The tune doesn't play until the end but lingers on one note for a moment. Than the tune stops. Often the next song won´t play at all. Next time I run the code it works just fine.
Any ideas?

The whole thing makes me go crazy because it messes up the whole choreography and makes it totally unreliable...

My set up:
Arduino Nano connected to serial port of Create2.
Arduino-Create2 Libary https://github.com/brinnLabs/Create2

```<code>// Include Create2 libary
#include <iRobotCreate2.h>

// connect Roomba TX to pin 10 and Roomba RX to pin 11,
// connect Roomba BaudRateChange to pin 5
iRobotCreate2 roomba(true, 10,11,5);

// set pins for LED and Startbutton:
const int ledPin =  13;
const int buttonPin = 4;
int incomingByte = 0;

// variable for reading the Startbutton status
int buttonState = 0;

// full rotation of roomba with VEL 100
int fullspin100 = 7319;
int fullspin200 = 3659;
int fullspin300 = 2439;

void setup(){

  Serial.begin(115200);

  // set pinmode for LED and Startbutton
  pinMode(ledPin, OUTPUT);
  pinMode(buttonPin, INPUT_PULLUP);

  // reset BaudRate to 19200
  roomba.resetBaudRate();
  delay(2000);

  // start communication and safe mode
  roomba.start();
  roomba.safeMode();
  delay(1000);

  // song nr. 1
  Notes songNotes[16];
  songNotes[0].note = 31;
  songNotes[1].note = 43;
  songNotes[2].note = 31;
  songNotes[3].note = 43;
  songNotes[4].note = 55;
  songNotes[5].note = 67;
  songNotes[6].note = 79;
  songNotes[7].note = 91;
  songNotes[8].note = 103;
  songNotes[9].note = 91;
  songNotes[10].note = 79;
  songNotes[11].note = 67;
  songNotes[12].note = 55;
  songNotes[13].note = 43;
  songNotes[14].note = 31;
  songNotes[15].note = 103;

  songNotes[0].duration = 8;
  songNotes[1].duration = 8;
  songNotes[2].duration = 8;
  songNotes[3].duration = 8;
  songNotes[4].duration = 8;
  songNotes[5].duration = 8;
  songNotes[6].duration = 8;
  songNotes[7].duration = 8;
  songNotes[8].duration = 8;
  songNotes[9].duration = 8;
  songNotes[10].duration = 8;
  songNotes[11].duration = 8;
  songNotes[12].duration = 8;
  songNotes[13].duration = 8;
  songNotes[14].duration = 8;
  songNotes[15].duration = 8;

  roomba.createSong(1, 16, songNotes);

  // turn on PowerLed red to indicate that roomba is ready for dancing
  roomba.setPowerLEDs(255, 255);
  delay(10);
}

void loop(){
  // listen to input from the button
  buttonState = digitalRead(buttonPin);

  // if button is pressed, start dancing
  if (buttonState == LOW) {
    digitalWrite(ledPin, LOW);
    dance();

  }
  if (buttonState == HIGH) {
   digitalWrite(ledPin, HIGH);
  }
}

void dance(){

  roomba.turnCW(100, 0);
  delay(fullspin100);
  roomba.drive(0, 0);
  delay(1000);

  roomba.turnCCW(100, 0);
  delay(fullspin100);
  roomba.drive(0, 0);
  delay(1000);

  roomba.turnCW(200, 0);
  delay(fullspin200);
  roomba.drive(0, 0);
  delay(1000);

  roomba.turnCCW(200, 0);
  delay(fullspin200);
  roomba.drive(0, 0);
  delay(1000);

  roomba.turnCW(300, 0);
  delay(fullspin300);
  roomba.drive(0, 0);
  delay(1000);

  roomba.turnCCW(300, 0);
  delay(fullspin300);
  roomba.drive(0, 0);
  delay(1000);

  roomba.playSong(1);
  delay(1300);

}
</code>```

答:
I think I found the problem that was causing the errors.
I was sending serial commands to the create 2 via Arduino. To make things easier I was using the create2 libary that summarizes commands (https://github.com/brinnLabs/Create2).

It looks like the freezing of the sound results from some bugs in that libary (maybe the calculation of the songlength). And also the changes in driving speed and the problem of not driving straight seem to disappear when i send all the commands directly (up to now they didn`t appear again).

With the create2 libary I used serial communication. Now I´m using soft serial. Not sure if that was also part of the problem.

I really liked the create2 libary, because it seemd to be the best libary available for create2 that allows easy control for all functions. But be sure to check that piece of your setup when problems occure.


问:
given current position and Quaternion and target local rotations (α, β, γ, in radians), how to calculate the new Quaternion
Say we have an object (e.g., end-effector of a robot) that is defined in a general coordinate system by a Cartesian position and Quaternion.

since the Quaternion is with respect to a general and fixed coordinate system, it is rather not intuitive for me how to calculate this Quaternion if the object is rotated locally.

That is, assuming a local coordinate system on this object, while keeping the position of it also fixed, I want to calculate a new Quaternion vector if the object is rotated by small angles (α, β, γ, or pitch, yaw, and roll) with respect to this local frame/coordinate-system.

答:
It is often useful to create a 2D example and use rigid-body transforms (i.e., $T\in\text{SE}(2)$, where ${\small T=\begin{bmatrix}R&t\\0&1\end{bmatrix}}$, $R\in\text{SO}(2),t\in\mathbb{R}^2$). In this way, the position and rotation are compactly encoded in a single element.

When working through these types of problems, it is important to clearly define notation and coordinate frames. For example, here we will write $T^a_b$ to describe the coordinate frame $\mathcal{F}^b$ with respect to $\mathcal{F}^a$. In other words, $T^a_b$ transforms data in $b$ and expresses it in $a$ (e.g., see more here).

In the following diagram, I have drawn the world frame axis $\mathcal{F}^W$ and an object that has a position and orientation with respect to that world frame (i.e., $T^W_b$). This object has its own coordinate frame $\mathcal{F}^b$. Now, a further transformation $T^b_{b'}$ occurs, where it just so happens that there was no translation. This final coordinate body position and orientation can be expressed in terms of the world frame by composition as

$$
T^W_{b'} = T^W_b T^b_{b'}.
$$

Now, to answer your question more concretely, we can consider just the unit quaternion encoding the orientation of $\mathcal{F}^{b'}$ w.r.t $\mathcal{F}^W$, that is, for $q\in S^3$,

$$
q^W_{b'} = q^W_b q^b_{b'}.
$$

In your setup, it sounds like you have $q^W_b$ and some Euler angles describing the rotation $q^b_{b'}$. Given everything above, your question seems to become "how do I convert Euler angles to a quaternion," for which many answers and software packages exist (Wikipedia). Please take care to know if your Euler angles are expressed as intrinsic or extrinsic and the correct rotation order. I recommend using something like MATLAB's eul2quat or, in Python, transformations.py (embedded in ROS as tf.transformations) to verify.


问:
Is it possible to use ROS2 real-time on a computer and connect to the robot actuator using a CAN bus?
I am using ROS2 real-time (still a lot to learn) to implement the various control element of my robot : SLAM, path planning, controller, etc...

For the first step of prototyping we would like to build a system with only the actuators : A PC will run ROS2 with the controller, an external power supply for the power. Our idea is to have the computer (through ROS2) send the torque control command and receive the sensor info from our actuators.

Our actuators have built in drivers with a CAN Bus interface :
What are our options to connect the computer running ROS2 to the drivers of the actuators ?

Since our controller must run real-time, I imagine the bandwidth of the Computer/Actuator interface is a critical factor.

答:
First, you should define what real-time requirements do you have, not necessarily from a CAN-Bus, but from the application perspective. What is your cycle time, what latency can you accept, what jitter is acceptable by your application.

If your requirements can be fulfilled by a PC with a CAN-Open extension, then you can use a ROS CanOpen package (like this) to act as a CAN-Open master and "bridge" the information on ROS and the CAM-Open network. If not, and you need faster cycle time in your control system, then what ROS on a PC with CAN extension can provide, then you can look at running parts of ROS system on a single board computer or to using a different solution (e.g. decoupling your controller loop from ROS).

I am not sure what current CAN-Open extensions are supported, but USB extensions compatible with the PCAN inerface (e.g. PEAK CAN USB converters) are definitely supported. USB obviously will add more latency then a PCI CAN card would.

If you are planning to send torque command form ROS, it is probably not the right approach. ROS should at best be included in the position loop, not in the torque loop.

.....


问:
How can I integrate a smart phone with my robotics project?
Smart phones these days typically come with a gyroscope, accelerometer, compass, camera, and GPS sensor all on board. They also usually have a connection to the internet with Wifi and mobile data networks. I've seen many cases of using a phone as a remote control for a robot, but to me, it seems like the phone itself is a perfect lightweight computing and sensing platform for an autonomous robot.

The main obstacle I see is interfacing with actuators. Being able to control motors to steer even a table-top robot, or control servos, for example. Connecting and communicating to a microcontroller could be an obstacle as well.

As a robot hobbyist, I'd like to know how I can overcome these and other obstacles to be able to harness the power of my smart phone with my robotics projects.

答:
The main problem as I see it isn't really interfacing with actuators-- if you have a good way to wirelessly transfer digital (even better would be analog, or a channel good enough to do PWM) signals, you can just interface that with a motor driver chip (e.g. L293D) and be good to go. For servos, you need good response time so that you can handle the pulses. If you have a PWM-capable channel, this shouldn't be hard as the channel has a high baud already. If you have a slow channel, servos may not be possible since you require accurate pulse timing.

The first option I see (though it's rather bulky) is to take a Raspberry Pi, connect it via ethernet to a wireless router, and connect the phone to the same wirelessly. Now just write a basic app for your phone that sends requests  to the Pi based on  input, and write another server like app for the Pi that handles these. Might be bulky and somewhat hard, but it's quite extensible, IMO. You can also put the router offboard and stick this into the Pi (or use hotspot and eliminate the router entirely).

Another option is to use a bluetooth controller like the one shown here. I've not used this before, but it looks like you'll still need a microcontroller to handle this. I guess you can connect its RX/TX pins directly to the TX/RX on an arduino, though I'm not too sure of this.

If you've used XBee/ZigBee before, you may want to try out the bluetooth bee.

A final option is to use the phone USB port with a conventional wireless control setup (XBee/whatever)--but this will require one to write drivers and all.


问:
What are the parameters that I need to consider when figuring out the horizontal viewing distance of a camera mounted overhead for effective tracking?
I am currently working on developing a hybrid Indoor Positioning system where images from an overhead camera will be used to identify and track people.

I am new to the field of visual tracking and a challenge that I have recently faced was choosing a suitable camera/lens combination.

My question is what are the parameters that would help me choose the right camera with the right field of view?

To clarify, I installed a raspberry pi camera module v2 on the ceiling and recorded its video output wirelessly. It seems the field of view was too small for looking at the scene effectively. My conclusion is there must be some techniques/equations which can guide me in choosing the horizontal view distance of the camera in relation to its height from the ground floor.

答:
A field of view of a camera is generally measured in degrees. If you connect the imaginary focus point of the lens with two sides of the camera sensor, in case of rectilinear lenses, you get your field of view. Obviously you can do that in the horizontal and in the vertical direction, you get practically two different fields of view. Usually the wider one is considered the field of view, and the other one is linked to the previous one by the aspect ration of the sensor (e.g. 16:9).

So the physical size of the imager and the focus length are what influence the field of view.

Den pending on the distance resolution might be important. Given a flat surface you are filming, perpendicular to the camera's optical axis , given the field of view, the physical size of the sensor and its resolution, using simple geometry, it can ba calculates how many pixels of the sensor will be used to register 1mm on the surface. This will be important to see if e.g. a QR code can be read by the camera or not.

For fisheye lenses, the calculations are not this simple, but are analogue. This page describes how fisheye lenses are modelled. For a Field of View, similar as in the rectilinear case, the angle between the outermost rays hitting the sensor have to be calculated.


问:
Motorised steering without shock absorber?
I was looking up on the Curiosity Mars Rover and noticed that the steering appears to be driven by a (geared) stepper:

I presume the drive contains a worm gear to reduce power consumption of the stepper motor. However, because there's no shock absorber visible, would the gears risk being damaged by shocks?

答:
My bet is on a PMSM not a stepper, but that cannot be determined from an image. A non-back-driveable mechanism is advantageous, and can reduce the steady state power consumption of a system. It does not necessarily have to be a worm-gear.

Regarding shock-absorbers..well a few things have to be considered:

Shock absorbers are normally tuned to filter out specific frequencies, which are correlated with the terrain they are designed for and their intended speed. The intended speed of the mars rover is 1cm/s, according to Nasa. (Maybe it is a different rover..but I expect no 100x change from rover to rover). Common shock absorbers (spring damper system in cars) are for a very different use case. One could argue that not much vibration can be excited at those speeds, so not much shock to absorb...

The wheel spikes have an interesting design. They probably act as a very simple shock absorber (probably more a spring, then a damper..). As they the closes to the wheel, the unsprung mass is only the wheel itself. It is always good to have minimum unsprung mass.

The rover overall has a rocker-bogie type suspension system. This might include some damping effect, but that is not clear from the pictures.


问:
How can I solve this block diagram problem?
I practiced various examples of block algebra and I came across this example where I have to reduce the block diagram. The problem now is that I have there successively set branching points and sum points. How can I get rid of them?

Here is how I came to this block diagram:

Starting point and first step:

Second step:

Third step:

Final block diagram:

Don't know what to do now... I thought to merge (G1G2) with (G5/G2 +1) and (G3G4) but again there isn't so much improvement.

答:
I'm not going to write in the answers, but if I redraw your stuff just a little:

One of the things that might make it easier is to duplicate some of your boxes. Consider doubling up the H2*G2*G3 section:

If I redraw THAT just a little then it should show you how to clear up the middle section:

I'll just call whatever the middle resolves to "A" so I can keep going without needing to do the work:

The last tricky business is that summing junction in the middle of that feedback line. What I'd like to is to push the summing junction to the left so it can be with the second summing junction (from the start). Then you can split each branch of the summation into its own feedback line. To do that, first get that H1 out of the way:

Now distribute the negative from the second summing junction and reorganize:

And then finally split the group of summing junctions up into their own lines, which makes the feedback branches clearer.

Now you just do like you'd do, resolve the feedback loops from inner to outer and you should have the reduction.


问:
Place for GPS antenna on autonomous vehicle
I used to think that the higher GPS antenna position the better until I read the following on GPSd FAQ:

One common error is to place the GPS or antenna as high as possible.
This will increase multipath effects due to signal bounce from the
ground or water, which can cause the GPS to mistake its position and
the time signal. The correct location for a boat GPS antenna is on the
gunwale rail or pushpit rail, close to the water and as far from the
mast as possible (to reduce signal bounce from the mast). If you're
outside or in a fixed location, put the GPS antenna as far from
buildings as possible, and on the ground.

If you're in a car, don't
put the GPS antenna on the roof, put it on the towbar or some similar
location. If you're driving in a heavily built up area, you're going
to get signal bounce off buildings and reduced accuracy. That's just
how the physics works. Note, however, that as your velocity goes up it
becomes easier for the convergence filters in your GPS to spot and
discard delayed signal, so multipath effects are proportionally less
important in fast-moving vehicles.

Does anyone has experience placing GPS antenna on a towbar of the car as suggested? Does it give reasonable effect?

My concern is that placing antenna there will not reduce an error that much, but will expose the device (antenna) to possible mechanical damage.

So, are there any better positions apart from roof and towbar?

Thanks

答: Inaccuracy due to multipath bounces is negligible compared to inaccuracy due to seeing fewer satellites, as Matthew Gordon answers.  So on a land vehicle, roof is best.  Towbar is very poor.</p>


问:
orientation transformation
I have a 6-dimensional vector(x,y,z,roll,pitch,yaw) in frame 1, I want to convert it to frame 2 in order to represent the value in frame 2.
I know that p2=R12*p1 where p1 is the (xyz) in frame 1.
The question is how I convert the roll, pitch yaw to the other frame?</p>

答:
Your 6d vector can also be considered a frame. Lets call it the frame of the object. Convert your 6d vector to something easier to work with like Homogeneous Transforms. Another alternative is Quaternion + Translation, but homogeneous matrices are easier.

Homogeneous Transform:

A 4x4 matrix that looks like:

$T=\begin{bmatrix}
R & t \\
0_3 & 1
\end{bmatrix}$

Here $R$ is a 3x3 rotation matrix. $t$ is your translation vector (x,y,z). $0_3$ is just a row vector of all 0s.

To get the rotation matrix you have to convert your euler angles to the rotation matrix form. You can google that or check out the equation on the wikipedia page.

Your frames 1,2 should also be stored in the homogeneous matrix form. We now can just multiply the various transforms together to get the end result. If you get confused it sometimes helps to draw it out.

By convention $T_{a,b}$ means the Transform of frame $b$ in frame $a$.

I have labeled the pieces of information we have. For instance we have the Transform of the object in frame 1( $T_{1,o}$ ). We want to find the question mark $?$. To do that just trace the path of the arrows. If 2 separate paths start on the same frame and end on the same frame then it means they are equivalent. So for this example the $?$ transform will be the same as the path using $T_{1,2}$ and $T_{1,o}$. If we ever end up going against the arrow direction then it means we need to inverse that Transform.

All that being said your final equation is:

$? = T_{2,o} = T_{1,2}^{-1}*T_{1,o} $

If you want your 6d vector form again then I would just convert the rotation matrix back to euler angles.

Title: orientation transformation


问:
Hector_navigaton on ROS Melodic error
I'm a newbie on robotics, and currently, I'm working on a project to make an autonomous robot that can mapping the surrounding environment and decide the shortest route towards the goal while avoiding the obstacles along the way. The sensor that I used for this project is RPLidar A1M8. I'm working using Ubuntu Mate 18.04 on my Raspberry Pi, but right now I'm testing it using my computer.

I use the hector_slam for mapping, and everything works fine. After that, I try to clone the hector_navigation from https://github.com/tu-darmstadt-ros-pkg/hector_navigation on my catkin workspace, but there's the problem began. When I try to catkin_make, the system found an error in the CMakeLists file. The error I'm getting is below

```<code>-- +++ processing catkin package: 'hector_gps_calibration' -- ==> add_subdirectory(hector_navigation/hector_gps_calibration) -- Using these message generators: gencpp;geneus;genlisp;gennodejs;genpy CMake Error at /home/foriansyah/catkin_ws/devel/share/ceres_catkin/cmake/ceres_catkinConfig.cmake:173 (message): Project 'hector_gps_calibration' tried to find library 'ceres'. The library is neither a target nor built/installed properly. Did you compile project 'ceres_catkin'? Did you find_package() it before the subdirectory containing its code is included? Call Stack (most recent call first): /opt/ros/melodic/share/catkin/cmake/catkinConfig.cmake:76 (find_package) hector_navigation/hector_gps_calibration/CMakeLists.txt:10 (find_package)

-- Configuring incomplete, errors occurred! See also "/home/foriansyah/catkin_ws/build/CMakeFiles/CMakeOutput.log". See also "/home/foriansyah/catkin_ws/build/CMakeFiles/CMakeError.log". Makefile:4632: recipe for target 'cmake_check_build_system' failed make: * [cmake_check_build_system] Error 1 Invoking "make cmake_check_build_system" failed
</code>```

What should I do about this? Any references that maybe I can use for this problem?
Thank in advance.

答:
Actually, the HECTOR_NAVIGATION package depends on other packages and one of them is ceres_catkin (https://github.com/tu-darmstadt-ros-pkg/ceres_catkin) which you need to clone and build in your catkin workspace. However, this ceres_catkin further depends on catkin_simple (https://github.com/catkin/catkin_simple), glog_catkin (https://github.com/ethz-asl/glog_catkin), gflags(https://github.com/gflags/gflags/blob/master/INSTALL.md).

After you clone all these in your workspace, I would recommend to run,
>>sudo apt-get install python-catkin-tools python-rosdep

and then from your catkin workspace
>> rosdep install -r -y --from-paths src --ignore-src
that will install all the missing dependencies of your catkin workspace packages.

Once you are done with the above, then try to run catkin_make.

Title: Hector_navigaton on ROS Melodic error


问:
How to make two frames relative to each other
I have a Velodyne and camera that both have transformations specified relative to the origin of the robot.

The original transformations were specified as yaw, roll, pitch, x, y, z. I wrote a script to convert these to extrinsic matrices (my end goal is to have an extrinsic matrix describing the camera's position relative to the Velodyne).

Figuring out the relative transformation of the camera to the Velodyne is easy for translation - I just calculate the offsets. However, I'm not sure how to approach making the camera rotation relative to the Velodyne, since I believe the translation will also need to be taken into account.

Here is the original ROS transform:

```<code>    <node pkg="tf" type="static_transform_publisher"
        respawn="true"
        name="camera_static_transform_publisher"
        args="0.5080 0.0 0.1778 0 0.05 0 base_link camera 100"
    />

    <node pkg="tf" type="static_transform_publisher"
        respawn="true"
        name="velodyne_static_transform_publisher"
        args="0.4445 0.0 0.09525 0.0 0.06981 0.0 base_link velodyne 100"
    />
</code>```

Here are the extrinsic matrices I calculated:

```<code>    Camera extrinsic relative to base_link:
    [[ 0.99875026  0.          0.04997917  0.508     ]
    [ 0.          1.          0.          0.        ]
    [-0.04997917  0.          0.99875026  0.1778    ]]

    Velodyne extrinsic  relative to base_link:
    [[ 0.99756427  0.          0.06975331  0.4445    ]
    [ 0.          1.          0.          0.        ]
    [-0.06975331  0.          0.99756427  0.09525   ]]
</code>```

Research:

I found this post that mentioned needing to do the inverse of one transformation * the other transformation, but not sure if that works.

I also found this post that also seems to invert the first transformation and multiply it by the second. I will try this out.

Edit: Here is my attempt

Here is my attempt based on the second post I linked. I'm not sure this is correct because the translation of camera_to_velodyne doesn't match what you would get if you just calculated the offsets between the camera and velodyne.

```<code>def get_homogeneous_transformation(rotation, translation):
    """
    Parameters:
    - rotation: a 3x3 rotation np array
    - translation: a 3x1 translation np array
    """
    homogeneous = np.zeros((4, 4))
    homogeneous[-1][-1] = 1
    homogeneous[:3, :3] = rotation
    homogeneous[:3, -1:] = translation
    return homogeneous

def get_relative_transformation(a_to_base, b_to_base):
    """
    Finds the relative transformation from a to b given transformations for both
    relative to base_link.

    See: https://stackoverflow.com/a/55169091/6942666 for more details

    Parameters:
    - a_to_base: 4x4 np.array representing homoegenous transformation of a to base_link
    - b_to_base: 4x4 np.array representing homoegenous transformation of b to base_link
    """

    base_to_a = np.linalg.inv(a_to_base)
    return base_to_a @ b_to_base

if __name__ == "__main__":
    """
    <node pkg="tf" type="static_transform_publisher"
        respawn="true"
        name="camera_static_transform_publisher"
        args="0.5080 0.0 0.1778 0 0.05 0 base_link camera 100"
    />

    <node pkg="tf" type="static_transform_publisher"
        respawn="true"
        name="velodyne_static_transform_publisher"
        args="0.4445 0.0 0.09525 0.0 0.06981 0.0 base_link velodyne 100"
    />

    Camera static transform:
    [[ 0.99875026  0.          0.04997917  0.508     ]
    [ 0.          1.          0.          0.        ]
    [-0.04997917  0.          0.99875026  0.1778    ]
    [ 0.          0.          0.          1.        ]]

    Velodyne static transform:
    [[ 0.99756427  0.          0.06975331  0.4445    ]
    [ 0.          1.          0.          0.        ]
    [-0.06975331  0.          0.99756427  0.09525   ]
    [ 0.          0.          0.          1.        ]]

    Camera relative to Velodyne
    [[ 0.99980379  0.          0.0198087  -0.05929486]
    [ 0.          1.          0.          0.        ]
    [-0.0198087   0.          0.99980379 -0.08562051]
    [ 0.          0.          0.          1.        ]]
    """

    print("Camera static transform: ")
    camera_to_base = static_transform_to_extrinsic([0.5080, 0.0, 0.1778, 0, 0.05, 0])
    print(camera_to_base)

    print("Velodyne static transform: ")
    velodyne_to_base = static_transform_to_extrinsic([0.4445, 0.0, 0.09525, 0.0, 0.06981, 0.0])
    print(velodyne_to_base)

    print("Camera relative to Velodyne")
    camera_to_velodyne = get_relative_transformation(
        camera_to_base, velodyne_to_base)
    print(camera_to_velodyne)

<span class="math-container">```
```

答:
So it seems like what you want to perform is camera-lidar extrinsic calibration. Seems like you have the translations figured out but not the rotation?

You can use one of the already available packages that do this for you. Like Autoware's calibration tool autoware_camera_lidar_calibration. This tool will let you select points in an image and corresponding points in the point-cloud that will be used to calculate a transform between the two and a new frame can be published


问:
Worm gearbox for steering
I'm planning on making a smallish off-road rover (15-25kg net weight) with 4-wheel steering and I was wondering if it'd be a good idea to use (self-locking) worm gearboxes for steering. I've thought about using planetary gearboxes, but self-locking gearboxes will help improve power consumption.

I've found this gearbox but it's not clear whether it's applicable to steering tasks. Here's the spec:

I have several questions and it'd be great if anyone could help:

As the 1:18 variant has a axial force of 250N, does that mean if the load is ~25kg, I'd need to have additional bearings and support for distributing the load?

The duty cycle is 20% at 5min. Is this generally adequate for steering? When steering in a straight line on off-road at low speed, the adjustment may be small but frequent.

A gearbox like that costs around $180. Are there more economical alternatives that are self-locking?

Any advice appreciated. TIA.

答:
Axial (or thrust) force refers to the force applied along the axis of rotation, not the load that is applied to the motor. (This load would be a torque, anyways... not a force).

Probably fine for a tractor, not fine for a F1 racecar. This is a question you need to answer based on your application.

There might be. You can adapt drills with anti-backdrive pins to accomplish this (they are a planetary gearset). Example: https://www.chiefdelphi.com/uploads/short-url/k05tcmRrq5EsCc5xvPtZvbKd5DT.pdf

Other things to think about:

A typical vehicle suspension system has caster and kingpin angles such that the tires automatically re-center. This not only produces a desirable user experience (resistance in an even direction), but it prevents wheels from shaking at high speeds. You'll probably want to consider having this self-centering behavior which will produce higher load on your actuator.

Linkages matter. What sort of linkages are you connecting the steering to the gearbox via?

Title: Worm gearbox for steering


问:
When not to use pseudoinverse?
I know this equation: $\tau = J^T f$. In my case, the Jacobian $J$ is not square. Is it valid to use the pseudoinverse of $J^T$ to calculate end effector forces $f$ based on joint torques $\tau$? I have just never seen this equation used with given joint torques, this is why I am curious.

答:
Short answer: you can use it.

Long answer: Depend on your case, but I have seen many papers where they use the pseudoinverse of the non-square Jacobian matrix.

The rectangular Jacobian matrix opens the window to optimize in terms of other variables such as space constraints or force minimization.

The pseudo inverse by definition (in a redundant manipulator) minimizes the joints velocities.

Source

Title: When not to use pseudoinverse?


问:
Reliably enabling/disabling a PID loop via FSM in a microcontroller
I have a PID loop for a small vehicle implemented in a microcontroller. Currently the PID loop is implemented in a timer interrupt. I want to implement an FSM that will have states that enable or disable the function that the PID loop is controlling, so in some states the PID loop should be disabled and in some it's enabled. I want to implement this control in a way that minimally disturbs the PID loop.

I've been told that I shouldn't disable the timer interrupt if possible. Is a good option for this to implement the FSM itself inside of the timer interrupt and then update the PID loop inside the FSM states?

The alternative would be to have just the PID loop inside the timer interrupt and then the FSM implemented inside the microcontroller's main(), which I think would need to temporarily enable or disable the timer loop while updating state.

答:
You could probably just have the FSM outside the interrupt routine since it is not a time critical function of your system as a PID is.

Then in the FSM can update an enable or disable variable that will enable or disable to use the PID computed value (rather than not compute)

A pseudo-code would look like

```<code>void time_interrupt()
    compute_PID()
    if USE_IT
        use_PID_computed_value()
    else
        not_use_PID_computed_value

void main()
    for( , ,)
        read_control_FSM();
        switch FSM:
            case A:
                 USE_IT = false
            case B:
                 USE_IT = true

</code>```

Body:
You could probably just have the FSM outside the interrupt routine since it is not a time critical function of your system as a PID is.

Then in the FSM can update an enable or disable variable that will enable or disable to use the PID computed value (rather than not compute)

A pseudo-code would look like

void time_interrupt()
    compute_PID()
    if USE_IT
        use_PID_computed_value()
    else
        not_use_PID_computed_value

void main()
    for( , ,)
        read_control_FSM();
        switch FSM:
            case A:
                 USE_IT = false
            case B:
                 USE_IT = true


问:
Robots and RTOS
Many computing platforms used by robots are not real time(ex Jetson).

How are real time constraints handled in real robots? Are all software transfered into a hardware solution with real time performance? How does it work?

答:
The answer is yes, for example many robot computers are based on the linux real-time kernels to handle the real-time constraints.

For small robots for teaching like the turtlebot uses a raspberry pi for the high-level control, and a motor control board for the real-time control.

Another example is the Ball balancing robot that uses an STM32 Nucleo board with the free RTOS kernel to handle the real-time control and communicates to the onboard computer for autonomous navigation.

Title: Robots and RTOS


问:
What is the most accurate method to calculate Torque of 5 DOF Robotic arm
I am working for development of a 5 DOF Robotic arm , and I am confused in the Torque calculation
This is a 5 Axis revolute Joints Robotic arm , where Motor for 1st axis will rotate the turning table and all the robotic arm and 2nd and third joints have a rotation axis out and into the page(such that against gravity) , 4th axis will rotate the last joint  and 5th axis will make payload up and down, Free body diagram is attached
I am calculating Torque of each joints considering the gravity using this formula on all 4 axis,

Torque(2nd axis) = (MLink2*(L2/2) + Mjoint2L2 +MLink3((L3/2) +L2) +Mj3*(L3+L2) +ML4*((L4/2) +L3+L2) + Mj4*(L4+L3+L2) +ML5*(L5/2+L4+L3+L2) + Payloas*(L5+L4+L3+L2)) *9.81

I am using this formula for all axis except 1st

and I am using
Torque= (moment of Inertia)*(Angular acceleration)
Torque=(1/2(Mass of all joints and link )*R^2)*Angular acceleration

on 1st axis,considering 1st axis will not have effect of gravity, it give very small value and when I calculate Torque of 1st joint using above same formula formula considering gravity and all Links, I get largest torque value, but I think 1st axis motor will only be resposible for rotating the turning table(with all joints) but 2nd axis will be against gravity it should have large value than 1st axis?
where I am being wrong in these calculations

and I want to know also about 4th and 5 axis calculation, should I use the same formula which I am using or some thing else should be considered

答:
I do not think your equation is correct.
You should use an established method for writing the equations, like the Recursive Newton Euler Method.</p>


问:
Calculate body speed in m/s with weight in the equation?
UPDATE: I have clarified my question for answerers to better understand what I am asking

If I have a flywheel, like the one shown in this image, and I know the torque, weight distribution, and weight. How do I know the drag of the flywheel, and resulting speed of this flywheel in full motion?

Just a heads up, I don't really know anything about physics.

答:
Torque is moment of inertia times angular acceleration. Theoretically, if you could provide a constant torque and there were no losses, you could accelerate to an infinite speed even with a small torque.

In reality, though, losses exist. Frictional losses are based solely on load weight, like static and dynamic friction in a bearing. There's also viscous damping, which is proportional to speed.

Given "enough time," your speed will equalize because the damping losses will exactly equal the applied torque. If you know how much torque you're applying and what the steady state speed is, then you can calculate the losses.

If you know what the damping constant is then you can calculate the ultimate speed for a given torque.

If you don't have either then there's nothing really you can do, though. You can try to get numbers from other systems but little differences in fiction could make the difference between 10,000 and 30,000 rpm, for example.


问:
360 degree ultrasonic beacon sensor
Basically, I want to detect an ultrasonic beacon in a radius around the robot.
The beacon would have a separate ultrasonic emitter while the robot would have the spinning receiver.

Are there any existing ultrasonic sensors that would meet this use case or am I stuck hacking one together myself?

Is ultrasonic even the best choice? I was hoping that the beacon would be kept in a pocket, so I figured optical sensors were out.

Edit: The beacon and robot will both be mobile so fixed base stations are not an option.

答:
I'm not going to mark this as the answer unless others agree it is the answer, but I found an omnidriectional ultrasonic transmitter/receiver: http://www.metrolog.net/ultra40k.php?lang=en

I haven't used it or tested it yet, but it seems promising.

Title: 360 degree ultrasonic beacon sensor


问:
Is it better to use Jacobians to compute IK when a closed form for IK is available?
I hope that my question is not too stupid ... but I have a hard time to understand why Jacobians computed from forward kinematics would allow a better and smoother control than using pure inverse kinematics equations when they exist.

I understand that Jacobian may map linearly cartesian coordinates to joints coordinates but why we could'nt get the same result whith small increments of inverse kinematics ?

With the example of a 3 joints robot having to reach the pose (x , y, z) from the current pose (u, v, w), if this new pose requires to increase its joints state from (q0, q1, q2) to (q0 + 10, q1 -20, q2 + 40) and if I order to my robot these joint goals computed form inverse kinematic equations with velocities (v0, v1, v2) proportional to the ordered angular motion (v0, v1 = v0 x 2, v2 = v0 x 4), why this would not provide the same result than those with jacobian control ?

Thanks for helping my understanding.

Ugo,

Many thanks for your detailed explanation. I understand from your reply that Jacobian is used for a large set of tasks I probably don't know. I realized that my questiion is not accurate enough. Practically, I am working on a demonstrator to get a proof of concept on microsurgery. I have designed a 4 DOF slave for which I have found FK and IK closed form equations. This slave is controlled with a custom master control for which I have also the FK equations. I applied a scaling factor from master to slave in order to convert large master motion to smaller slave motion. The scalling is applied on cartesian space. Since this is made within a loop, the motion increments are small.  Each joints velocity is, at each step, ordered to be proportionnal to each joint course to get a synchronized movement. I get a decent result but not as smooth as I expected (hardware is presumed optimally tuned). It is why I consider using only velocity control with numerical jacobians, but I don't understand why it could be better than using small steps of IK with proprtionnal velocities...

Ugo,

Thank you for interest. The slave robot is essentially made of 3 revolute joints which rotation axis are respectively q0: around z (= gravity axis), q1 and q2 both turn around x and q3 rotates around y axis. At the output of motor q3 we have an universal joint linking to a bar which end is constrained in a second universal joint itself linked to the ground.
The master control have similar structure. Right hand rule.

Here are the FK equations:
( Where p4 is the position of the center of rotation of the universal joint linked
to q3 output and p2p4 the constant distance beetween p4 and the center of rotation of motor q1)

```<code> 1- first we compute position of p4:
 p4x = qx - q1 * s0 - q2 * s0 * c1 + p2p4 * (s0 * s1 + s0 * c1)
 p4y = qy + q2 * s1 + p2p4 * (c1 * s1)
 p4z = qz + q1 * c0 + q2 * c0 * c1 - p2p4 * ( s1 * c0 +* c0 * c1)

2- Compute translation and orientation of link p4 to distal pivot (which is the tool)
pE is the part of the link outside the distal pivot (pivotX, pivotY, pivotZ).
(note that pivotZ = 0 by construction):
Ia = sqrt((pivotX - p4x)^2 + (pivotY - p4y)^2 + p4z ^2 )
Ib = IL - Ia //(Translation outside the distal pivot point)
Ibr = Ib / Ia
pEx = pivotX + (pivotX - p4x) * Ibr //(vector projection)
pEy = pivotY + (pivotY - p4y) * Ibr
pEz = -p4z * Ibr
fk.tr = Ib
## tool orientation:
alpha = atan((pEyy - p4y) / (pEz - p4z))
beta = atan((pEx - p4x) / sqrt((pEz - p4z)^2 + (pEy - p4y)^2))
//(gamma, the tool rotation is directly given by q3).
</code>```

A loop read at 650 Hz the pose of the 4 DOF master control, compute the FK and if a change occurs, apply a scaling factor to the cartesian change of mastercontrol and send a new pose to the slave by adding to its IK order the cartesian change to the starting pose of the slave. Velocity orders are sent with values proportionnal to the change in joints values. Basically the slave is position controlled, velocties are used only to synchronize joints motions.
This works, but the global motion is not as smooth as got when motors are individually controlled. And here (microsurgery), smoothness is very important.

答:
Saying in general terms that with the Jacobian we can obtain smoother control and thus better movements is an overstatement if it is not referred to a specific context where one is able to analyze all the implications in depth.

In fact, we can do great things by knowing the exact IK anyway.

From a broader perspective, the Jacobian is a linear map relating the velocities of the joints with the velocity of the robot's end-effector, hence it is intimately connected to the control problem where we are asked to generate suitable velocity commands (we are dealing here with the kinematics and not with the dynamics) to achieve a goal.

With this in mind, your following definition of the Jacobian is not quite right:

I understand that Jacobian may map linearly cartesian coordinates to joints coordinates but why we could'nt get the same result whith small increments of inverse kinematics ?

That said, we need to consider other essential aspects that are more difficult to be implemented, if not impossible, with a closed-form IK:

Generally, the movements we aim to perform through a controller are not only specified in the joint space but rather also in the task space. For example, we need to move linearly with the tip while staying away from the joint bounds. To this end, we ought to use the Jacobian to trade off these requirements.

Nowadays, robots tend to be equipped with lots of degrees of freedom so that closed-form solutions to IK do not even exist.


问:
TV Umpire replaced by Robot (machine)
https://en.wikipedia.org/wiki/Third_umpire

Is it possible using Robotics, Artificial intelligence, Machine learning technologies to replace the TV umpire - Human by a Robot (machine)?

i.e. The Robot will take the decision whether the batsman is out or not out after watching the replays.

答: No robotics, machine learning or AI required. Tennis relies heavily on HawkEye technology for a while now, static high speed cameras and analytical dynamic models. Hawkeye also makes progress in cricket and other sports.</p>

Title: TV Umpire replaced by Robot (machine)