问:
Solving pseudo inverse of Jacobian in Matlab/Octave
I'm attempting to create a simulation of a 3 (rotational) DOF robotic arm in 3D space. So far I've had success getting to the Jacobian. What seems to be the issue is that the pseudo inverse of the Jacobian produces illogical values the second time it's calculated. I've reviewed the code and the Jacobian seems to make sense each time it's calculated, and after the second iteration of the pseudo inverse the values make sense so I'm unable to identify the specific cause.

Code provided below.

```<code>clc;
clear;
clearvars;

function H = Hg(th,a,r,d) % Creates homogenous transform
  H = [cosd(th) -sind(th)*cosd(a) sind(th)*sind(a) r*cosd(th);
      sind(th) cosd(th)*cosd(a) -cosd(th)*sind(a) r*sind(th);
      0 sind(a) cosd(a) d;
      0 0 0 1];
end

function drawLeg(t, xp, yp, zp, Body, CoxaS, FemurS, TibiaS, EndEfS)
  CoxaV = [CoxaS(1), FemurS(1); CoxaS(2), FemurS(2); CoxaS(3), FemurS(3)];
  FemurV = [FemurS(1), TibiaS(1); FemurS(2), TibiaS(2); FemurS(3), TibiaS(3)];
  TibiaV = [TibiaS(1), EndEfS(1); TibiaS(2), EndEfS(2); TibiaS(3), EndEfS(3)];

  plot3(xp, yp, zp, 'b.') %draw path
  hold on
  plot3(CoxaV(1,:), CoxaV(2,:), CoxaV(3,:), 'm', 'LineWidth', 3) %draw coxa
  plot3(FemurV(1,:), FemurV(2,:), FemurV(3,:), 'g', 'LineWidth', 3) %draw femur
  plot3(TibiaV(1,:), TibiaV(2,:), TibiaV(3,:), 'r', 'LineWidth', 3) %draw tibia
  axis([Body(1)-5 Body(1)+5 Body(2)-5 Body(2)+7 Body(3)-5 Body(3)+5])
  axis equal
  %body centered graph to make sure it stays in view while walking
  grid on
  hold off
  pause(t)
end

function [Th1, Th2, Th3] = stepLeg (t, Th1, Th2, Th3, x, y, z)
  ### DESIGN ROBOT ###
  % 3DOF articulting arm
  % Start with only z offset for center of robot.
  x00 = 0;
  y00 = 0;
  z00 = .5;

  % Link lengths
  a0 = 2; %distance from body center to leg 1, UPDATE FROM CAD
  a1 = .5; %length of coxa in cm aka base (joint 0) to joint 1 in z direction
  a2 = 1; %length of femur
  a3 = 1; %length of tibia

  ## DH Parameters ##
  Th0 = 30; %UPDATE FROM CAD
  A0 = 0;
  A1 = 90;
  A2 = 0;
  A3 = 0;
  r0 = a0;
  r1 = 0;
  r2 = a2;
  r3 = a3;
  d0 = 0;
  d1 = a1;
  d2 = 0;
  d3 = 0;

  ## Build vectors for each segment ##
  % Each line has an extra element to help the matrix math
  Body = [x00; y00; z00; 1];
  Coxa = [0; a1; 0; 1];
  Femur = [a2; 0; 0; 1];
  Tibia = [a3; 0; 0; 1];

  ## Generate all homogenous transforms ##
  Hb0 = Hg(Th0, A0, r0, d0);
  H01 = Hg(Th1, A1, r1, d1);
  H12 = Hg(Th2, A2, r2, d2);
  H23 = Hg(Th3, A3, r3, d3);
  Hb1 = Hb0*H01;
  Hb2 = Hb0*H01*H12;
  Hb3 = Hb0*H01*H12*H23;

  ## Calculate starting locations for each segment ##
  CoxaS = Hb0*Body;
  FemurS = Hb0*H01*Coxa;
  TibiaS = Hb0*H01*H12*Femur;
  EndEfS = Hb0*H01*H12*H23*Tibia;

  ## Draw the leg ##
  drawLeg(t, x, y, z, Body, CoxaS, FemurS, TibiaS, EndEfS)

  ### Now to give a target for the leg ###
  ## Generate Jacobian ##
  # Rotation matrices #
  Rbb = [1, 0, 0; 0, 1, 0; 0, 0, 1];
  Rb0 = Hb0(1:3, 1:3); %use first three rows and first three columns
  Rb1 = Hb1(1:3, 1:3);
  Rb2 = Hb2(1:3, 1:3);

  # Displacement vectors #
  dbb = 0;
  db0 = Hb0(1:3, 4); %use first three rows of fourth column
  db1 = Hb1(1:3, 4);
  db2 = Hb2(1:3, 4);
  db3 = Hb3(1:3, 4);

  # Joint rotation velocity matrices #
  rbb = Rbb*[0;0;1];
  rb0 = Rb0*[0;0;1];
  rb1 = Rb1*[0;0;1];
  rb2 = Rb2*[0;0;1];

  # Create the culumns representing partial diffferentials for each joint #
  %zeroth = [cross(rbb, db3-dbb); rbb] % not needed since it's a fixed joint
  first = [cross(rb0, db3-db0); rb0];
  second = [cross(rb1, db3-db1); rb1];
  third = [cross(rb2, db3-db2); rb2];

  # Concatinate into the jacobian #
  fullJ = [first, second, third];
  % only need the cartesian portion
  J = fullJ(1:3, 1:3)
  Jinv = pinv(J) %Print for troubleshooting

  ## Calculate the velocity of the end effector ##
  EndEfS %Print for troubleshooting
  TargetX = x(1) %Print for troubleshooting
  TargetY = y(1) %Print for troubleshooting
  TargetZ = z(1) %Print for troubleshooting
  Err =[x(1); y(1); z(1); 0] - EndEfS %Print for troubleshooting
  fullV = ([x(1); y(1); z(1); 0] - EndEfS)*t;% Calculate velocity of EE
  V = fullV(1:3); % Removing the last element since it's trash info

  # Calc new joint angles #
  w = pinv(J)*V; % Calc joint velocities
  delTh = w.'/t % Calc change in joint angles
  Th0 = Th0; %this should never change?
  Th1 = Th1 + delTh(1);
  Th2 = Th2 + delTh(2);
  Th3 = Th3 + delTh(3);
end

### TIME STEP ###
t = .001

### BUILD PATH ###
% test to determine how piecewise funcitons would work to create path.
% This square looks really nice for now
for i = 1:96
  if i <= 24
    x(i) = i/6;
    y(i) = 3;
    z(i) = 1;
  elseif 24 < i  && i <= 48
    x(i) = x(i-1);
    z(i) = z(i-1)+1/6;
    y(i) = 3;
  elseif 48 < i && i <= 72
    x(i) = x(i-1) - 1/6;
    z(i) = z(i-1);
    y(i) = 3;
  elseif 72 < i && i <= 96
    x(i) = x(i-1);
    z(i) = z(i-1) - 1/6;
    y(i) = 3;
  end
end

### Set Up Initial Angles ###
% Starting angles
Th1 = 60; %UPDATE FROM CAD
Th2 = 0;
Th3 = 0;

### Time Loop ###
for i = 1:10
  [Th1, Th2, Th3] = stepLeg(t, Th1, Th2, Th3, x, y, z)
end
</code>```

答:
After running your code, it looks like everything is behaving how you have defined it to. The problem you are running into has more to do with the Jacobian itself and less to do with any mistakes you have made.

Consider what the Jacobian is:
$$
J(\mathbf{q})=\left[\begin{array}{ccc}
\dfrac{\partial \mathbf{f}(\mathbf{q})}{\partial q_{1}} & \cdots & \dfrac{\partial \mathbf{f}(\mathbf{q})}{\partial q_{n}}
\end{array}\right]=\left[\begin{array}{ccc}
\dfrac{\partial f_{1}(\mathbf{q})}{\partial q_{1}} & \cdots & \dfrac{\partial f_{1}(\mathbf{q})}{\partial q_{n}} \\
\vdots & \ddots & \vdots \\
\dfrac{\partial f_{m}(\mathbf{q})}{\partial q_{1}} & \cdots & \dfrac{\partial f_{m}(\mathbf{q})}{\partial q_{n}}
\end{array}\right]
$$

where $\mathbf{q} \in \mathbb{R}^{n}$ is your current joint configuration and $f_{i}(\mathbf{q})$ is the forward kinematics map of your $i^{th}$ workspace degree of freedom. So, conceptually this is a mapping from joint velocity to end-effector velocity, given the current joint configuration. Now, consider what happens when two joint axes are parallel and there respective vectors to the end-effector are colinear. We have something like:
$$J(\mathbf{q}) = \left[\begin{array}{ccc} \mathbf{j}_1 \cdots \mathbf{j}_i \cdots \alpha \mathbf{j}_i \cdots \mathbf{j}_n \end{array} \right]$$
where $\mathbf{j}_i \in \mathbb{R}^{m\times1}$ and $\alpha \ne 0$. Now, let's try to derive some joint angles that would move our end-effector in a desired way. To do so, we need an inverse of our map $\dot{\mathbf{x}} = J(\mathbf{q})\dot{\mathbf{q}}$. In the case that $n = m$, the above case should be a good enough example to demonstrate that taking the inverse of $J(\mathbf{q})$ is not a good idea. Thus, we can take the pseudo-inverse instead - which conveniently generalizes to the case where $n \ne m$. Before we define the pseudo-inverse $J^{+}(\mathbf{q})$, let's take a look at the singular value decomposition of $J(\mathbf{q})$. The SVD theorem states that any matrix (we'll use the Jacobian here) may be decomposed into the following form:
$$J(\mathbf{q}) = UDV^{T}$$
where $U \in \mathbb{R}^{m \times m}$ and $V^{T} \in \mathbb{R}^{n \times n}$ are orthonormal matrices, and $D \in \mathbb{R}^{m \times n}$ has non-zero entries only along its diagonal. Intuitively we can think of this equation as  decoupling our configuration space from our workspace with some scaling factors that connect the two ($d_{i,i}$ are the only entries in $D$ that may not be zero). Now, going back to the case of two parallel joint axes, we make the intuitive magic-wand-waving jump to the fact that at least one $d_{i,i} = 0$. We can rationalize this with the fact that the end-effector velocity with one of these joints moving will be a scalar multiple of the velocity caused by the other joint moving.

Thus, two dimensions in our configuration space essentially reduce to one in the workspace. This phenomenon is known as a singularity - or a singular configuration. They cause plenty of headache and should be handled with care. Now, let's define the pseudo-inverse of our Jacobian and see what happens around singular configurations.

Without getting into any derivations, the pseudo-inverse of $J(\mathbf{q})$ can be defined as:

$$J^{+}(\mathbf{q}) := \sum_{i}^{r}\frac{1}{d_{i,i}}\mathbf{v}_{i}\mathbf{u}_{i}^{T} $$
where $r$ is the rank of $J(\mathbf{q})$ or, equivalently, the number of non-zero entries in $D$. So, given a singular configuration, we should not see much out of the ordinary. However, consider the configurations in a close proximity to the singular configuration. Remember, $U$ and $V^{T}$ are orthonormal matrices. Thus, as the Jacobian has two almost (but not) linearly dependent column vectors, $U$ and $V^{T}$ will not represent that we are no longer in a singular configuration. Instead, one of the $d_{i,i}$ entries will. This $d_{i,i}$ will be very small as it now represents that we have some newly realized direction we can move in our workspace, even if it is not by much (i.e. the linearly independent Jacobian column vectors return our ability to move in two directions, even if they are almost exactly the same).

Using what is surely abusive notation, we note that at least one $d_{i,i}$ now satisfies the condition that $d_{i,i} \approx 0, d_{i,i} \ne 0$. Thus, when we return to our pseudo-inverse definition, we see why your problems are arising with the pseudo-inverse of the Jacobian: $\frac{1}{d_{i,i}}$ grows arbitrarily large as we approach singular configurations. Of course, this term is never actually infinity as this singular value would not be considered in the psuedo-inverse calculations if it is zero - but an argument could be made that $\lim\limits_{d_{i,i} \to 0} \frac{1}{d_{i,i}} = \infty$ (assuming you subscribe to the limit's school of math). Thus, we find ourselves with an ill-conditioned way of "inverting" our Jacobian.

Luckily, there exists a nice, relatively simple solution to this problem. I won't get into the math on why this works, but the general idea is to convert the end-effector motion problem into a damped least-squares optimization problem. Using the above derivation of the pseudo-inverse, we get the following formula to find a sufficient joint velocity for a requested end-effector velocity:
$$\dot{\mathbf{q}} = J^{+}(\mathbf{q})\dot{\mathbf{x}}$$
So, as we approach singular configurations, we will see some $\dot{q}_i \to \infty$. To alleviate this issue, we introduce the damped least-squares solution by defining some $\lambda$ such that $0 \lt \lambda \lt 1$. Now we solve for $\dot{\mathbf{q}}$ as follows:

$$\dot{\mathbf{q}} = J^{T}[JJ^{T} + \lambda^{2}I]^{-1}\dot{\mathbf{x}}$$
where $J = J(\mathbf{q})$ and $I \in \mathbb{R}^{m \times m}$ is the $m \times m$ identity matrix. Scaling lambda will allow you to choose how large joint velocities may be around singular configurations. The basic idea of the adjustment here is to keep our "inversion" technique well-conditioned at all configurations by "damping" the small singular values such that some $\dot{q_i} \to 0$, instead of $\infty$, as we approach singular configurations. Just as an aside, the above formula is optimal for the case where $n \ge m$.

Here are some good sources for the curious:

https://cs.uwaterloo.ca/research/tr/2000/19/CS-2000-19.pdf

https://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm

https://en.wikipedia.org/wiki/Singular_value_decomposition

Joel Burdick's "Kinematic analysis and design of redundant robot manipulators"

NOTE: There are more than just this one presented "type" of singular that can occur with revolute joint manipulators. The other cases can be both more restrictive (i.e. consider $\alpha = 0$ in the original case given) and less restrictive. Their effects on the Jacobian vary, the given case was just a sufficient case to demonstrate what was necessary.


问:
How to avoid gimbal with Quaternions
I'am working with an LSM6DSO32, so I'am starting with Kalman filter, everything works but with Euler angle I got gimbal lock I think... But if my pitch angle approach to 90° my roll angle jump and I get a bad value and my pitch is like blocked around 90°.

So I searched and I foud this phenomenon "Gimbal lock" after some other researches I have read I should use Quaternions to avoid this Gimbal lock. I found this code that implement MadgwickAHRS algo who use Quaternions here

But if I use computeAngles, this function return Euler angles I have again Gimbal problem. So my question is, how can I only use Quaternions without Euler transformation to compute my roll and pitch angles? My objective is to know if my roll or my pitch angles has move to 90 or -90° aboyt my start position. For example, if my start position is roll 20° and pitch 10°, I would like to detect if my roll is 110° or -70° and if my pitch is 100° or -80°

How can I solve my problem with only q0, q1, q2, q3 values ?

I found this article here I've tried the solution for gimbal lock, but when I set my roll to 0 and compute yaw with 2atan2(q1, q0) for pitch equals PI/2 the value I got isn't good, yaw move a lot...

答:
Quaternions are a more efficient way of storing the orientation matrix of a frame.

I use the vector-scalar convention for quaternions (3+1 = 4 quantities) and have defined the following utility functions

Quaternion definition $$q=\begin{pmatrix}\vec{v}\\
s
\end{pmatrix}$$

Quatenion from rotation axis $\hat{z}$ and angle  $\theta$ $$q=\begin{pmatrix}\hat{z}\sin\left(\frac{\theta}{2}\right)\\
\cos\left(\frac{\theta}{2}\right)
\end{pmatrix}$$

Identity quaternion $$q=\begin{pmatrix}\vec{0}\\
1
\end{pmatrix}$$

Magnitude $$ \|q \| = \sqrt{ \vec{v} \cdot \vec{v}  + s^2 }$$

Unit quaternion (to represent a rotation) $$ {q} = \frac{1}{\sqrt{ \vec{v} \cdot \vec{v}  + s^2 }} \begin{pmatrix} \vec{v}\\
s \end{pmatrix}$$

Inverse quaternion $$q^{-1} = \frac{1}{\vec{v} \cdot \vec{v} + s^2}\begin{pmatrix}-\vec{v}\\
s
\end{pmatrix}$$

Sequence of two rotations $$q_1 q_2 = \begin{pmatrix}s_{1}\vec{v}_{2}+s_{2}\vec{v}_{1}+\vec{v}_{1}\times\vec{v}_{2}\\
s_{1}s_{2}-\vec{v}_{1}\cdot\vec{v}_{2}
\end{pmatrix}$$

Transform vector $\vec{p}$ by quatertion rotation $q$ $$ \vec{p}' = \vec{p}+2s\left(\vec{v}\times\vec{p}\right)+2\left(\vec{v}\times\left(\vec{v}\times\vec{p}\right)\right)$$

Inverse Transform vector $\vec{p}'$ by quatertion rotation $q$ $$ \vec{p} = \vec{p}' - 2s\left(\vec{v}\times\vec{p}'\right)+2\left(\vec{v}\times\left(\vec{v}\times\vec{p}'\right)\right)$$

Rodrigues 3×3 rotation matrix from quatenion $$\mathbf{R}=\mathbf{1}+2s[\vec{v}\times]+2[\vec{v}\times][\vec{v}\times]$$ where $[\vec{v}\times] = \begin{vmatrix}0 & -v_z & v_y \\ v_z & 0 & -v_x \\ -v_y & v_x & 0 \end{vmatrix} $ is the skew symmetric cross product operator matrix.

Inverse Rodrigues 3×3 rotation matrix from quatenion $$\mathbf{R}^\top=\mathbf{1}-2s[\vec{v}\times]+2[\vec{v}\times][\vec{v}\times]$$

Quaternion from 3×3 rotation matrix $\mathbf{R}$ $$\begin{aligned}s & =\frac{1}{2}\sqrt{\frac{\left(\mathbf{R}_{32}-\mathbf{R}_{23}\right)^{2}+\left(\mathbf{R}_{13}-\mathbf{R}_{31}\right)^{2}+\left(\mathbf{R}_{21}-\mathbf{R}_{12}\right)^{2}}{3-\mathbf{R}_{11}-\mathbf{R}_{22}-\mathbf{R}_{33}}}\\
\vec{v} & =\frac{1}{4s}\begin{pmatrix}\mathbf{R}_{32}-\mathbf{R}_{23}\\
\mathbf{R}_{13}-\mathbf{R}_{31}\\
\mathbf{R}_{21}-\mathbf{R}_{12}
\end{pmatrix}
\end{aligned}$$

Time derivative of quaternion subject to rotational velocity vector $\vec{\omega}$ $$ \dot{q} = \frac{1}{2}\begin{pmatrix}s\vec{\omega}+\vec{\omega}\times\vec{v}\\
-\vec{\omega} \cdot \vec{v}
\end{pmatrix}$$ Remeber $\vec{v}$ is the vector part of the quaternion and not a velocity vector.

Rotation velocity vector from quaternion and its time derivative $$\begin{pmatrix}\vec{\omega}\\
0
\end{pmatrix} = 2\begin{pmatrix}s\dot{\vec{v}}-\vec{v}\dot{s}+\vec{v}\times\dot{\vec{v}}\\
s\dot{s}+\vec{v} \cdot \dot{\vec{v}}
\end{pmatrix}$$

All of the above functions can be used in the simulation and modeling of rigid bodies by keeping track of each body's orientation $q$ in the configuration space, and doing a time integration with $q \rightarrow q + \dot{q} \Delta t$, just like you would integrate position $\vec{r} \rightarrow \vec{r} + \dot{\vec{r}} \Delta t$

No Euler angles needed for the model. If you need to extract Euler angles, then convert the quaternion into a rotation matrix, and extract from there.

ContentLicense: CC BY-SA 4.0

Title: How to avoid gimbal with Quaternions


问:
How to solve this basic dynamics/inverse kinematics problem
Suppose I have a 6-DOF robot arm with revolute joints whose tooltip is at some position $\bf{r_0}$ and I want to move it through some trajectory ${\bf r}(t)$.

For simplicity, assume there are just 2 links of length $l_1$ and $l_2$, with orthogonal joints at the ends of each link.

Link masses are $w_1$ and $w_2$, uniformly distributed

The initial joint angles of interest are

$${\bf \theta_0} = \left[\theta_{10}, \theta_{20}, ... \theta_{40}\right]$$

The maximum available torque from each actuator of interest is

$${\bf T_{max}} = \left[{T_{1}}_{max}, {T_{2}}_{max}, ... {T_{4}}_{max}\right]$$

The maximum available speed from each actuator of interest is

$${\bf \omega_{max}} = \left[{\omega_{1}}_{max}, {\omega_{2}}_{max}, ... {\omega_{4}}_{max}\right]$$

The actuators weights are

$${\bf w_a} = \left[w_{a1}, w_{a2}, ... w_{a6}\right]$$

Only the lower 4 actuators are "of interest" because we are interested in the location of the center of mass of the final orthogonal actuators.

How does one go about solving the constrained optimization problem of finding the "optimal" tensor:

$${\bf \theta^*}(t) = \left[{\bf\theta_{1}^*}(t), {\bf\theta_{2}^*}(t), ... {\bf\theta_{5}^*}(t)\right]$$

where we assume that $t$ is discretized appropriately (i.e. not undersampled).

Let's assume optimality in this case means the solution that gets the tooltip through ${\bf r}(t)$ as quickly as possible given the constraints.

答:
This seems to be a simplified version of the classic serial manipulator minimum-time-control (MTC) problem.

In general the solution will always have at Least one actuator saturated.

In general there is not a closed form solution.  A solution method typically uses the Hamiltonian and energy based Lagrange multiplier that uses a shooting method to iteratively converge to the optimal solution.

Here is a good proof for the solution:
https://ieeexplore.ieee.org/abstract/document/56659

Here is a good practical example for a 2 DOF manipulator
https://ieeexplore.ieee.org/abstract/document/12226


问:
what are the main steps to build a swarm robots system and train it to achieve foraging task using deep Q network
I studied reinforcement learning deeper and prepared myself to use Webots, and when I decided to build a swarm robots system and drive it by deep_Q_networks I feel too confused
how can I begin and is there any tutorial that can help that includes DQN,swarm,and Webots</p>

答:
Let's assume this foraging task is a continuous motion planning problem, i.e. you have dynamics accounted for (does not necessarily have to be the case, but should help convergence if it is) and your available actions are some movements relative to each robots current position. I would first advise ditching DQN. Learning how DQN's work is an important first step in reinforcement learning, but a more modern solution like SAC does not add much computational complexity while providing vast improvements (also it uses a continuous action space which tends to be a win in robotics). So, the next question is how do I use SAC? Pretty easy here, go to a repo like this https://github.com/denisyarats/pytorch_sac and clone it. You do not need to be an expert at all on the inner workings of these RL algorithms right away. Give yourself some time using them and build up an intuitive understanding of what is going on. It takes time to learn them as PyTorch and Tensorflow are complex libraries that do a lot under the hood.

Now, you have your RL code ready to go, how do you actually apply it to your problem? This is where the actual engineering/math comes in. First, you'll probably want to set up a simulation environment with all of your robots. You'll want a function that returns the state of all of your robots in a long vector, $\mathbf{s} \in \mathbb{R}^{e\cdot |s_i|\times 1}$ where $|s_i|$ represents the number of states of an individual robot and $e$ represents how many robots you have. You will also need an adequate reward function. This is the most important part of any RL algorithm. Your reward here would obviously be based on foraging performance, but I would imagine the swarm aspect of the problem means you are looking to maximize coverage over some space? Making this assumption, you could have a reward that is simply $r(\mathbf{s}) = f(\mathbf{s}) + A(\mathbf{s})$, where $f(\mathbf{s})$ denotes the foraging performance of the swarm and $A(\mathbf{s})$ represents the area they are covering. Keep in mind you may also want to include some statistics about foraging performance in your state then.

Now, you have a reward function and a state function - the final step is to make a step function. This will take you from your current state to another state given some action sampled from your network. So, we will need an action vector $\mathbf{a} \in \mathbb{R}^{e \cdot |a_i| \times 1}$, where $|a_i|$ represents the number of available actions per robot. So, the step function will look something like $(\mathbf{s}_{t+1}, r_{t}, d) = step(\mathbf{s}_t, \mathbf{a}_t)$, where $d \in \{0, 1\}$ indicates the termination status of your current run (not necessarily the whole simulation, just one of many runs needed to collect training data) and $t$ represents the current time step.

For convenience sake, most RL implementations are set up to be interfaced with OpenAI Gym environments. Thus, if you make the above functions, you can simply "inject" your own environment into many pre-existing RL implementation and get going in a relatively smooth manner. The Gym interface typically only requires a $step$ and $reset$ (just resets environment state to some default) function as well as perhaps some other values that should be apparent by any error messages ;) In my personal experience, the actual coding of the environment is much more tedious (albeit rewarding) than interfacing the environment with a prefab RL implementation.

Various sources to help in your journey:

Reward function crafting:

https://stats.stackexchange.com/questions/189067/how-to-make-a-reward-function-in-reinforcement-learning

https://www.mathworks.com/videos/reinforcement-learning-part-2-understanding-the-environment-and-rewards-1551976590603.html

https://people.eecs.berkeley.edu/~pabbeel/cs287-fa09/readings/NgHaradaRussell-shaping-ICML1999.pdf (this one is a little dense)

RL Code and Theory:

https://github.com/sweetice/Deep-reinforcement-learning-with-pytorch

https://github.com/aikorea/awesome-rl

https://github.com/nicklashansen/tdmpc (relatively "cutting-edge" here)


问:
PointCloud2 parse to xyz array in ROS2
I'm trying to find a solution of converting PointCloud2 message data into xyz array for further analysis. Though, in my case I'm interested only in 2d application.

The information is either scarce or confusing.

Each point data in the PointCloud2 is stored as a binary blob. For example: First 1-4 numbers represent x as a 32 bit float, 5-8 represents y, 9-12 represents z, 13-16 represents the intensity.

So, my question is, is there any ROS2 library to make the conversion? Alternatively, any example of implementing it in C++.

For both cases, please provide a full code snippet or a link to an example

答: Look in the sensor_msgs_py package. It contains support functions for working with some of the more complex sensor messages, including point clouds. For your specific case, you could use the read_points() or read_points_numpy() or even the read_points_list() function to get the data out as a numpy array, which you can then analyse in the way you want.

Title: PointCloud2 parse to xyz array in ROS2


问:
Why do we parametrize matrices?
I'm learning about rotation matrices in class and one of the topics mentioned to parametrize a matrix.
What do we need to do this?

For further reference: https://www.youtube.com/watch?v=Y8gB7nvtmrU

答:
I can't really think of a good example of when to parameterize a system of equations involving rotation matrices, but one general thing we can gain from parameterization regardless is a clear indication of the subspace that solves a linear systems of equations. For example, the video you linked shows that the solution to:
$$
  \left[ {\begin{array}{cccccc}
    1 & -4 & -2 & 0 & 3 & -5\\
    0 & 0 & 1 & 0 & 0 & -1\\
    0 & 0 & 0 & 0 & 1 & -4\\
    0 & 0 & 0 & 0 & 0 & 0\\
  \end{array} } \right] \mathbf{x} = 0,\  \mathbf{x} \in \mathbb{R}^{6\times 1}$$

is the linear subspace spanned by:

$$
  \left[ {\begin{array}{ccc}
    4 & 0 & -5  \\
    1 & 0 & 0  \\
    0 & 0 & 1 \\
    0 & 1 & 0 \\
 0 & 0 & 4 \\
 0 & 0 & 1 \\
  \end{array} } \right]   \left[ {\begin{array}{c}
    v_1 \\
    v_2  \\
    v_3  \\
  \end{array} } \right] \subset \mathbb{R}^{6}
$$

where $\mathbf{v} \in \mathbb{R}^{3}$. As far as actual applications go, I suppose you could look at solutions to different robotics equations (forward/inverse kinematics) as constrained subspaces of your original space. My best guess as to why parameterization has been brought up alongside rotation matrices it to find solutions that lie in a certain subspace once a rotation has been applied. However, given the orthonormal nature of rotation matrices, you would be needing to solve for a solution subspace only given that the constraints you are trying to satisfy already lie in some linear subspace (that might be confusing, just think about how a point only has one vector "pre-rotation" that could now satisfy that point, while a line would have a line "pre-rotation" as well). If anything you would be more likely to parameterize a Jacobian or generally any other matrix that can have free variables.

To talk about a parameterizable matrix in robotics, consider a prismatic robot with two joints that both move in the z-axis only. Since you are just now learning about rotation matrices, I won't define this robot's Jacobian or anything like that - so just trust me that:

$$ \mathbf{q}_{} = \left[ {\begin{array}{c}
    1 \\
    -1  \\
  \end{array} } \right] x \subset \mathbb{R}^{2}$$

is the linear subspace of joint configurations where the robot's end-effector is in some consistent configuration - having $\mathbf{q}$ represents the robot's configuration and $x \in \mathbb{R}$. As far as revolute joint robot's go, you will only be able to use this technique at instantaneous configurations. I personally wouldn't lose much sleep over the concept, it's a cool way to think about solutions that lie in a given subspace - but techniques like singular value decomposition accomplish the same ideas in a much more relatable-to-robotics kind of way.

Title: Why do we parametrize matrices?


问:
What kind of motors do industrial robot have?
What kind of motors do industrial robots use and how do you control them?</p>

答:
Most industrial robot have servo motors. This allows you to control velocity, torque and position of motors - the latter usually in some form of closed-loop control system.

I work for Stäubli. Our early robots were actually hydraulic, the UniMate. Next were the PUMA robots which had DC servo motors. The modern RX, TX, TX2, TS, and TS2 arms have 3-phase AC servo motors.


问:
Is there a G-Code command to step motors - Implementing custom backlash compensation
I've designed my own cnc plotter machine. I've programmed by own "paint" program that generates G-Code which is sent from Universal G-Code Sender (UGS) to an Arduino running GRBL.

As far as I'm aware GRBL does not support backlash compensation, therefore I think I will have to add it to my paint program.

Is there a G-Code command that can be used to step the motors without the position of being changed? So when my code is going to change the direction of one of the steppers I want to be able to add in a few extra steps to take up the backlash without UGS updating the position that it thinks the head is at.

I think I may be able to do this with the G92 offset command but it seems like there should be an easier way...

Any help would be greatly appreciated.

答:
I don't know anything about G-Code, but would refer you to the concept of Separation of Concerns - Should a paint program really be doing mechanical backlash compensation? More regarding your question - how would the G-code know that position won't change if the motors are stepped?

I know this isn't really an answer, but it kind of is?

Don't prematurely optimize. Backlash isn't an issue until you can prove it's an issue.

If backlash is really an issue then the easiest solution would be to fix it where it originates - mechanically. Get a better gearbox.

Finally, if replacing the gearbox isn't an issue, then it might be possible to change your toolpath to only approach from one direction. Instead of and active left-to-right followed by an active right-to-left, try starting at a home position, move right, then return home with the tool disengaged before moving right again. If you only cut/draw in one direction then you wind up with no backlash effects, as long as you move far enough in the opposite direction that you run out the backlash before the tool engages.


问:
Design of the baseplate of mobile robot
I need to construct a Mobile robots preferably circular type of diameter 30 cm, it will be carrying a payload of 5kg. What will be optimal thickness the baseplate considering the material as aluminium? How to calculate these kind of these kind of problems</p>

答:
To be able to select the right material. You will need to estimate the maximum forces that will be applied to it and make sure that it's stronger than that expected maximum.

There's a range of ways to achieve this.

At one extreme, you can create a full model and run finite element analysis on the whole thing and iterate until you have every part of the system to be stronger than the maximum expected load.

In the middle you can do some back of the hand computations to find the worst configuration loads and the weakest points for those loads and make sure that you have a safety margin.

And at the experimental end, you can take a guess, build it and test it in realistic situations and iterate to see if you need stronger or lighter. And iterate on the design.

Each one of these approaches has tradeoffs. More time in the design, more time in fabrication. Or more "optimal" design capabilities. You need to make your own tradeoffs as to what your full optimization function is including design and fabrication time.

Title: Design of the baseplate of mobile robot


问:
Calling external ROS service from C++ code
How can I call a ROS-service that was neither written nor published by me from within C++ code?

I see the service listed when using rosservice list, which shows me

and I can call it easily by using rosservice call /airsim_node/SimpleFlight1/takeoff for instance.

Now, what I want, is to work with this service from within a C++ file (send request, receive response).

The documentation explains how to do this but requires you to have access to the service's source such as its compiled source code or even its *srv-file.

But I want to call an arbitrary service that is out of my scope. I, as a user, am able to do this via rosservice call, so I guess it should be simple to do the same via code?

答:
rosservice call is written in Python, so it has access to a Python module containing the service types. If you were using Python you would be able to use the same module yourself.

However since you want to use C++, you instead need the header files that were generated from the service definition. There unfortunately is no way around this, as those header files and the library that goes with them contain a lot of information required to make the service work: the C++ types for the request and reply, how to serialise and deserialise those types for transport over the wire, etc.

If you can see the service available on your system using rosservice list and rosservice call works, then the Python module for the service is available. This implies that the C++ library and headers are also available. You haven't said what the type of your service is but the header file names follow a pattern of [package_name]/[TypeInCamelCase.h]. You will also need to link to the library containing the service's compiled support code in your CMakeLists.txt; this library name also follows a pattern of [package_name]_gencpp.

Once you know the header name, the library name, and the C++ type that represents the service (which you can get from the header file if you don't already know it), then the rest of your code is the same as in the tutorial you linked to.


问:
What is the consquence of Gimbal lock?
I'm learning about Gimbal lock and I understand the reason why that happened.

But I don't understand why that is a big deal because it only affect this rotation. After this rotation finished, we have 3 new axis to rotate from, so we get back our 3 degree of freedom.

And if the resulting attitude is not what we desired, doesn't that mean we have the wrong Euler angle? which imply that the middle Euler angle should not be 90 degrees.

The only consquence that I can think of is that perhapes some rotation cannot be achieved if the 3 axis rotation order(ex. x->y->z) have been decided? I'm not sure this is the case, please help me out!

Thank you~~

答:
I made a clip for you (https://imgur.com/a/KPeQ7Ia) using Unity, which internally represents rotations as quaternions, but uses Euler angles for display and positioning.

You can see that, at zero orientation, slight rotations about an axis just result in... slight changes about that axis.

We can approach gimbal lock by setting the x-axis rotation to near 90 degrees. Then a slight rotation one way about z generates large positive angles about the y- and z-axes:

... but moving a slight rotation the other way generates large negative angles about the y- and z-axes:

So, is this a problem? It is if you're acting on those angles! If you have a roll controller, a pitch controller, and a yaw controller, and then you get into the gimbal lock condition, then as you can see in the latter two images above one axis remains basically stationary while the other two axes flip almost 180 degrees.

The real rotation of the object doesn't actually change very much, but if you choose to represent the rotation with Euler angles then you get these wild speeds about the gimbal-locked rotation, which can affect your rotational position estimators, your rotational position controllers, etc.

Title: What is the consquence of Gimbal lock?


问:
ROS Network configuration on multiple machines - Publisher and Subsciber
I am trying to set up a simple and listener talker across two machines connected on the same wifi network.
Machine A has the 10.42.0.89 address and is running the roscore and another node that publishes messages on a topic.

```<code>$ printenv | grep ROS
ROS_IP=10.42.0.89
ROS_MASTER_URI=http://10.42.0.89:11311
</code>```

The other Machine B has the ROS_MASTER_URI set to match the one of machine A

```<code>$ printenv | grep ROS
ROS_MASTER_URI=http://10.42.0.89:11311
</code>```

Machine B can get the list of available topics and running nodes correctly

```<code>$ rostopic list
/pose/estimate
</code>```

But can't print out messages in that topic using rostopic echo /pose/estimate.

Do you have any advice?

答:
ROS1

Machine1 [MASTER]:

Will run roscore, but don't run it yet till configurations are done.

Has an IP of 192.168.1.10.

1- Run the following in the terminal:

1.1- export ROS_MASTER_URI=http://192.168.1.10:11311.

1.2- export ROS_IP=192.168.1.10.

2- Now, run roscore.

Machine2 [SLAVE]

Will NOT run roscore.

Has and IP 192.168.1.15.

1- Run the following in the terminal:

1.1- export ROS_MASTER_URI=http://192.168.1.10:11311.

1.2- export ROS_IP=192.168.1.15.

2- Now, you are connected to the Master.

More information in the ROS 1 NetworkSetup guide

ROS2

ROS2 will populate the information on the LAN out of the box without any configuration.

By default you'll want to make sure multicast is enabled


问:
OPC-UA in ROS 2 to communicate with PLC
I would like to establish a communication between my PLC (RevPi) which runs Codesys and a Jetson Nano (or Rasperry Pi) which will be running ROS2. The latter will be connected to a Router (for uploading data to a server), as can be seen on the image below.

My question is therefore: Is it possible to use OPC UA in ROS 2 to transfer the data to my PLC?

Thank you for your answers!

Edit: I am aware of this package and this solution, but it doesn't seem to be compatible with ROS 2.

答:
So to your direct question there is work to provide OPC-UA as a ROS 2 transport "inside" of ROS 2. See these slides form the 2016 ROS I conference

However, I think that the more practical approach will be not to try to run ROS 2 on your PLC, but to bridge it out with a standard OPC-UA client on the Jetson, and then republish it on the ROS 2 communications channel. This will keep things separate and not require you to try to merge two different communication protocols.

Alternatively you could also consider running a ROS 2 node on your PLC and then publishing it via ROS 2 messages to the Jetson as well. Which will require compiling onto the PLC but then just be one more network connection in ROS 2.

Title: OPC-UA in ROS 2 to communicate with PLC


问:
RpLIDAR unable to see black objects
I am working on a ROSBOT for a natural navigation project.

While working with RpLIDAR A2 version, I found that it does not sense thin black surfaces like black pants/trousers while at close range (less than 30 cms).

The screenshots of the RVIZ visualization of the robot is attached below

When the person at more than 80 cm from the LIDAR

When the person at more than 50 cm from the LIDAR

When the person reaches 30 cm from the LIDAR

I tried debugging the code, but no errors in the code.

What are the methods we can use to improve sensing of the robot, can we do any hardware or software tuning to the RpLIDAR sensor to improve its sensing?

What are the likely errors causing this behaviour?

答:
LIDAR sensors rely on sending out a pulse of light it hitting an object and then traveling back to the sensor to detect it.

Black objects are often problematic because they absorb the light and not enough of the light gets back to the sensor. Depending on the technology that the LIDAR uses this can be more or less of a problem at different ranges but usually you will see errors start to occur near the edges of the working range, near or far depending on which limit your hitting.

The most obvious place to see this is in many of the longer range sensors they will spec the maximum range of the sensor for a given level of reflectivity of the object. Because for long range sensing the observable light bouncing back gets so weak as to be undetectable.

Also, black pants versus a "black" board can have very different surface properties which can change the absorbsion. And also remember that most LIDARs also work in infrared wavelengths so the "blackness" of the object is important in a different part of the spectrum than we can see.

And lastly I believe that the RPLIDAR uses a triangulation method which may be having trouble at closer ranges on oblique surfaces such as pant legs. The incident angle also significantly effects the reflectivity.

Title: RpLIDAR unable to see black objects


问:
How to do path planning using only a fixed number of points?
I have a vehicle with cameras to detect different colored cones.
I want to generate a drivable path based on the location of the detected cones.The number of detected cones varies, as it depends on the detection accuracy: for example I might detect 4 blue cones on the left and only 3 yellow cones on the right.
I was thinking of using Bezier's curve to perform this task, however it seems like this method will not result in feasible driving paths.
Here is what I used to visualise a simple 2D path using the Bezier code that @reptilicus posted in this question:

```<code>import numpy as np
from scipy.special import comb

def bernstein_poly(i, n, t):

    return comb(n, i) * ( t**(n-i) ) * (1 - t)**i

def bezier_curve(points,nPoints, nTimes=100):

    nPoints = len(points)
    xPoints = np.array([p[0] for p in points])
    yPoints = np.array([p[1] for p in points])

    t = np.linspace(0.0, 1.0, nTimes)

    polynomial_array = np.array([ bernstein_poly(i, nPoints-1, t) for i in range(0, nPoints)   ])

    xvals = np.dot(xPoints, polynomial_array)
    yvals = np.dot(yPoints, polynomial_array)

    return xvals, yvals

if __name__ == "__main__":
    from matplotlib import pyplot as plt
    nYellow = 3
    nBlue = 3
    pointsYellow = [[0,0],[10,10], [5,20]]
    pointsBlue = [[10,0],[20,10],[15,20]]
    xPointsYellow = [p[0] for p in pointsYellow]
    yPointsYellow = [p[1] for p in pointsYellow]
    xPointsBlue = [p[0] for p in pointsBlue]
    yPointsBlue = [p[1] for p in pointsBlue]
    xvalsY, yvalsY = bezier_curve(pointsYellow,nYellow, nTimes=100)
    xvalsB, yvalsB = bezier_curve(pointsBlue,nBlue, nTimes=100)

    xPath = (xvalsY+xvalsB)/2
    yPath = (yvalsY+ yvalsB)/2

    plt.plot(xvalsY, yvalsY, 'y')
    plt.plot(xPointsYellow, yPointsYellow, "yo")
    plt.plot(xvalsB, yvalsB, 'b')
    plt.plot(xPointsBlue, yPointsBlue, "bo")
    plt.plot(xPath, yPath, 'r')

    plt.show()
</code>```

I used the midpoint between the two boundaries as my drivable path (in red). The result is as follows:

It is evident that bezier curve will not be appropriate as the vehicle will knock the cone if it follows this path.

Is there another method I should look into to constrain my path? I haven't come across many examples that use cones as the boundaries.

答:
If I remember correctly, Bezier curves have the unfortunate side-effect that they do not necessarily form trajectories that move through all of your control points. Two techniques that move through their given control points are Catmull-Rom splines and Hermite Interpolation. Both of these methods have other negative side-effects - I'm not terribly familiar with them, but regardless they will still probably outperform Bezier curves given your problem.

The basic idea of both concepts is that we create polynomials related to our control points, where the trajectory must move through these points (plus some other constraints). Cubic hermite interpolation (the most popular form of Hermite interpolation) puts constraints on "hitting" control points and user-defined velocities at these points. Catmull-Rom splines uses surrounding control points to develop velocity constraints at each control point - thus, we lose control of determining our speed at points of interest (there's a downside right there!).

The actual formulations can be a bit tedious, so perhaps it's best to focus on what constraints are being satisfied when applying one spline technique versus another.

Also - I would say that the technique of forming splines from points like this is a good idea! Personally, I think using the midpoint of your boundaries as control points is pretty clever, so I wouldn't worry about changing this part of your solution.

Here are some resources - I tried to find application-oriented sites for both of the interpolation techniques:

Splines: https://people.cs.clemson.edu/~dhouse/courses/405/notes/splines.pdf

Catmull-Rom: https://qroph.github.io/2018/07/30/smooth-paths-using-catmull-rom-splines.html

Hermite: https://blog.demofox.org/2015/08/08/cubic-hermite-interpolation/


问:
What is the Kalman Filter in the basics of its aspects?
My question is very broad. However I would like a complete description to the very last detail in a way that a foreign exchange student would understand. I want to try my best to master the way the Kalman Filter works. Please be as through as you possibly can, and more.</p>

答:
Imagine watching a bird flying behind some trees.  You can observe the bird's speed and direction, and when it passes behind a tree you have an expectation of (approximately) where you will see it appear on the other side.  If the tree is wide, or there are several trees in a group, the bird will be hidden for longer -- and there will be a larger set of possibilities for where you might see the bird again.  A Kalman filter is a more formal version of this process.

Because this is a statistical technique, it works best when the possible variations in the path of the bird (or whatever you're filtering) follow a gaussian distribution -- in other words, are completely random.

Those are the very basic concepts, and I'm not sure how I'd explain the rest any better than something you'd find on Google.

ContentLicense: CC BY-SA 3.0


问:
/rtabmap/rtabmap: Did not receive data since 5 seconds
trying to run RtabMap on my realsense-435 device. I've set up all the topics and made sure they are published and subscribed, nevertheless, RtabMap gives me this error:

```<code>[ WARN] [1649509757.946524434]: /rtabmap/rgbd_odometry: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set.
/rtabmap/rgbd_odometry subscribed to (approx sync):
   /rtabmap/odom \
   /dynamic_image \
   /dynamic_masked_image_raw \
   /sync_camera_info
</code>```

P.S.: I've synced all these ( the last three topics), therefore there aren't any delays among them.

/rtabmap/odom topic doesn't get published. I'm guessing the issue might have been raised from a bad tf tree??!!

any help is appreciated

the codes I ran:

```<code>roslaunch realsense2_camera rs_camera.launch align_depth:=true
</code>```

then

```<code>roslaunch rtabmap_ros rtabmap.launch     rtabmap_args:="--delete_db_on_start"     depth_topic:=/dynamic_masked_image_raw     rgb_topic:=/dynamic_image     camera_info_topic:=/sync_camera_info     approx_sync:=false
</code>```

the three topics(i.e./dynamic_masked_image_raw   /dynamic_image   /sync_camera_info)  are published with the same timestamp and contain the frames and camera info.
each terminal is sourced with the specific workspace.
out put of rostopic hz (the topics):

```<code>/dynamic_image              16.09   0.04809     0.09111     0.008274   354
/dynamic_masked_image_raw   16.09   0.04802     0.09153     0.008369   354
/sync_camera_info           16.09   0.048       0.09235     0.00834    353
</code>```

full output error:

```<code>[ WARN] [1649758359.612186938]: /rtabmap/rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called.
/rtabmap/rtabmap subscribed to (exact sync):
   /rtabmap/odom \
   /dynamic_image \
   /dynamic_masked_image_raw \
   /sync_camera_info \
   /rtabmap/odom_info
</code>```

答:
You're showing that these topics are published:

```<code>/dynamic_image
/dynamic_masked_image_raw
/sync_camera_info
</code>```

but your warning says it's also subscribed to:

```<code>/rtabmap/odom
/rtabmap/odom_info
</code>```

so what's up with those topics? You also mention that

the three topics(i.e./dynamic_masked_image_raw /dynamic_image /sync_camera_info) are published with the same timestamp

but you have FIVE topics that you're subscribed to, and your error message says (emphasizing):

[ WARN] [1649758359.612186938]: /rtabmap/rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called.

There is an answer here on the ROS forums that says, in part:

try setting approx_sync to true. Could be some of the topics don't have sync'd timesteps.

Body:
You're showing that these topics are published:

/dynamic_image
/dynamic_masked_image_raw
/sync_camera_info

but your warning says it's also subscribed to:

/rtabmap/odom
/rtabmap/odom_info

so what's up with those topics? You also mention that

the three topics(i.e./dynamic_masked_image_raw /dynamic_image /sync_camera_info) are published with the same timestamp

but you have FIVE topics that you're subscribed to, and your error message says (emphasizing):

[ WARN] [1649758359.612186938]: /rtabmap/rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called.

There is an answer here on the ROS forums that says, in part:

try setting approx_sync to true. Could be some of the topics don't have sync'd timesteps.


问:
How does a biped humanoid robot like OttoDIY move?
How does a biped humanoid robot like OttoDIY moves? What is the sequence of foot & legs servos? Can make it move forward, by lifting one foot up and turning leg, and then repeating the same steps for the other leg, it just staying in the same place.
Can't use other libraries, need the algorithm, the sequence of movements. Is there a tutorial which explains different movements using 4 servos.

Left Foot Up

Left Leg Turn

Left Foot Down

Right Foot Up

Right Leg Turn

Right Foot Down

This doesn't seems to work, and the robot stays in the same place

答:
Unlike most bipeds, who walk by shifting their center of gravity towards the direction they want to move, the Otto DIY robot doesn't seem to have any clear shift in center of gravity. This is probably what makes its "walk" so confusing to analyze. While it does not shift its center of gravity in the direction it plans on walking, it shifts its center of gravity nonetheless.

If you analyze the Otto very closely you'll see that right before it starts its "ambulation", it flexes one foot up ("toes" up) and the other foot down. The foot that is flexed up does not affect the center of gravity of the robot. However, the foot that is flexed down lifts the robot up on that side causing the robot's center of gravity to be shifted towards the flexed down foot. We should note that the surface area of contact between ground and foot for both feet is reduced when the feet are pointed. This is important as the foot pointed up now has a normal force from the ground almost colinear to the "leg turn" (as you've labeled it) motor.

So, with the center of gravity now "leaning" towards the foot up leg, the robot's body will rotate over this leg when the motor rotates. This process is then repeated with the other leg to accomplish what is closer to waddling than walking.

Of course, the actual analysis of such a robot would be more complicated than this. For instance, if you implement a similar design yourself you may notice that the foot up leg twists about the leg turn motor instead of the robot's body twisting about the leg. In this case you would need to look at the actual dynamics of the robot to figure out what torques (if any) will cause the desired motion.


问:
Kinematics of a 4 wheeled differential drive robots
I have a 4 wheeled differential drive robot, like the Pioneer 3-AT. There are only two motors, one for left wheels and one for right wheels.

I want to send velocity commands to the robot, I'm using ROS and the standard commands are: [linear_velocity, angular_velocity].

I need to convert them into left and right velocities, from literature if I had 2 wheels I should do this:

$v_l = linear_v - \omega * |r|$

$v_r = linear_v + \omega * |r|$

where |r| is the absolute value of the distance from the wheels to the robot "center".

How should I take into account that I have 4 wheels?

答:
If there is no differential between the front and rear wheels, you will be skid-steering.  You will just treat it like a simple 2-wheeled differential drive robot:
Calculate position of differential drive robot

It shouldn't change much, but if you really wanted to you could find the robot's center of rotation (e.g. following this question) and consider a virtual "axle" running through that point on the vehicle.


答: turns out the culprit was the type of encoding that I used to convert the depth topic from ROS message to OpenCV. For others facing the same issue, the depth image should have 16UC1 encoding. I'm still trying to fix the problem but for now I've found the source of the issue

Body:
trying to run RtabMap on my realsense-435 device. I've set up all the topics and made sure they are published and subscribed, nevertheless, RtabMap gives me this error:

```<code>[ WARN] [1649509757.946524434]: /rtabmap/rgbd_odometry: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set.
/rtabmap/rgbd_odometry subscribed to (approx sync):
   /rtabmap/odom \
   /dynamic_image \
   /dynamic_masked_image_raw \
   /sync_camera_info
</code>```

P.S.: I've synced all these ( the last three topics), therefore there aren't any delays among them.

/rtabmap/odom topic doesn't get published. I'm guessing the issue might have been raised from a bad tf tree??!!

any help is appreciated

the codes I ran:

```<code>roslaunch realsense2_camera rs_camera.launch align_depth:=true
</code>```

then

```<code>roslaunch rtabmap_ros rtabmap.launch     rtabmap_args:="--delete_db_on_start"     depth_topic:=/dynamic_masked_image_raw     rgb_topic:=/dynamic_image     camera_info_topic:=/sync_camera_info     approx_sync:=false
</code>```

the three topics(i.e./dynamic_masked_image_raw   /dynamic_image   /sync_camera_info)  are published with the same timestamp and contain the frames and camera info.
each terminal is sourced with the specific workspace.
out put of rostopic hz (the topics):

```<code>/dynamic_image              16.09   0.04809     0.09111     0.008274   354
/dynamic_masked_image_raw   16.09   0.04802     0.09153     0.008369   354
/sync_camera_info           16.09   0.048       0.09235     0.00834    353
</code>```

full output error:

```<code>[ WARN] [1649758359.612186938]: /rtabmap/rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called.
/rtabmap/rtabmap subscribed to (exact sync):
   /rtabmap/odom \
   /dynamic_image \
   /dynamic_masked_image_raw \
   /sync_camera_info \
   /rtabmap/odom_info
</code>```


问:
Motion Planning vs. Control
Often I see positions for robotics companies split into planning and controls engineers.

But motion planning and controls seem to have a lot of overlap.

As I understand in motion planning the planner produces a trajectory. But to make sure the trajectory is feasible the planner must also take into consideration information about the dynamics of the system and can output controls as well.

Similarly methods like MPC and optimal control output a trajectory of states and times with the controls. In fact in any control paradigm where you have a model or simulator you can produce a trajectory of states from your control inputs and initial condition.

Thus I have 4 questions:

How does motion planning differ from controls?

On a robotics team what is the problem the planning team seeks to solve? Once they solve it, what information do they give to the control module?

On a robotics team what is the problem the control team seeks to solve? How do they use the information from the planning module?

What sort of experience/classes do people on planning teams have? There's plenty of classes on control at my university but I have never seen any about planning. I took a robotics class this semester and it doesn't cover planning in depth and none of the syllabi for other robotics classes cover planning for more than a lecture.

Note here I'm assuming that the planning team works on motion planning instead of path planning. If they work on path planning then I understand 2 and 3.

答:
I would answer your first question. I would say motion planning can be treated as a higher-level control. You can think about motion planning problems as a subclass of optimal control problems (OCP).

You have some goals, e.g. you want to reach a place ASAP, this gives you a cost function.

Your vehicle has its governing dynamics, as well as state and input constraints.

You want to avoid obstacles, either static or dynamic, these are some constraints.

In practice, there are many applications using MPC/LQR to deal with the planning problems to generate a feasible trajectory, which is then tracked by the low-lever controller.

You might also have seen some algorithms used in motion planning, e.g sampling-based methods like A*, RRT. These methods can also be regarded as a way to solve the optimal control problem by using heuristics or sampling. Other algorithms like Artificial Potential Field can be viewed as constructing a Lyapunov function if you're familiar with some control theoty. As you go deeper, you will find more interesting connections between planning and control :)

Title: Motion Planning vs. Control


问:
EKF slam vs global bundle adjustment
How does EKF slam and the covariance matrix interact with global bundle adjustment?

In particular the covariance matrix gives you a confidence on location but global bundle adjustment does not. How do you use the confidences when running global bundle adjustment?

答:
I am assuming your pipeline is that you are running an EKF SLAM algorithm over some data to estimate a set of initial poses and landmarks, and then you feed all of these states into a global bundle adjustment.

In particular the covariance matrix gives you a confidence on location
but global bundle adjustment does not.

First things first is that the global bundle adjustment algorithm does give you the confidence of the states(poses, and landmarks). It is just called something different typically the Hessian or more commonly the Information Matrix($I$). The Information Matrix is just the matrix inverse of the covariance matrix and vice versa. So if you are more comfortable in working with the covariance matrix($C$) then you can just do that operation first.

$$ C = I^{-1}$$

This page for Ceres which is commonly used to solve bundle adjustment problems has a good description of how to extract the covariance.

If you want to do it manually you need to compute your Jacobians($J$), form the Hessian($H$), and then invert it to get the covariance.

$$ C = (J^T*J)^{-1} = H^{-1} $$

How do you use the confidences when running global bundle adjustment?

This is actually kind of difficult to do and very much depends on how you want your EKF algorithm to interact with your Bundle adjustment algorithm. I would say the general approach people do is ignore the EKF covariances for your pose and landmarks. Instead your measurements/factors(e.g the reprojection error) each have their own uncertainty just like in the EKF. And your global bundle adjustment uncertainty would come from those.

If you really want to apply the uncertainty from your EKF algorithm then you would instead set a prior measurement/factor on each pose/landmark and you would set the measurement uncertainty to be equal to the EKF's uncertainty.

Title: EKF slam vs global bundle adjustment


问:
Difference between odometry and localization wrt determining robot's pose?
AFAIK, localization is used to determine a robot's pose from a given a map and sensor data. I know of statistical localization approaches which use particle filters to determine a robot's location within a map.

But doesn't odometry also do the same thing using a different technique? That is, it just uses sensor data to estimate the position (and velocity) of a robot using geometric methods instead of statistical methods?

In other words, aren't both odometry and localization determining the pose of the robot? Is the difference that odometry doesn't need a map, but localization does?

答:
You are right! Both Localization and odometry are intricately related but neither "needs a map".

Odometry tells you how much (and maybe how fast) you are moving. It may also give direction but gives no information about the map or what is around you.

Localization on the other hand is more intricately tied with map building and figuring out where you are on a given map. Simultaneous Localization and Mapping (SLAM) is an important problem in robotics and localization is often used to build up the localization component. But localization does not always need odometry. The idea is that you can extract poses (translations and rotations) using localization techniques.


答: Odometry is the task of calculating where the robot has traveled w.r.t. the robot's previous pose using sensors that measure actuator output (motor encoders, for example.) Localization is the task of placing a robot inside of a pre-defined map. These tasks are often intertwined, but they don't require each other to complete robot positioning. The key here is that there is no map involved with odometry. The odometer on your car is doing odometry - the sensors keep track of how many miles have been driven. You can't reliably calculate the pose of a car using the odometer, but you are still doing odometry. Some cars use GPS to match the location of the car to a map. This does not need an odometer, but odometers do help in correcting errors in GPS sensors. The accelerometer can be used to detect if the car is still moving or not regardless of what the GPS says. I hope this helps clarify things for you.</p>

Title: Difference between odometry and localization wrt determining robot's pose?

Body:
AFAIK, localization is used to determine a robot's pose from a given a map and sensor data. I know of statistical localization approaches which use particle filters to determine a robot's location within a map.

But doesn't odometry also do the same thing using a different technique? That is, it just uses sensor data to estimate the position (and velocity) of a robot using geometric methods instead of statistical methods?

In other words, aren't both odometry and localization determining the pose of the robot? Is the difference that odometry doesn't need a map, but localization does?


问:
Finding correct RPY for approaching a known point on a plane
I have a robot with 6 DOF that I want to use to grab a hanging sheet at a specific point near its edge. The grip needs to be perpendicular to the edge and co-planar with the point of course.

I have the inverse kinematics and I have the XYZ-coordinate that I want to grab, however, I'm struggling a bit with finding the correct RPY needed to approach the point correctly. I have looked at the post How to calculate roll, pitch and yaw from XYZ coordinates of 3 planar points? on Mathwork Matlab Answers, which helped me find the normal to the plane, but there's something I'm not doing or understanding correctly in regards to the calculation of the RPY.

Here's a sketch of the problem:

P2 is the point I want to grab and the arrow illustrates the angle of approach I want to achieve. I know the angle (theta), but none of my robot's axes align with this axis of rotation so I can't simply add this.

Here:

I tried to follow the instructions in the MathWorks post above, ending up with a sketch of the sheet's coordinate frame looking like this:

(Z points into the plane).

Next I calculate the alpha, beta and gamma values like suggested in the post. But what do I do with these?

If I plug the results into my robot the TCP's angle is not even coplanar. I know I haven't taken the theta angle from the sketch into account yet, but I don't want to consider that until I see the TCP is coplanar with the sheet.

Can anyone give me a clue on what I'm doing wrong or if I'm missing some steps? Maybe there's even a better way of achieving what I want than what I tried here? And let me know if I need to clarify anything - this is my first post here, so please be gentle.

Here's the code I have written to do the calculations. p1t, p2t and p3t are the three homogeneous coordinates P1, P2, and P3 respectively in the robot's frame (what I call the global frame):

```<code>// Transform coordinates from cam to robot
Eigen::Vector4f p1t = H * p1;
Eigen::Vector4f p2t = H * p2;
Eigen::Vector4f p3t = H * p3;

Eigen::Vector3f p1t3D, p2t3D, p3t3D;
p1t3D << p1t(0), p1t(1), p1t(2);
p2t3D << p2t(0), p2t(1), p2t(2);
p3t3D << p3t(0), p3t(1), p3t(2);

Eigen::Vector3f v1, v2;
v1 = p2t3D - p1t3D;
v2 = p3t3D - p1t3D;

Eigen::Vector3f xUnitVec = ((p1t3D + p2t3D)/2) - p3t3D;
xUnitVec.normalize();

Eigen::Vector3f zUnitVec = v1.cross(v2);
zUnitVec.normalize();

Eigen::Vector3f yUnitVec = zUnitVec.cross(xUnitVec);

std::cout << "p2t: \n" << p2t << std::endl;

float x1 = xUnitVec(0);
float y1 = yUnitVec(0);
float z1 = zUnitVec(0);
float z2 = zUnitVec(1);
float z3 = zUnitVec(2);

float alpha = atan2(-z2, z3);
float beta = asin(z1);
float gamma = atan2(-y1, x1);

std::cout << "\nRotations: " << std::endl;
std::cout << "Alpha: " << alpha;
std::cout << ", Beta: " << beta;
std::cout << ", Gamma: " << gamma;

pickPoint.clear();
pickPoint = {p2t(0), p2t(1), p2t(2), alpha, beta, gamma};
</code>```

答: A plane is defined through 3 points that pass through it or by a single point and a perpendicular vector. There are formulas that describe this behavior. Then, that theta you drew in your first diagram looks to me like the angle-axis representation of a frame transform. You could then use Rodriguez formula to create a 3x3 transformation matrix of the rotation matrix that defines the approach angle. I suggest adding a 90 degree rotation about the perpendicular vector (whose XYZ location is at P2) to the theta angle to get the approach angle of your gripper. Does this make sense? I hope this helps.</p>


问:
Inverse kinematics with curve constraints
I must calculate the inverse kinematics of a machine in which one of the joints is constrained to a curvilinear path.

I looked for existing libraries to model the problem, but none of them seems to offer support for such kind of joints. In particular I looked ad KDL, but it only allows movements on a straight line (with a prismatic joint) or on a circle arc (with a rotational joint), but not on arbitrary curves.

Are there any tools suitable for such system that could be used instead of a handwritten solver? I would be interested in particular to libraries in C/C++.

答:
You can convert between curvilinear and cartesian coordinates no problem. If you are using cylindrical or spherical coordinates, you can choose a convention and convert between the two.

If you are concerned about arbitrary curves, model your joint as multiple joints with extra constraints. There is nothing wrong with breaking down the problem into sub-problems that are easier to solve. There is no need for KDL. I like to use Eigen for my implementations, but prototyping in Python with Numpy should be enough of a start.


问:
3-links Robot Dynamic PD Computed Torque Control
I am designing the PD control of 3-links robot using Simulink.

I can't figure out what to do in the 'Robot' block to get the acceleration d_qq back to calculate the error.

What is happening in this 'system' block?

答: You should develop the (forward) dynamic model of the manipulator. In fact, by definition, the problem that gives you the joint accelerations from the joint torques in called the Forward Dynamics problem. Which kind of 3 DOF manipulator is?</p>


问:
Powering down servos completely in RobotC+Tetrix
For a certain robotic application (actually for the FTC challenge this year) our team is performing an operation where a servo-driven arm could potentially be forced into an unknown position. We are using NXT+Tetrix.

Since this could damage a powered servo working against this forced position (servo holding arm weight on fixed base is now trying to move heavy base relative to fixed arm), we are thinking about somehow de-powering our servos (or servo controller), in order to get the servos to "relax" and accept the mechanically-forced position.

Originally, we were thinking of having our RobotC code determine the physical position of a given servo and set its desired position to there every loop, limiting how much a servo would try to fight the movement, but to our dismay, ServoValue[fooServo] actually gives us the setpoint, and not the physical location (due to the servo being unable to provide this information).

We also considered setting ServoChangeRate[fooServo] to 1(the minimal value) but this only changes the rate of the target location changing relative to the previous target.

So, we're concluding that the only way to really do this is to fully depower the servos. Is this possible on NXT/Tetrix with RobotC?

A few notes:

I realized as well that one could suggest to rig an encoder(associated with a Tetrix motor that does not need an encoder) onto the rotating area. That actually would not work for mechanical constraints.

I looked into setting PWM enable as shown here but am not sure how to send the i2c commands needed. If someone could clue me in to how these commands would be sent in terms of c code, that would be very helpful.

答: Well, when you change the value of PWM to 0 in an instant, it will definitely shut the servo down, but it will happen in such a sudden fashion that it could break the servo. I am also partaking in this event as well at my school and we are also had the same problem. We even tried setting the value of PWM to 0 in the end, but it broke two of our servos due to the sudden movement. What I recommend is, which I used with my FTC robot to solve my problem is to decrease the value of the angle instead of the PWM by decreasing the servo angle, or speed, by 1.25 every second. Every second is crucial in the games so it is always helpful to use your time efficiently and as fast as possible.</p>


问:
Question about programming approach of a dog like quadruped robot
I and my friends are planning to build a dog like quadruped robot. We have designed  it like MIT Cheetah Mini like legs structure, each leg having 3 Servos, 3rd DOF for rotation. We have calculated inverse kinematics equation for each leg. But I am not able to figure out what our approach should be for programming. We were planning to have trot gait, and to travel on straight path, we planned to find safe distance by each leg and plan next point for each leg on the foot and then just transition legs to that point in trot gait.
But still I don't think it will be a great idea. I need to ask what should be our approach to get the walk done smoothly. </p>

答:
Josh Pieper blogged a few detailed notes as he was developing the quad A1. First you'll want to get the balancing right:

Balancing gait in 2D

Balancing on estimated terrain

Testing real-life hill operation

Then introduce some gaits and see how fast the beast can get ;-)

Higher speed gait formulation

Stable gait sequencing

Another quad A1 speed record – 2.5 m/s

Hoping these links can help develop other quadrupeds!


问:
Are there inverse kinematic solvers that accommodate translational joints?
(Disclaimer: I'm fairly new to this topic, so I apologize if I use some incorrect terminology, and corrections are welcome.)

Normally inverse kinematics involves a kinematic chain of fixed-length links connected by rotational joints. Are there solvers which can also accommodate some of the links being instead translational joints (i.e. a link which can change its length)? I've tried searching online but couldn't come to anything conclusive.

Note that I am not asking about solvers which can account for translational DOFs specified for the end effector - I'm asking about solving for the lengths of translational joints within a kinematic chain that may or may not also contain rotational joints.

答:
Well, when you have a variable link length, all you have to do is assign the length of the link to a variable and then solve the rest of the arm. You will have your joint solutions in terms of a variable link length. You then substitute values in for you link lengths. End of solver.

Linear joints just need to be stored as a simple variable in order to propagate through a solver. Rotational joints involve trigonometry, which is less straightforward. You know how tool tip poses are represented? XYZ, rpy

They aren't represented in RcosTheta, RsineTheta, etc.
They are in Cartesian coordinates.
This is because Cartesian coordinates are easier to understand during presentation.
In other words, the convention makes translation joints trivial.

Link lengths are always assumed to be a constant in solvers, and the algebra used to solve angular joints does not require link lengths be constant or variable, they are always dependent on link lengths anyways. In other words, link lengths don't matter, because the math doesn't change.

It is assumed, given your question, that the maximum and minimum link length values are known. Given this assumption, you need to run your IK solver twice - once with the minimum link length, and once with the maximum. That will give you the range of possible angular joint values. That is what you are looking for.

When you have linear links that change values, you are creating a range of valid angular joint values. This is the crux of your question. You don't have 1 value, you have infinite values.


问:
Mission planning vs path planning vs motion planning
I am struggling to understand the difference between mission planning, path planning, and motion planning. This answer on stack exchange clarified the difference between path planning and motion planning. However, where does mission planning come in?

From what I understand mission planning includes motion planning and path planning.

答:
These are 3 different layers of abstraction.

Mission planning on top
then Path planning, then
Motion planning

Mission planning is: how are we going to get the robot from A to B while achieving sub goals C, D, and E?
Very high level description of overall goals to complete.

Path planning is: These are the waypoints that we will visit that aren't too costly (inside a building or a wall or other obstruction).

Motion planning is: What motor velocity curves do we need to go from waypoint 2 to waypoint 3 considering the constraints of the drivetrain?

Does this make sense?


问:
Gears modeling in Google SketchUp and SketchyPhisics
I'm trying to make differential in Google SketchUp using this tutorial http://support.ponoko.com/entries/21249896-Gears-and-Joints-with-SketchUp-Sketchy-Physics for gears modeling.
But I have problem: gears don't collide with any objects (and other gears). What's wrong? How to fix this?
How to make a bevel gear placed at 90 degrees relative to each other and conical cylindrical gears joints?

P.S. Is there something like SketchUp and SketchyPhisics in Linux?

答: In my personal experience, I hate Google SketchUp. I Recommend that you use CAD Solidworks. Google SketchUp is not meant for gear design. In Solidworks, you are able to create gears however you may and also make it function on a three-dimensional visual on your computer screen. *I personally recommend this to you with great honestly.</p>


问:
How do I handle yaw wrapping when implementing a simple GPS lever arm correction?
I am trying to implement a lever arm correction. I have an aircraft with a gps antenna which is displaced from an on-board sensor whose GPS position I would like to know.

I have the distance from the antenna to the sensor in body coordinates (NOSE-RIGHTWING-DOWN) which I am calling a lever arm:

$LA^{body} = [x_{body} , y_{body} , z_{body}]$

My methodology is:

First convert gps trajectory of the antenna to NED: $Antenna^{NED}$.

Second obtain a DCM rotation matrix $C_{body}^{NED}$ using Yaw, Pitch, Roll

Third transform $LA^{body}$ to $LA^{NED}$ by $LA^{NED} = C_{body}^{NED} LA^{body}$

Finally find NED position of sensor by adding $LA^{NED}$ to $Antenna^{NED}$ \

However, after doing this I am noticing that the NED position of the sensor will jump whenever the yaw goes below -180, since in that case it wraps to +180, and vice versa. How do I handle this problem?

答: The answer actually ended up being very simple, and the problem was indeed due to pi wrapping. Wherever the yaw becomes discontinuous (180 -> -180), just force it to become continuous by adding 360. Similarly, subtract 360 whenever the jump goes the other way. Once the yaw is continuous, the input into the DCM will work because tan is invariant under 360 phase shifts.</p>


问:
How to find the camera matrix and distortion matrix from the given data
```<code>width: 640
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [381.36246688113556, 0.0, 320.5, 0.0, 381.36246688113556, 240.5, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [381.36246688113556, 0.0, 320.5, -0.0, 0.0, 381.36246688113556, 240.5, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi:
x_offset: 0
y_offset: 0
height: 0
width: 0
do_rectify: False
k_int = np.array([[381.36246688113556, 0.0, 320.5],
                  [0.0, 381.36246688113556, 240.5],
                  [0.0, 0.0, 1.0]])
</code>```

I need the camera matrix and the distortion matrix for AR tag detection.I need to identify the camera matrix which is supposed to be a 3*3 matrix from the above data.But,I am unable to determine it.

```<code>corners, ids, _ = aruco.detectMarkers(
                        gray,
                        aruco_dict,
                        parameters=parameters,
                        cameraMatrix=matrix_coefficients,
                        distCoeff=distortion_coefficients)
</code>```

This is where I need the two matrices.I read on internet about the plumb_bob model and it says there are five parameters for it.
[k1,k2,k3,t1,t2].

Someone please help me to understand it.

答:
I assume you got the camera information from somewhere so I would look at the documentation from that source.

K is the camera matrix and D are the distortion params.

It looks like you got this from ROS. The CameraInfo.msg documentation has all the info you need.

So camera matrix is

$$\begin{bmatrix}
381.36246688113556& 0.0& 320.5 \\ 0.0& 381.36246688113556& 240.5 \\ 0.0 & 0.0& 1.0
\end{bmatrix}$$

And distortion params [k1,k2,k3,t1,t2] is all zeros.

$$ [0.0, 0.0, 0.0, 0.0, 0.0]$$


问:
I don't understand Integral part of PID controller
I dont understand integral part of PID controller. Let's assume this pseudocode from Wikipedia:

```<code>previous_error = 0
integral = 0
start:
  error = setpoint - measured_value
  integral = integral + error*dt
  derivative = (error - previous_error)/dt
  output = Kp*error + Ki*integral + Kd*derivative
  previous_error = error
  wait(dt)
  goto start
</code>```

Integral is set to zero in the beginning. And then in the loop it's integrating the error over the time. When I make a (positive) change in setpoint, the error will become positive and integral will "eat" the values over the time (from the beginning). But what I dont understand is, when error stabilizes back to zero, the integral part will still have some value (integrated errors over time) and will still contribute to the output value of controller, but it should not, because if error is zero, output of PID should be zero as well, right?

Can somebody explain me that please?

答:
But what I dont understand is, when error stabilizes back to zero, the
integral part will still have some value (integrated errors over time)
and will still contribute to the output value of controller, but it
should not, because if error is zero, output of PID should be zero as
well, right?

Not right--the output should be whatever it takes to hold the system at the setpoint.  Consider the system of an electric space heater with a 0-11 knob that needs to be set at "5" to maintain the steady state temperature at the setpoint of 72°F.

If PID measures temperature and calculates every second, the error is in units of °F, the integrated error is in units of °F*s, and the derivative is in units °F/s.

At steady state, the error between setpoint and measurement is zero °F, so the proportional term can't contribute anything.    At steady state the derivative of the error is zero °F/s so that term can't contribute anything. All that is left to hold the heater knob at the proper setting is the integral term, which converts the sum of the history of errors into heater knob units.

Supposing Kp = 1, Ki = 0.001, and Kd = 1, then for an output setting of 5 knob units at steady state the PID equation is:

```<code>5 knobUnits = 0°F * 1 knobUnit/°F
            + 0.001 knobUnit/(°F*sec) * integral
            + 0°F/s * 1 knobUnit/(°F/sec)
</code>```

... where the proportional and derivative terms drop out,  and the integrated sum of errors is

```<code>5 knobUnits / (0.001 knobUnit/(°F*sec))= integral
5000 °F*sec = integral
</code>```

It is perfectly fine for the integral to be non-zero when the process is at setpoint--It is the only term providing output at steady state.

Body:
But what I dont understand is, when error stabilizes back to zero, the
integral part will still have some value (integrated errors over time)
and will still contribute to the output value of controller, but it
should not, because if error is zero, output of PID should be zero as
well, right?

Not right--the output should be whatever it takes to hold the system at the setpoint.  Consider the system of an electric space heater with a 0-11 knob that needs to be set at "5" to maintain the steady state temperature at the setpoint of 72°F.

If PID measures temperature and calculates every second, the error is in units of °F, the integrated error is in units of °F*s, and the derivative is in units °F/s.

At steady state, the error between setpoint and measurement is zero °F, so the proportional term can't contribute anything.    At steady state the derivative of the error is zero °F/s so that term can't contribute anything. All that is left to hold the heater knob at the proper setting is the integral term, which converts the sum of the history of errors into heater knob units.

Supposing Kp = 1, Ki = 0.001, and Kd = 1, then for an output setting of 5 knob units at steady state the PID equation is:

5 knobUnits = 0°F * 1 knobUnit/°F
            + 0.001 knobUnit/(°F*sec) * integral
            + 0°F/s * 1 knobUnit/(°F/sec)

... where the proportional and derivative terms drop out,  and the integrated sum of errors is

5 knobUnits / (0.001 knobUnit/(°F*sec))= integral
5000 °F*sec = integral

It is perfectly fine for the integral to be non-zero when the process is at setpoint--It is the only term providing output at steady state.


问:
Manually generate antipodal grasps for grasping
I am getting into robot manipulation using parallel grippers, and am learning about it, and programming grasping different objects. I have the 3D model of the objects I wish to grasp. The next step is to actually generate antipodal grasps on the object. I was wondering if there is a tool where I can specify / annotate the grasp location on the 3D object. Then, using this I should be able to pick up the object in simulation.

Any help would be appreciated.

答:
You can use GraspIt! from the moveit ROS metapackage. I would recommend, however, deriving your own vector approach for grasping objects. You have to give your grasping software your gripper dimensions anyways, might as well do your own grasping algorithm.

I suggest you use vectors to place the origin of the gripper as well as how you wan to approach the object. These 2 separate problems can then be joined together to grasp objects.


问:
Incorporating motion model in Graph Slam and Bundle adjustment
When doing global bundle adjustment how does one incorporate a motion model or IMU information?

Does it simply impose a cost function in relation to how far away the expected location is of the robot or is there something else?

答:
Would say you can generally do 2 different ways of incorporating the information.

Compute the odometry.

Simply compute how far your robot is supposed to move in between frames. Either using your motion model, or integrating you IMU measurements. You then as you said simply create a cost function(edge) that has a cost the further away the node is from the expected location. Essentially this equation

$$ e(T_{w1},T_{w2}) = T_{w1}^{-1}*T_{w2} - T_{odometry}$$

Where $T_{odometry}$ is the computed movement from your motion model or IMU integration.

You can find an actual example of this here in Ceres.

Use a IMU/ motion model equations directly

You can actually use the equations directly to form a constraint between 2 pose nodes.So your cost function may look something like this:
$$ e(T_{w1},T_{w2}) = T_{w1}^{-1}*T_{w2} - f(encodervalues)$$
where $f(encodervalues)$ is some motion model function.

However, this is usually done in a slightly different manner typically called a preintegrated factor. The paper describing it can be found here, with a code example in GTSAM..

I am not going to go into full detail on why it is done, but essentially this allows you to sum multiple IMU measurements into 1 cost function(generally IMU is running at a much higher rate so you may have 20 measurements between 2 camera measurements and we only want 1 cost function not 20).


问:
How can I receive Genicam packets from a device in a docker container?
I installed a command line tool which accepts genicam packets from a device as input into a docker image. If I use the option --network host in the run command for the container the packets are received. If I use -p 3956:3956/udp to expose the Genicam default port 3956 (e.g. stated here: RoboRealm - GenICam - Instructions) in the run command of the container I don't get packets. What did I wrong? Could the port be different from the default? How could I get the changed port then?

答:
The solution by @Gornoka works well. There is no need to add UDP port to detect the Genicam GigE camera. If multiple networks are to be used, something like below can be used

```<code>version: "3.7"

networks:
  some_net:
    ipam:
      config:
        - subnet: 172.20.0.0/24
  camera_vlan:
    driver: macvlan
    driver_opts:
      parent: enp4s0 # adapter name changes as per deployment computer
    ipam:
      driver: default
      config:
        - subnet: 192.168.3.0/24
services:
  some_service:
    image: image_name
    networks:
      some_net:
        ipv4_address: 172.20.0.8
      camera_vlan:
        ipv4_address: 192.168.3.150

</code>```

It might be worth specifying a static ip-address to the camera_vlan network so that there is no ip-address conflict with other devices on the network.

Body:
The solution by @Gornoka works well. There is no need to add UDP port to detect the Genicam GigE camera. If multiple networks are to be used, something like below can be used

version: "3.7"

networks:
  some_net:
    ipam:
      config:
        - subnet: 172.20.0.0/24
  camera_vlan:
    driver: macvlan
    driver_opts:
      parent: enp4s0 # adapter name changes as per deployment computer
    ipam:
      driver: default
      config:
        - subnet: 192.168.3.0/24
services:
  some_service:
    image: image_name
    networks:
      some_net:
        ipv4_address: 172.20.0.8
      camera_vlan:
        ipv4_address: 192.168.3.150

It might be worth specifying a static ip-address to the camera_vlan network so that there is no ip-address conflict with other devices on the network.


问:
Beginner Soldering question
So, I need to know a couple of things about soldering. My primary workspace, robotics and otherwise, is a desk with a computer and only a little bit of free space (4 ft. by 6 in.). I am wondering if it is safe to solder in such a small area. Also, what level of ventilation do I need to solder safely? My desk is in a normal house room and my desk is write next to an air vent. My house has heating and A/C. Do I need a fan or a fume sucker thing? I plan to only solder a little to get things to stay in my solder less bread board (soldering header pins onto wires and such). So, basically, what are the minimum requirements for soldering safely (space and ventilation)? Also, if anyone could point me to some hobby/beginner level soldering must-haves on Amazon that would be great, thanks.</p>

答:
Modern solders are lead free. Even with older solders that contained lead, the amount present in the "smoke" was minimal. Having said that common sense say to thoroughly wash your hands when get done and don't eat or drink while soldering.

If you are concerned there are number of small desktop "filters" you can purchase. Most are not much more than a simple fan, like one for a PC power supply, with a carbon filter element over the fan inlet or outlet.

Title: Beginner Soldering question


问:
Simulating robotic swarm
I am playing around with swarm robotics and trying to figure out a way to simulate it. Here is my dilemma:

I am trying to simulate thousands of agents that will asynchronously communicate with local neighbors to determine an action so a global behavior can be achieved without a centralized logic center. The initial solution to simulate this seems pretty straightforward - each agent is a thread, however, I can't spawn thousands of threads. I also looked into the actor-model framework, but that also does not work since it's single-threaded so I lose the ability for it to be async and whenever an agent is waiting for a signal, it will be thread blocking so other agents will never be able to execute and logic. Any direction will help. Happy to add any clarification. Below is an example of threading. I am trying to create a swarm of 20,000 agents however I cannot create 20,000 threads. I need each agent to run in parallel as that is how it would work with real hardware.

main.py

```<code>n = 20000
for i in range(n):
    agent = Agent()
    agent.start()

# Additional logic to start sending and receiving messages between agents.
</code>```

agent.py

```<code>class Agent(Thread):
    def __init__(self):
        ...

    def send_message_to_neighbor(self):
        pass

    def receive_message(self, message):
        pass
</code>```

error

```<code>error: can't start new thread
</code>```

答:
This is an ideal use case for async/await.

Your simulated agents will spend nearly all their time waiting for something. It doesn't take a whole thread, with its stack size and address space used and all that, just to wait. Using the async formalism, a small number of threads can service a large number of async agents. At any moment, only a small number of agents will have something to do, and once they do it they will go back to waiting.

A lower-tech, lower-elegance alternative is to frequently poll each agent, giving it a chance to discover whether it has anything to do yet. In this arrangement, you need to explicitly code the agent to return to the caller ASAP so it can get on with polling the next. This also means that any state it needs to preserve from one poll to the next -- even tiny things like loop counters -- must be stored in some instance.

With async/await, the system automagically takes care of that for you. Unfortunately, in my experience, it also leaves you on your own to wrap your mind around this kind of concurrency.

As an aside: in your simulation, even if you use a polling loop, try to manage simulated time abstractly, separated from real time (but with a way to tie them together). This can radically speed up rerunning flocking algorithms, for instance.

Title: Simulating robotic swarm


问:
Robot odometry calculation On uC or on pc
I need to do odometry calculations but I'm contemplating where I should do it. I'm using a core i5 6th gen mini pc with 8Gb ram so it's plenty strong for a robot brain but my issue is with the communication between it and my micro which is an f407 discovery. Using a mini pc has locked me out of any PCIe mesa card interface and researching on USB I found out it can have up to a 65ms delay worse case when transferring/receiving data. Which just leaves ethernet (I can't do that because my Uc doesn't have a DMA channel for ethernet).

So after finding all that out, My next idea was to just let the Robo brain tell my uC where to go and the uC does all the odometry calculations and sends an acknowledgement back to the brain when it has reached the desired position.

My uC will be responsible for reading some sensors, 2x1000c/r encoders, 2x motor control, battery management etc. I think it can handle it...I think.

Is this a reasonable approach or should I try something else?

答:
It's common to do the encoder reading and low level odometry calculations at lower levels in microcontrollers. As well as for battery management and motor control as those things require very low latency updates. And if you can then integrate the results locally and forward their abstracted output periodically to the higher level computer that's a common approach.

There will be tradeoffs for how much computation you can do in the microcontroller both what algorithms you use and how you implement them, as well as how much bandwidth to spend on communications, trading off throughput and latency etc. But I would suggest going ahead with your approach and for your application just like every other one there are likely to be specific tradeoffs that you'll have to learn to balance.


问:
How do ultrasonic range finders detect objects at an angle?
As far as I can tell, an ultrasonic rangefinder works by reflecting inaudible soundwaves off of objects and timing their return.  But if the object has a flat surface and is angled with respect to the line to the rangefinder, how does it detect that object?  Under what circumstances might it give a false distance or otherwise fail to detect the object?</p>

答:
The sound beam is not traveling in a straight line but is leaving the range finder is a multi-lobed pattern.

Of course we are interested in the main lobe. When the sound wave hits an object, it is reflected in various directions. So some of that energy returns to the sensor and triggers it, therefore measuring the distance. Reflection depends on the material and the shape of the object as well as the angle with respect to the centerline of the range finder. As you pointed out, if this angle is large enough the reported distance will be off, as shown in the following figure

Both figures are from the paper "M. Drumheller - Mobile robot localization using sonar (1987)"


问:
Why in forward bending knee orientation in quadruped legged robot require less torque in knee joint?
I'm trying to find answer in terms of physics explanation, why knee joint torque requirement is less in the forward bending leg orientation of a quadruped legged robot.
I'm referring this article, in the paragraph 2.2. (Table 1 & 2)---
https://nereus.mech.ntua.gr/Documents/pdf_ps/eccomas15-2.pdf

Thank you in advance. Any answer or comment or upvote will be appreciated.

答:
The forward bent knees require less torque for these use cases because of the configuration. With the forward bent knee the force vector from the point of contact goes much closer to the knee joint resulting in a much lower moment arm. This lower moment arm means that the torque is much lower for a given force.

The other simple way to think about this is that in the forward knee position when pushing forward and upward, the lower limb is much more under compression rather than needing to be twisted to provide the force at the foot.


问:
Quadcopter force/torques duty cycle conversion
after having been determined my control loops for my quadcopter project, I'm going to determine the motor commands (PWM duty cycle) from the motor forces/torques. I was following the guidelines of this document but when I was trying to do the inverse of the matrix M (page 17) it has determinant equal to 0. The procedure is correct? Anyone can suggest me some other link for doing this conversion? I have searched in the Internet but I haven't found so much about that. Thanks
The part of the document that I'm referring is the following:

答: Each axis input is a linear combination of the control input and the sensor (gyros and accelerometers) for that axis.  Not necessarily answering your question, but add this to your matrix</p>