问:
ROS2 Nav2 Not Starting with namespace
I have a ROS2 workspace with a package that is functioning properly. This package starts Nav2 using the bringup_launch script.

Now I want to start Nav2 with a given namespace. However, when I pass the namespace argument (as below), everything starts correctly but the Nav2 stack doesn't show any info in the command line and also shows up as unknown in RViz (It hasn't started). The other nodes being launched along with Nav2 start correctly with the namespace argument (ex. robot_state_publisher and robot_localization)

Nav2 is started as follows:

```<code>    nav2_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(nav2_launch_dir, 'bringup_launch.py')),
        launch_arguments = {'map': static_map_path,
                            'params_file': nav2_params_path,
                            'use_sim_time': LaunchConfiguration('use_sim_time'),
                            'autostart': 'true',
                            'namespace': 'abc',
                            'use_namespace': 'true'}.items()
    )
</code>```

Any Ideas why? Also, can someone please provide the correct way to launch Nav2 if the given approach is not correct?

ROS2 Distribution: humble

答:
You can try this approach that my fellow student used here:

https://github.com/igrak34/TurtleBot2-on-ROS2/blob/main/turtlebot2_nav/launch/nav2.launch.py

https://github.com/igrak34/TurtleBot2-on-ROS2/blob/main/turtlebot2_nav/param/nav2_params.yaml

In nav2_params.yaml you have odom_topic: /namespace/odom etc.

In the Nav2 launch file you have

...

```<code>use_namespace=LaunchConfiguration('use_namespace')
namespace=LaunchConfiguration('namespace')
</code>```

...

```<code>namespaced_params= ReplaceString(
    source_file=param_path, replacements={"/namespace":("/",namespace)}
)

namespaced_rviz_config_file = ReplaceString(
    source_file=rviz_config_file, replacements={"/tb2": ("/", namespace)})
</code>```

Remember about preparing the RViz file:

https://github.com/igrak34/TurtleBot2-on-ROS2/blob/main/turtlebot2_nav/rviz/namespaced_nav2.rviz

We found this way the easiest.

Approaches without modifying the YAML had problems with some nodes not in the namespaces as far as I remember (at least in the case of the TurtleBot2 upgrade to ROS2). Maybe these problems are fixed now.

Title: ROS2 Nav2 Not Starting with namespace

Body:
You can try this approach that my fellow student used here:

https://github.com/igrak34/TurtleBot2-on-ROS2/blob/main/turtlebot2_nav/launch/nav2.launch.py

https://github.com/igrak34/TurtleBot2-on-ROS2/blob/main/turtlebot2_nav/param/nav2_params.yaml

In nav2_params.yaml you have odom_topic: /namespace/odom etc.

In the Nav2 launch file you have

...

use_namespace=LaunchConfiguration('use_namespace')
namespace=LaunchConfiguration('namespace')

...

namespaced_params= ReplaceString(
    source_file=param_path, replacements={"/namespace":("/",namespace)}
)

namespaced_rviz_config_file = ReplaceString(
    source_file=rviz_config_file, replacements={"/tb2": ("/", namespace)})

Remember about preparing the RViz file:

https://github.com/igrak34/TurtleBot2-on-ROS2/blob/main/turtlebot2_nav/rviz/namespaced_nav2.rviz

We found this way the easiest.

Approaches without modifying the YAML had problems with some nodes not in the namespaces as far as I remember (at least in the case of the TurtleBot2 upgrade to ROS2). Maybe these problems are fixed now.


问:
Want to make an URDF starting from a robot .STL
I want to make an URDF from a Staubli TX2-140. I have the .STL of the all arm. Does someone know how to separate it to get individual links .STL?

Thanks

答:
I am also looking for a way to convert .STL or .STEP files to urdf. The best I've found so far was to use onshape or gazebo and recreate the model.

"There are plenty of options, for these tools, just search online." Searching the Internet brings back this post. Care to give specifics?

ContentLicense: CC BY-SA 4.0


问:
Why does the low variance resampling algorithm for particle filters work?
I am studying and coding particle filters and I am using the Low variance sampling algorithm suggested in the Probabilistic Robotics book. I understand the procedure for the algorithm. A random number r is picked from the interval (0, 1 / M) and a variable U, calculated based on r is used to navigate the sample space systematically. A variable c(cumulative sum) is initialized with the first weight, and incremented by adding weights until it is higher than U. Once the cumulative sum is higher than U, it picks the sample corresponding to the weight last added.

The problem that I have is that I don't see how this picks up a good sample set for the next iteration. This seems very random or at least favorable to lower valued weights. If the initial value of r is very low, U is also low initially and it may pick a sample whose weight is low, unless weight vector is sorted from high to low (Is it sorted?). However, this video suggests that particles with higher weight have a better chance of getting picked. The algorithm doesn't convey this idea to me. Please help if you have an explanation.

答:
1. What is this algorithm doing?

Imagine laying out a yardstick and partitioning it proportionally according to the individual parents’ fitnesses. For $M$ offspring particles, you want to find $M$ equally spaced points (think np.linspace) along this yardstick. The partition that the $i$’th point lies in determines the parent that gets to produce the $i$’th offspring. The $r$ in the algorithm is simply choosing a uniform-at-random starting point for your linspace, which of course needs to be in the interval $(0, \frac{1}{M})$ for you to be able to fit all $M$ of your points. This randomness ensures that the resampling is fair and also not completely deterministic.

2. Why does this strategy work?

TLDR; A particle filter’s convergence rate is inversely proportional to the variance in the parents’ offspring counts. Low variance means fast convergence.

NOTE: Below, I discuss (but never explicitly mention) the concept of variance effective population size.

The effectiveness of this resampling strategy makes a lot of sense when you consider that the particle filter is, at its core, an evolutionary algorithm (EA). All EAs tend to lose diversity because of the random sampling step, at a rate proportional to the  variance in how many offspring the parents produce. This effect is known as genetic drift, and it adversely affects biological populations as well. As a result of this diversity loss, the EA prematurely converges to suboptimal solutions. This is bad news.

Intuitively, genetic drift makes sense. When parents are chosen at random (the most common setup), those with low fitness/quality are likely to get skipped and produce zero offspring and be eliminated from the gene pool, thus reducing the population’s diversity. Now, the rate at which natural selection improves a population’s fitness is directly proportional to the population’s diversity—diversity is literally the fuel that drives natural selection. Genetic drift reduces the the diversity, thus decreasing the rate at which the population’s fitness can improve and causing the aforementioned bad news.

Let’s now look at the low-variance resampling method. This method gives almost every parent exactly the number of offspring that they would be expected to get with the random sampling, without the extra uncertainty (variance). This reduces genetic drift and greatly increases the efficiency with which the particle filter can improve its solution and/or adapt to changing conditions.


问:
Redundancy, Null Space and joint limit avoidance
I am developing a controller for a Franka Emika with 7 DoF. The cartesian motion is working, but I'd like to add some constraints in the joint movement range, with the values described by the manufacturer.
I've read a little but I think I couldn't get the principle so well. I've got a general idea from this question:

Redundancy and Null space projection

But didn't really got how to get a function

$$
\dot{q} = k_0 \frac{\partial w(q)}{\partial q}
$$

such that I can project into the Null Space and achieve this secondary task.

Some references to build better foundations and go deeper are also very appreciated.

Thank you!

答:
Let's consider the definition of $w(\mathbf{q})$ from the end of the linked post:

$$ w(\mathbf{q}) = - \frac{\alpha}{n} \sum^{n}_{i=1}
\left( \frac{q_{i} - \hat{q}_{i}}{\bar{l}_{i} - \underline{l}_{i}} \right)^{2}   $$

where some terms have been renamed or changed - for instance $\alpha$ represents an arbitrary but constant weight to be scaled by $n$, $\bar{l}_{i}$ and $\underline{l}_{i}$ are the upper limit and lower limits of joint $i$ respectively, and $\hat{q}_{i} = \frac{\bar{l}_{i} + \underline{l}_{i}}{2}$ is the midpoint between the joint limits. Note that $\mathbf{q}$ represents the current joint configuration as a vector, and $q_{i}$ represents the $i^{\text{th}}$ joint belonging to that vector. Now, let's solve the equation $\dot{\mathbf{q}} = k_{0}\frac{\partial w(\mathbf{q})}{\partial \mathbf{q}}$ entry-wise as follows:

$$ \dot{q}_{i} = - k_{0} \frac{2\alpha}{n} \left( \frac{q_{i} - \hat{q}_{i}}{\bar{l}_{i} - \underline{l}_{i}} \right)  $$

There's alot of terms here, so we can use $\alpha$ to remove the $k_{0}$ and $n$ by setting it to some value relative to $k_{0}$ and $n$, such as $\frac{\beta n}{2k_{0}}$. This gives us:

$$ \dot{q}_{i} = - \beta \left( \frac{q_{i} - \hat{q}_{i}}{\bar{l}_{i} - \underline{l}_{i}} \right)  $$

Note that the only role $\alpha$ played in the original definition was to make this simplification possible. Because we are looking at a single joint, we can boil down this equation further by defining a $\gamma_{i}$ term to combine $\beta$ and the joint limits together (keeping in mind that applying this process to each joint would use the parameters of that specific joint). With $\gamma_{i} = \left( \frac{\beta}{\bar{l}_{i} - \underline{l}_{i}} \right) \gt 0$, we have:

$$ \dot{\mathbf{q}} = \left[ \begin{array}{c} - \gamma_{1} (q_{1} - \hat{q}_{1}) \\ \vdots \\ - \gamma_{n} (q_{n} - \hat{q}_{n}) \end{array} \right] $$

So, now that we have a formula for $\dot{\mathbf{q}}$, let's analyze what's happening. As we can see, if this unconstrained velocity is applied to the robot, then each joint will move towards the halfway point between its joint limits, $\hat{q}_{i}$. This is because $- \gamma_{i} (q_{i} - \hat{q}_{i})$ is negative when the value of joint $i$ is greater than this midpoint, and $- \gamma_{i} (q_{i} - \hat{q}_{i})$ is positive given the opposite case.

This concept of using a quadratic function as a objective function is very common - especially in gradient-based applications such as controls. Because a quadratic has only one minimum or maximum and the gradient along these functions can be used to iteratively compute this min/max, they make a good choice for simple and effective optimization objectives. That is essentially the idea behind how such a function is defined - if you have a desired position, velocity, acceleration, etc. profile for the motion of your robot, quadratic functions can be used to represent the "cost" or "quality" of the current state of your arm plus a well-defined way to reach the minimum of this "cost" or maximum of this "quality".

So, let's say we have an arbitrary objective function $v(\mathbf{q})$ that is quadratic and has a maximum value that is achieved by setting the joint velocities to $\frac{\partial v(\mathbf{q})}{\partial \mathbf{q}}$ (the same properties as $w(\mathbf{q})$). Note that $\frac{\partial v(\mathbf{q})}{\partial \mathbf{q}} \in \mathbb{R}^{n}$. If we now want to apply these joint velocities in a manner such that the end-effector velocity is not affected, they need to be projected onto the null-space of the end-effector Jacobian. For the following analysis, note that the pseudo-inverse of the Jacobian, $\mathbf{J}^{+}$, conceptually represents a way of solving for the smallest joint velocities that can exactly satisfy the desired end-effector velocity, if that end-effector velocity is achievable. So, given the null-space projection matrix, $\mathbf{I} - \mathbf{J}^{+} \mathbf{J}$, where $\mathbf{I}$ is the $n\times n$ identity matrix, we see that constraining the velocity $\frac{\partial v(\mathbf{q})}{\partial \mathbf{q}}$ to the null-space gives us:

$$ \dot{\mathbf{q}} = (\mathbf{I} - \mathbf{J}^{+} \mathbf{J}) \frac{\partial v(\mathbf{q})}{\partial \mathbf{q}} $$

Noting that:

$$ \dot{\mathbf{x}}_{v} = \mathbf{J} \frac{\partial v(\mathbf{q})}{\partial \mathbf{q}} $$

represents the end-effector velocities created by setting the joint velocities to $\frac{\partial v(\mathbf{q})}{\partial \mathbf{q}}$. Incorporating this term into the null-space projection gives us:

$$ \dot{\mathbf{q}} =  \mathbf{I} \frac{\partial v(\mathbf{q})}{\partial \mathbf{q}} - \mathbf{J}^{+} \dot{\mathbf{x}}_{v} = \frac{\partial v(\mathbf{q})}{\partial \mathbf{q}} - \mathbf{J}^{+} \dot{\mathbf{x}}_{v} $$

So, in essence the null-space projection solves for the end-effector velocities that will occur from using an unconstrained joint velocity and then removes the joint velocities that cause this end-effector velocity. Thus, as long as the optimization objective function can be maximized/minimized without moving the end-effector, this method is effective. However, it's very important to note that this method does not guarantee that the objective function will be maximized/minimized, and even worse it cannot guarantee that it will have any effect at all (consider the case where $\frac{\partial v(\mathbf{q})}{\partial \mathbf{q}}$ is orthogonal to the null-space).

To guarantee joint limit avoidance, manually disabling the given joint from moving towards the limit once it has passed some threshold is an option. In addition, I believe model-predictive control (MPC) is another tool people use for this sort of problem, but I'm not terribly familiar with this.

As far as references go, the go-to paper here is titled “On the inverse kinematics of redundant manipulators: Characterization of the self-motion manifolds” by Joel Burdick. It's an older paper, but it lays out the fundamental concepts of redundant robot kinematics about as good as anyone will. Apart from this, there are many, many papers that should go over various techniques for trajectory optimization based on the null-space of the Jacobian.


问:
How to control a 4 wheel mechanum wheel robot with PID?
I have a robot with 4 mechanum wheels. I implemented a pid controller on each wheel and as a result it doesn't drive straight. When I send a command to simply move in the x direction forward, it ends up doing an arc.

Obviously my wheels/motors aren't identical and I can't have the same PID tuning parameters for all of them. I tried to tune all 4 controllers manually and gave up, because I wasn't able to get a consistent result.

So now I am trying a different approach where I want to tune the linear and angular velocities individually. I also split up the linear velocity into x and y. The parts in green are the ones that I'm currently working on.

Does this approach make sense?
One thing that doesn't seem right to me is that in the inverse kinematic equation I convert the set point to a PWM value that I then write to the robot. Should there be an additional PID controller that is only working on setting a predetermined RPM to the motor?

Also is there a way to avoid including a low pass filter? I had to implement one because the speed is calculated from quadrature encoders and it was too noisy.

If anyone has any thoughts on this mechanum wheel pid control, any help would be greatly appreciated. Thank you.

答:
Let's consider first the task you are wanting to accomplish: following some sort of Cartesian space trajectory. So, you are wanting to make sure your robot is only moving in the x-direction. However, you have no measurement of where you are along the x-direction. To remedy this, you've chosen to use the motor encodings - I am assuming to do basic distance calculations determining how far you have moved. Now, I would like to stress this very, very much: motor encodings of wheeled robots are not suitable for forward kinematics. In the same vein:  motor velocities of wheeled robots are not suitable for forward kinematics. The biggest problems with using these values is that the wheels can, and are designed to, slip. This will cause compounding errors over time resulting in unusable information in the long run (really even after a few seconds of motion). So, instead of using the motor encodings to determine your location in the world, use a sensor designed to do just that - like an IMU or a camera.

Now, let's say you have an accurate estimation of your position in the world frame. You might then want to determine the error between your current trajectory and your desired trajectory. Let $x_{t-1}$ be your previously computed x location and $x_{t}$ be your current world location (same for the y location and angle $\theta$). To find the velocity error (without using a desired position), we get an error vector that looks like:

$$ \mathbf{e} = \left[ \begin{array}{c} v_{x} - \frac{x_{t}-x_{t-1}}{\Delta t}  \\ v_{y} - \frac{y_{t}-y_{t-1}}{\Delta t}  \\ v_{\theta} -\frac{\theta_{t}-\theta_{t-1}}{\Delta t}   \end{array}  \right] $$

where $v_{x}$ is your desired x velocity, and likewise for $v_{y}$ and $v_{\theta}$. Since you are using the inverse kinematics, I am assuming you have a Jacobian matrix $\mathbf{J}$ that maps wheel velocities to current frame velocities (that is how I've seen the analysis done at least). Take the pseudo-inverse of this, giving you $\mathbf{J}^{+}$, which maps current frame velocities to wheel velocities. Solve for the wheel velocities that will correct your world frame velocity error:

$$ \mathbf{\omega} = \mathbf{J}^{+} \left[ \begin{array}{ccc} \cos(\theta) & -\sin(\theta) & 0 \\ \sin(\theta) & \cos(\theta) & 0 \\ 0 & 0 & 1  \end{array} \right] \mathbf{e} $$

where $\omega$ is the computed velocity to correct this error, and the listed rotation matrix moves the error from the world frame to the current frame. You could of course try to come up with a PID controller for the motors themselves to achieve $\omega$, however that still doesn't solve the actual velocity error issue, as there is almost no chance your robot will match the modeled world frame velocity just by using well-controlled motors. So, my suggestion would be to use a PID controller to determine wheel velocity commands based on the velocity of the mobile robot in the world frame.

First, I would use a variable $\Omega$ as the sum of the computed $\omega$ values at each time step, i.e.

$$\Omega_{T} = \sum^{T}_{t=1} \omega_{t} $$

where $T$ is the current time and $\omega_{t}$ is the computed $\omega$ at each time $t$. Because you are looking to reach a constant velocity (this could be extended to non-constant velocities as well), your derivative error term would look something like:

$$ \Delta \omega_{t} = \boldsymbol{0} - \frac{\omega_{t} - \omega_{t-1}}{\Delta t} =  - \frac{\omega_{t} - \omega_{t-1}}{\Delta t} $$

where $\omega_{t}$ is your currently computed wheel velocities and $\omega_{t-1}$ is your previously computed wheel velocities. Combining these terms together, your control law could be something like:

$$ \mathbf{u}_{t} =  k_{p} \omega_{t} + k_{i} \Omega_{t} + k_{d} \Delta \omega_{t} $$

Noting that $\Omega$, $\omega$, and $\Delta \omega$ are vectors, and $k_{p}$, $k_{i}$, and $k_{d}$ are the proportional, integral, and derivative gains of the PID controller. Because PID controllers are designed to work with unknown dynamics (such as how PWM inputs correspond to motor velocities), this should allow you to use PWM inputs assuming the gains are properly tuned.


问:
RC Helicopter Connect with computer
I am planing to control my RC helicopter with my computer. I have experience of programming in .Net. Could we use .Net to control RC helicopter?
From where can I start this project?</p>

答: You can use onboard microcontroller like FEZ cerb40 which you can program using .NET micro framework. You can use this to control servo and ESC. You can use Xbee or any similar wireless serial communication system to receive commands from your computer using serial terminal program.</p>

ContentLicense: CC BY-SA 3.0

Title: RC Helicopter Connect with computer


问:
How to compute orientation error for a Puma end effector
i hope that you'll be ok!

I'm trying to control a Puma robot using the inverse kinematics in Matlab.
I used unit quaternions to express the orientation of End Effector frame and orientation of desired frame from Rotation matrix, like in Sciavicco-Siciliano.
This is the code:

```<code>Rd = [-sin(beta)  0  cos(beta);  % desired orientation matrix of ee
      cos(beta)  0  sin(beta);
      0         -1  0        ];

Re = H0e(1:3,1:3); % orientation matrix of EE from direct kinematics

u0 = @(R) 0.5* sqrt(1+R(1,1)+R(2,2)+R(3,3));
u1 = @(R) 0.5* sign(R(3,2)-R(2,3)) * sqrt(1+R(1,1)-R(2,2)-R(3,3));
u2 = @(R) 0.5* sign(R(1,3)-R(3,1)) * sqrt(1-R(1,1)+R(2,2)-R(3,3));
u3 = @(R) 0.5* sign(R(2,1)-R(1,2)) * sqrt(1-R(1,1)-R(2,2)+R(3,3));
S = @(x,y,z) [0 -z y; z 0 -x; -y x 0];

Qd = [u0(Rd) u1(Rd) u2(Rd) u3(Rd)]';  % {eta_d, epsilon_d} desired unit quaternion
Qe = [u0(Re) u1(Re) u2(Re) u3(Re)]';  % {eta_e, epsilon_e} unit quaternion of ee
</code>```

The problem is that two different rotation matrix produce the same unit quaternion and therefore the error is zero.

These are the two matrices:

```<code>Rd =

0.7250         0    0.6887
0.6887         0   -0.7250
     0   -1.0000         0

Re =

0.7250   -0.6887    0.0000
-0.0000   -0.0000    1.0000
-0.6887   -0.7250   -0.0000
</code>```

Where am i doing wrong? Could you help me?

Thank a lot!

答:
Where did you get those equations from for the conversion to quaternion? I went to this question and implemented two of the answers there in Matlab (Octave), and both answers gave the same results:

```<code>>> Qd2
Qd2 =

   0.10469  -0.26218  -0.26218   0.65670

>> Qe2
Qe2 =

   0.65670  -0.26218  -0.26218   0.65670
</code>```

I converted this answer, and your question meets the (b1_squared >= 0.25) criteria, so I could use it, but it's not suitable for all conversions because not all cases are implemented there ("I will leave it as an exercise to the OP to fill out the other three.")

Then I converted this answer and got the same results. That answer does apply to all conversions, so I'll give it to you here:

```<code>function q = MakeQFromR(R)
  if (R(3,3) < 0)
      if (R(1,1) >R(2,2))
          t = 1 + R(1,1) -R(2,2) -R(3,3);
          q = [t, R(1,2)+R(2,1), R(3,1)+R(1,3), R(2,3)-R(3,2)];
      else
          t = 1 -R(1,1) + R(2,2) -R(3,3);
          q = [R(1,2)+R(2,1), t, R(2,3)+R(3,2), R(3,1)-R(1,3)];
      end
  else
      if (R(1,1) < -R(2,2))
          t = 1 -R(1,1) -R(2,2) + R(3,3);
          q = [R(3,1)+R(1,3), R(2,3)+R(3,2), t, R(1,2)-R(2,1)];
      else
          t = 1 + R(1,1) + R(2,2) + R(3,3);
          q = [R(2,3)-R(3,2), R(3,1)-R(1,3), R(1,2)-R(2,1), t];
      end
  end
  q = q * 0.5 / sqrt(t);
end
</code>```

Body:
Where did you get those equations from for the conversion to quaternion? I went to this question and implemented two of the answers there in Matlab (Octave), and both answers gave the same results:

>> Qd2
Qd2 =

   0.10469  -0.26218  -0.26218   0.65670

>> Qe2
Qe2 =

   0.65670  -0.26218  -0.26218   0.65670

I converted this answer, and your question meets the (b1_squared >= 0.25) criteria, so I could use it, but it's not suitable for all conversions because not all cases are implemented there ("I will leave it as an exercise to the OP to fill out the other three.")

Then I converted this answer and got the same results. That answer does apply to all conversions, so I'll give it to you here:

function q = MakeQFromR(R)
  if (R(3,3) < 0)
      if (R(1,1) >R(2,2))
          t = 1 + R(1,1) -R(2,2) -R(3,3);
          q = [t, R(1,2)+R(2,1), R(3,1)+R(1,3), R(2,3)-R(3,2)];
      else
          t = 1 -R(1,1) + R(2,2) -R(3,3);
          q = [R(1,2)+R(2,1), t, R(2,3)+R(3,2), R(3,1)-R(1,3)];
      end
  else
      if (R(1,1) < -R(2,2))
          t = 1 -R(1,1) -R(2,2) + R(3,3);
          q = [R(3,1)+R(1,3), R(2,3)+R(3,2), t, R(1,2)-R(2,1)];
      else
          t = 1 + R(1,1) + R(2,2) + R(3,3);
          q = [R(2,3)-R(3,2), R(3,1)-R(1,3), R(1,2)-R(2,1), t];
      end
  end
  q = q * 0.5 / sqrt(t);
end


问:
How to compute the force applied to point P given the torque registered by the sensor?
I have the following set up:

I want to understand how to compute the force that is being applied to the F_knife point given the Force/Torque registered by the sensor at T_Sensor. The position vector of the point F_Knife relative to T_Sensor is [0.21, 1.58, 0]. For simplicity, I want to assume that there is no gravity so there is only an external force applied to the point F_Knife in any direction. Also, I assume that the point where the gripper holds the knife is rigid.

答:
If everything is stationary, then the sum of linear forces must be zero and the sum of torques must also be zero. If they're not zero, then you have a non-zero net force or torque, which means there will be some linear or angular acceleration, respectively.

If you have some transform $T_{knife}$ that expresses the position and orientation (pose) of your knife frame, then you can convert applied forces in the knife frame back to the sensor frame by using the inverse transform $T_{knife}^{-1}$.

Once everything is in the sensor frame, the resulting linear forces must be equal (or opposite, depending on the polarity of your sensor). $F_x$ must equal $F_x$, etc., or again there is some non-zero force in the x-direction, and you'd have linear acceleration in the x-direction.

Find the torque, $\vec{r} \times \vec{F}_{applied}$, where $\vec{r}$ is the location of the knife frame in sensor coordinates, and again your sensor must detect equal/opposite torques.

If the knife is not stationary then it gets more complicated, in that you must also take the motion into account. The end of the robot arm must be providing enough force and torque to get the knife to move despite the applied force. That is,

$$
F_{base} = m_{knife}a_{knife} - F_{applied} \\
\tau_{base} = I_{knife}\alpha_{knife} - \left(\vec{r} \times \vec{F}_{applied}\right) \\
$$

If the signs are confusing, welcome to the club lol. It really depends a lot on how you set your datum, what you choose as positive, etc. The key with all of this is to be consistent. The best way for me personally is to assume everything moves in a particular direction. If you get a negative answer, it's moving in the other direction.

Also, FYI, the first case, where sum of all forces and torques is zero, is a semester-long mechanical engineering class usually called "statics," and the second case, where $\Sigma F = ma$ and $\Sigma \tau = I\alpha$, is a second semester-long mechanical engineering class called "dynamics."

That's basically it, those two equations, and they're a combined academic year of study, and those classes are famously so difficult that they convince a very large portion of the students to quit mechanical engineering altogether.


答:
I found the error, the rotation matrix Rd is not a rotation matrix because its determinant is equal to -1:

```<code>det(Rd) = -1
</code>```

I'm so sorry for your time! Forgive me!

Title: How to compute orientation error for a Puma end effector

Body:
i hope that you'll be ok!

I'm trying to control a Puma robot using the inverse kinematics in Matlab.
I used unit quaternions to express the orientation of End Effector frame and orientation of desired frame from Rotation matrix, like in Sciavicco-Siciliano.
This is the code:

```<code>Rd = [-sin(beta)  0  cos(beta);  % desired orientation matrix of ee
      cos(beta)  0  sin(beta);
      0         -1  0        ];

Re = H0e(1:3,1:3); % orientation matrix of EE from direct kinematics

u0 = @(R) 0.5* sqrt(1+R(1,1)+R(2,2)+R(3,3));
u1 = @(R) 0.5* sign(R(3,2)-R(2,3)) * sqrt(1+R(1,1)-R(2,2)-R(3,3));
u2 = @(R) 0.5* sign(R(1,3)-R(3,1)) * sqrt(1-R(1,1)+R(2,2)-R(3,3));
u3 = @(R) 0.5* sign(R(2,1)-R(1,2)) * sqrt(1-R(1,1)-R(2,2)+R(3,3));
S = @(x,y,z) [0 -z y; z 0 -x; -y x 0];

Qd = [u0(Rd) u1(Rd) u2(Rd) u3(Rd)]';  % {eta_d, epsilon_d} desired unit quaternion
Qe = [u0(Re) u1(Re) u2(Re) u3(Re)]';  % {eta_e, epsilon_e} unit quaternion of ee
</code>```

The problem is that two different rotation matrix produce the same unit quaternion and therefore the error is zero.

These are the two matrices:

```<code>Rd =

0.7250         0    0.6887
0.6887         0   -0.7250
     0   -1.0000         0

Re =

0.7250   -0.6887    0.0000
-0.0000   -0.0000    1.0000
-0.6887   -0.7250   -0.0000
</code>```

Where am i doing wrong? Could you help me?

Thank a lot!

Body:
I found the error, the rotation matrix Rd is not a rotation matrix because its determinant is equal to -1:

det(Rd) = -1

I'm so sorry for your time! Forgive me!


问:
Launch test for non-terminating processes
I'm using ROS2 Humble and Ubuntu 22.04 (through Docker, ros:humble).
I'm writing a system_tests package to run integration tests for my stack, which has features like teleop, mapping, navigation etc. The stack is of course a continuously running process.

I've found that the launch_testing package was made for integration tests, and it has a lot of examples, but it doesn't mention non-terminating processes.
I tried to include the bringup launch file which launches my stack correctly but since it never terminates on its own, the test never returns.

Has anyone ever managed to implement these end-to-end / integration tests in ros?

Here is my minimal non-working example

import rclpy

from threading import Thread
import unittest
import pytest
import launch_testing

from ament_index_python.packages \
    import get_package_share_directory as package_dir

from launch import LaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.actions import IncludeLaunchDescription

from my_interfaces.srv import MyMessage

@pytest.mark.launch_test
def generate_test_description() -> LaunchDescription:
    return LaunchDescription([
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                package_dir('bringup'), 'bringup.launch.py')),

        launch_testing.actions.ReadyToTest(),
    ])

class TestMyCase(unittest.TestCase):

    @classmethod
    def setUpClass(cls):
        rclpy.init()

    @classmethod
    def tearDownClass(cls):
        rclpy.shutdown()

    def setUp(self) -> None:
        self.node = rclpy.create_node('test_my_case')
        self.spinner_thread = Thread(target=lambda: rclpy.spin(self.node))
        self.spinner_thread.start()

    def tearDown(self) -> None:
        self.node.destroy_node()

    def test_call_add_node(self) -> None:
        service_cli = self.node.create_client(MyMessage, '/my_service')

        self.assertTrue(service_cli.wait_for_service(10.),
                        "Service '/my_service' not available")

        req = MyMessage.Request()
        res: MyMessage.Response = service_cli.call(req)
        self.assertTrue(res.success, f'Service call failed: {res.message}')
```</span>
</code>```

答:
I found an answer to a related question which might be usable here: link.

Based on that answer:

launch_process = subprocess.Popen(
    ["ros2", "launch", relative_path_to_launch_file], text=True)

# Run tests while process is running, then
# get this process handle in the tearDown method
# after the tests have finished
# (or simply wait for a set amount of time)

launch_process.send_signal(SIGINT)
launch_process.wait(timeout=30)
```

Also, I found this issue about this exact same problem.

Body:
I found an answer to a related question which might be usable here: link.

Based on that answer:

launch_process = subprocess.Popen(
    ["ros2", "launch", relative_path_to_launch_file], text=True)

# Run tests while process is running, then
# get this process handle in the tearDown method
# after the tests have finished
# (or simply wait for a set amount of time)

launch_process.send_signal(SIGINT)
launch_process.wait(timeout=30)

Also, I found this issue about this exact same problem.


问:
URDF to SDF Frame of Reference Calculation for Base Joint Fixed vs Floating
I am trying to simulate a robot I do not want fixed to the Gazebo world I am spawning it into. However, when I make the joint between my base link and the world floating, I get unexpected behavior in the way children frames of reference are established.

To preface, here is all the relevant XACRO

```<code><?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="frye_arm_1">

  <xacro:include filename="/home/jackfrye/Documents/Robotics/workspace/src/frye_arm_1/urdf/base_wide.xacro" />
  <xacro:include filename="/home/jackfrye/Documents/Robotics/workspace/src/frye_arm_1/urdf/polou_4801.xacro" />

  <link name="world"></link>

  <joint name="world_to_base" type="floating">
      <parent link="world" />
  <child link="a_base_wide" />
  <origin  rpy="0 0 0" xyz="0 0 0" />
  </joint>

  <xacro:base_wide prefix="a" x="0.0" y="0.0" z="0.374" />

  <joint name="base_to_motor_body" type="fixed">
      <parent link="a_base_wide" />
      <child link="a_robot_body" />
      <origin  rpy="0 0 0" xyz="0 0 .359" />
  </joint>

  <xacro:polou_4801 prefix="a" x="0.0" y="0.0" z="0.0" phi="0.0" theta="0.0" psi="0.0" />
</robot>
</code>```

base_wide.xacro

```<code><?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="frye_arm_1">
  <xacro:macro name="base_wide" params="prefix x y z">

  <link name="<span class="math-container">${prefix}_base_wide">
    <visual>
      <origin xyz="$</span>{x} <span class="math-container">${y} $</span>{z}" rpy="0 0 0" />
      <geometry>
          <mesh filename="file:///home/jackfrye/Documents/Robotics/FryeArm1/Mechanical/ROS2_Prep/base_widen/base_widen_x10.dae"/>
      </geometry>
      <material name="blue"/>
    </visual>
    <collision>
      <origin xyz="<span class="math-container">${x} $</span>{y} <span class="math-container">${z}" rpy="0 0 0"/>
      <geometry>
          <mesh filename="file:///home/jackfrye/Documents/Robotics/FryeArm1/Mechanical/ROS2_Prep/base_widen/base_widen_x10.dae"/>
      </geometry>
    </collision>
    <inertial>
        <origin xyz="$</span>{x} <span class="math-container">${y} $</span>{z}" rpy="0 0 0"/>
        <mass value="131.0971" />
        <inertia ixx="0.1811" ixy="0.0" ixz="0.0" iyy="0.1809" iyz="0.0002" izz="0.18" />
    </inertial>
  </link>
  </xacro:macro>
</robot>
</code>```

polou_4801.xacro

```<code><?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="frye_arm_1">
  <xacro:macro name="polou_4801" params="prefix x y z phi theta psi">

    <link name="<span class="math-container">${prefix}_robot_body">
    <visual>
      <origin xyz="$</span>{x} <span class="math-container">${y} $</span>{z}" rpy="<span class="math-container">${phi} $</span>{theta} <span class="math-container">${psi}" />
      <geometry>
        <mesh filename="file:///home/jackfrye/Documents/Robotics/FryeArm1/Mechanical/ROS2_Prep/polou_4801_body/polou_4801_body_x10_no_wire.dae"/>
      </geometry>
      <material name="blue"/>
    </visual>
    <collision>
      <origin xyz="$</span>{x} <span class="math-container">${y} $</span>{z}" rpy="<span class="math-container">${phi} $</span>{theta} <span class="math-container">${psi}" />
      <geometry>
          <mesh filename="file://file:///home/jackfrye/Documents/Robotics/FryeArm1/Mechanical/ROS2_Prep/polou_4801_body/polou_4801_body_x10_no_wire.dae"/>
      </geometry>
    </collision>
    <inertial>
        <origin xyz="$</span>{x} <span class="math-container">${y} $</span>{z}" rpy="<span class="math-container">${phi} $</span>{theta} ${psi}" />
        <mass value="94" />
        <inertia ixx="3.4438" ixy="0.0" ixz="0.0" iyy="3.4438" iyz="0.0" izz="0.7338" />
    </inertial>
  </link>

  <joint name="<span class="math-container">${prefix}_body_to_rotor" type="revolute">
      <parent link="$</span>{prefix}_robot_body" />
      <child link="<span class="math-container">${prefix}_robot_rotor" />
      <axis xyz="0 0 1" />
<!--      <origin  xyz="$</span>{x} <span class="math-container">${y} $</span>{z}" rpy="<span class="math-container">${phi} $</span>{theta} <span class="math-container">${psi}" /> -->
         <origin  xyz="0 0 0.374" rpy="0 0 0" />
      <limit lower="0" upper="$</span>{4*pi}" effort="10" velocity="3"/>
  </joint>

  <link name="<span class="math-container">${prefix}_robot_rotor">
    <visual>
<!--      <origin xyz="$</span>{x} <span class="math-container">${y} $</span>{z+0.75}" rpy="<span class="math-container">${phi} $</span>{theta} <span class="math-container">${psi}" /> -->
      <origin xyz="$</span>{x} <span class="math-container">${y} $</span>{z}" rpy="0 0 0" />
      <geometry>
          <mesh filename="file:///home/jackfrye/Documents/Robotics/FryeArm1/Mechanical/ROS2_Prep/polou_4801_shaft/polou_4801_shaft_x10.dae"/>
      </geometry>
      <material name="blue"/>
    </visual>
    <collision>
<!--      <origin xyz="<span class="math-container">${x} $</span>{y} <span class="math-container">${z+0.75}" rpy="$</span>{phi} <span class="math-container">${theta} $</span>{psi}" /> -->
      <origin xyz="<span class="math-container">${x} $</span>{y} <span class="math-container">${z}" rpy="0 0 0" />
      <geometry>
          <mesh filename="file:///home/jackfrye/Documents/Robotics/FryeArm1/Mechanical/ROS2_Prep/polou_4801_shaft/polou_4801_shaft_x10.dae"/>
      </geometry>
    </collision>
    <inertial>
<!--      <origin xyz="$</span>{x} <span class="math-container">${y} $</span>{z+0.75}" rpy="<span class="math-container">${phi} $</span>{theta} <span class="math-container">${psi}" /> -->
      <origin xyz="$</span>{x} <span class="math-container">${y} $</span>{z}" rpy="0 0 0" />
        <mass value=".05" />
        <inertia ixx="0.00000" ixy="0.0" ixz="0.0" iyy="0.0000" iyz="0.0" izz="0.0000" />
    </inertial>
   </link>
  </xacro:macro>

</robot>
</code>```

Now in the top level for the joint "world_to_base", when I used floating, which is the setting I want to use, it seems like the transforms are not being done as expected. If I say I want to make a joint at 0.374 +Z, that means I want that joint at +.374 from the parent's frame of reference. If I create a floating joint the move the child +Z .359, that should create the frame of reference from which the joint is created. In that case, the joint would then be at +.359+.374 in the "world" frame. When I do a floating joint, I do not see this behavior.

However, when I used fixed for the connection between my base link and world, I do see this relationship in the link-joint tree.

It makes it difficult to write portable XACRO if I cannot assume that my make first joint in the XACRO (eg the motor body joint) is going to be offset WRT to its parent not and not world. Maybe I just need to refactor and add logic into the polou xacro that takes into account that all joints are being layed out wrt the world frame, but I would prefer not to and just have it calculate like it does for the "fixed" case.

答:
In my experience, I've only seen the pattern where you add a world link to the URDF to affix the robot to the world. If you want it to be floating, you don't need the world link at all, just start from a_base_wide. There is no floating joint in SDF since that is equivalent to not having a joint (https://answers.gazebosim.org/question/14728/what-is-a-floating-joint-physically/?answer=14737#post-id-14737). If you still need to offset the whole robot when it's spawned, you can use the <gazebo> tag to directly insert a <pose> tag to the resulting model.

  <gazebo>
    <pose>1 0 0  0 0 0</pose>
  </gazebo>
```

I haven't tested that so it may or may not work. Alternatively, you can change your launch script to spawn the robot at a desired pose.

Body:
In my experience, I've only seen the pattern where you add a world link to the URDF to affix the robot to the world. If you want it to be floating, you don't need the world link at all, just start from a_base_wide. There is no floating joint in SDF since that is equivalent to not having a joint (https://answers.gazebosim.org/question/14728/what-is-a-floating-joint-physically/?answer=14737#post-id-14737). If you still need to offset the whole robot when it's spawned, you can use the <gazebo> tag to directly insert a <pose> tag to the resulting model.

  <gazebo>
    <pose>1 0 0  0 0 0</pose>
  </gazebo>

I haven't tested that so it may or may not work. Alternatively, you can change your launch script to spawn the robot at a desired pose.


答:
Waqas, your question is a little vague. So the simple answer is of course why not. But do consider the following:

What type of RC helicopter is it?

RC helicopters come in many shape and sizes, and they require special mixing for each type. For example a collective pitch/roll helicopter uses different mixing than a non collective type RC. So given this you will have to think about how many different types do you wish to support.

In some cases the mixing is done onboard the helicopters most commonly known as FlightControllers(FC). At the same time some advanced Transmitters also do certain pre-mixing before sending commands to the FC.

How do I connect to it?

Again the possibilities here are endless. You can tether to it from your laptop, but then you are limited to how far it can go. How about instead you use an onboard controller which you can control using WiFi or bluetooth, try http://www.buildyourowndrone.co.uk/ArduPilot-Mega-s/36.htm. You could build a program to send commands to your ArduPilot over UDP.

How do I start tinkering?

Try maybe starting small, try controlling and RC car, trust me it is a lot easier to get past the initial problems, and you will experience the problems in easier dimensions.

See which parts you can pull out and replace with your application

Try to turn you PC into a transmitter

When ready replace the Car with the Helicopter

If you wish try replacing the FC onboard the Helicopter with your custom version should you choose to.

I am for learning actually doing something similar, by building an onboard framework use to control many types of RC vehicle http://dashee.co.uk. This is very much a work in progress, so I recommend you use ArduPilot to control an RC over Radio/Wifi using your .NET program.

Title: RC Helicopter Connect with computer

Body:
I am planing to control my RC helicopter with my computer. I have experience of programming in .Net. Could we use .Net to control RC helicopter?
From where can I start this project?</p>


问:
Compute tranformation matrix for odom to base_link depending on tracking camera attached relative to base_link
To complete my tf tree, I must broadcast transformation from odom to base_link. I am receiving position from the tracking camera, which is attached to the camera mount relative to base_link.
So, I have a pose from the tracking_camera_frame
and I know static transform between base_link and tracking_camera_frame

```<code>map
 |
 |
odom
 |
 | ?
 +------base_link
            |
            |
            +----camera_mount
                        |
                        +----tracking_camera_frame
</code>```

I don't know how to express the transformation odom to base_link if I have two mentioned transformations.

When I read about it on the internet, there is always only an example that with multiplication, you can express a point from one coordinate system relative to another.

However, I probably found a solution in the ZED wrapper for ROS, but I don't understand it.

```<code>tf2::Transform deltaOdomTf_base = mSensor2BaseTransf.inverse() * deltaOdomTf * mSensor2BaseTransf;
</code>```

According to this, my example should be

```<code>odom_to_base_link = base_link_to_tracking_camera_frame.inverse() * pose * base_link_to_tracking_camera_frame
</code>```

I would like to understand the background of this calculation. Why is it so, or what is the correct form?

Update:

I asked chatGPT, and it told me that for equation resultTF = deltaTF^-1 * pose * deltaTF, the deltaTF will be effectively cancelled, and the result will be directly related to parent TF. It is what I want. However, what is the mathematical background of this? I want to understand it step by step.
It looks like resultTF == pose, but this is not true. So why must these matrices be applied in this order?

答:
One way is with a utility node I made called 'old_tf_to_new_tf' which is configured with four frames: lookup_parent lookup_child, broadcast_parent, and broadcast_child- the first 3 need to exist in your system already, there needs to be an existing chain of transforms between the two lookup frames (they can be parents/childs/distant cousins in either position as long as there is a connection), then broadcast_child is published by the old_tf_to_new_tf node.

For your case you would want a second tracking_camera_frame, call it tracking_camera_frame_zed or something else distinct from the one that probably came out of your urdf or similar, and configure the zed node to publish 'odom -> tracking_camera_frame_zed' when it does visual odometry (or whatever, just assuming that based on your reference to zed cameras).  Then the old to new tf node would have:

```<code>lookup_parent:=tracking_camera_frame
lookup_child:=base_link
broadcast_parent:=tracking_camera_frame_zed
broadcast_child:=base_link
</code>```

It does a lookup transform with the two lookup frames, then re-publishes the returned transform out but with the provided broadcast parent and child frames replacing the looked-up ones, mirroring the transform from one part of a tree to another part.

https://github.com/lucasw/tf_demo/blob/old_tf_to_new_tf/src/old_tf_to_new_tf.cpp

There's an example launch file in the package too, trying that out and looking at the tf tree in rviz may help it more clear.

It's possible to call it directly inside your C++ node  (it sounds like you have one) instead of having a separate node, make your own CopyTransform instance like in the cpp source linked to above.

Body:
One way is with a utility node I made called 'old_tf_to_new_tf' which is configured with four frames: lookup_parent lookup_child, broadcast_parent, and broadcast_child- the first 3 need to exist in your system already, there needs to be an existing chain of transforms between the two lookup frames (they can be parents/childs/distant cousins in either position as long as there is a connection), then broadcast_child is published by the old_tf_to_new_tf node.

For your case you would want a second tracking_camera_frame, call it tracking_camera_frame_zed or something else distinct from the one that probably came out of your urdf or similar, and configure the zed node to publish 'odom -> tracking_camera_frame_zed' when it does visual odometry (or whatever, just assuming that based on your reference to zed cameras).  Then the old to new tf node would have:

lookup_parent:=tracking_camera_frame
lookup_child:=base_link
broadcast_parent:=tracking_camera_frame_zed
broadcast_child:=base_link

It does a lookup transform with the two lookup frames, then re-publishes the returned transform out but with the provided broadcast parent and child frames replacing the looked-up ones, mirroring the transform from one part of a tree to another part.

https://github.com/lucasw/tf_demo/blob/old_tf_to_new_tf/src/old_tf_to_new_tf.cpp

There's an example launch file in the package too, trying that out and looking at the tf tree in rviz may help it more clear.

It's possible to call it directly inside your C++ node  (it sounds like you have one) instead of having a separate node, make your own CopyTransform instance like in the cpp source linked to above.


问:
ROS2 robot_localization not working correctly with namespace
I have a straightforward ROS2 launch script that launches the following:

gazebo client, gazebo server and spawn_entity to spawn a robot in gazebo

robot_state_publisher and map->odom static publisher

robot_localization

Nav2

Everything starts up correctly. However, when set a namespace to be used for all the entities in the launch file, robot_localization stops functioning correctly. Specifically, it does not subscribe to the topics /namespace/odom or /namespace/imu (The robot published both these messages correctly).

Any idea why?

Here are parts of the launch script for reference

```<code>remappings = [('/tf', 'tf'),
              ('/tf_static', 'tf_static')]

robot_localization_node = launch_ros.actions.Node(
    package = 'robot_localization',
    executable = 'ekf_node',
    name = 'ekf_filter_node',
    output = 'screen',
    parameters = [os.path.join(pkg_share, 'config', 'ekf.yaml'),
                  {'use_sim_time': use_sim_time}],
    remappings = remappings,
    namespace = namespace
)

#note: if 'use_composition' is True, the nav2 stack will not start if a namespace is given
nav2_launch = IncludeLaunchDescription(
    PythonLaunchDescriptionSource(os.path.join(nav2_launch_dir, 'bringup_launch.py')),
    launch_arguments = {'map': static_map_path,
                        'params_file': nav2_params_path,
                        'use_sim_time': use_sim_time,
                        'autostart': autostart_nav2,
                        'namespace': namespace,
                        'use_namespace': 'True',
                        'use_composition': 'False'}.items()
)
</code>```

And below are the robot_localization configurations used for the transforms and the sensors

```<code>frequency: 30.0
publish_tf: true
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: odom

odom0: odom
odom0_config: [true,  true,  true,
               false, false, false,
               false, false, false,
               false, false, true,
               false, false, false]

imu0: imu
imu0_config: [false, false, false,
              true,  true,  true,
              false, false, false,
              false, false, false,
              false, false, false]
</code>```

答: You need to change the subscriptions in RL node config. The lines that state, "odom0: odom" should include the namespace, like namespace/odom</p>


问:
Transformation a robot in 2D?
I'm watching this video at 36.00 min. The guy gave an example but I'm not sure what is the problem. He stated that if we want to move a robot then we should to the following

for inhomogeneous case,
$$
x' = Rx + t \\
R =
\begin{bmatrix}
cos\theta & -sin\theta \\
sin\theta  & cos\theta
\end{bmatrix}
$$
where $t$ is the translation vector and $x$ is the previous position.

in homogeneous case,
$$
x' =
\begin{bmatrix}
R & \bf{t} \\
\bf{0}^{T} & 1
\end{bmatrix}
x
$$
Now, he gave an example in which
$$
t =
\begin{bmatrix}
1 \\
0
\end{bmatrix}
,
x =
\begin{bmatrix}
0.7 \\
0.5
\end{bmatrix}
$$
My solution is as the following in Matlab

```<code>% inhomogeneous case
>> a = 45;
>> R = [cosd(a) -sind(a); sind(a) cosd(a)];
>> t = [1; 0];
>> x = [0.7; 0.5];
>> xnew = R*x + t
xnew =

    1.1414
    0.8485
</code>```

For homogeneous case

```<code>>> xn = [R t; 0 0 1]*[x ; 1]

xn =

    1.1414
    0.8485
    1.0000
</code>```

Both have same result, but the guy got another result. What exactly he did is

```<code>>> xf = [R x; 0 0 1]*[t ; 1]

xf =

    1.4071
    1.2071
    1.0000
</code>```

Why he did switch $t$ and $x$? I'm aware of the issue that he is trying to calculate the velocity but in fact he is computing the position. This mistake in the notation won't affect the final result.

Second question, why he assumed that the forward movement of the robot in the above example should be
$$
t = \begin{bmatrix}1\\0\end{bmatrix}
$$
? He said that because the robot always in the forward movement move in +x axis. Why this is the case? The movement in robot's frame is determined based on the direction of the robot and the distance the robot travels which is specified as hypotenuse length.

答:
First of all thanx for sharing a good video
i have me first lecture of Computer vision tomorrow so i revised all the matrix and vector operations.
Its been a while when i last studied these things

OK your Second Question
The assumption is made to simplify the problem to understand .if we have a robot with too many variables changing at the same time it will become complicated.
So we assume just for the sake of simplicity
When i first studied we used to assume a simple robot just going forward to understand control theory.

Now the First Question
Why you answer is wrong

Simple Logic(just plot the values with a graph) :

 robot was at x,y(0.7,0.5).it then moves another 1 Meter forward .which will end up increasing x and y eventually   :)
If your answer is right then the increased difference in your answer will give answer 1 by Pythagoras theorem

```<code>a^2 + b^2 = c^2
</code>```

We cannot apply this  x′=[R0Tt1]x    formula straight forward because the teacher in the video said the distance of 1 meter it traveled is Local.

To change that local distance to global he uses the method Matrix multiplication with Vector.

There can be a lot more dicussion that can be done in solving these kind of questions.thats why i am posting the answer to see your response

Title: Transformation a robot in 2D?

Body:
First of all thanx for sharing a good video
i have me first lecture of Computer vision tomorrow so i revised all the matrix and vector operations.
Its been a while when i last studied these things

OK your Second Question
The assumption is made to simplify the problem to understand .if we have a robot with too many variables changing at the same time it will become complicated.
So we assume just for the sake of simplicity
When i first studied we used to assume a simple robot just going forward to understand control theory.

Now the First Question
Why you answer is wrong

Simple Logic(just plot the values with a graph) :

 robot was at x,y(0.7,0.5).it then moves another 1 Meter forward .which will end up increasing x and y eventually   :)
If your answer is right then the increased difference in your answer will give answer 1 by Pythagoras theorem

a^2 + b^2 = c^2

We cannot apply this  x′=[R0Tt1]x    formula straight forward because the teacher in the video said the distance of 1 meter it traveled is Local.

To change that local distance to global he uses the method Matrix multiplication with Vector.

There can be a lot more dicussion that can be done in solving these kind of questions.thats why i am posting the answer to see your response


问:
Webots: Spawn multiple nodes and disable collision between them, but not the environment
In Webots, I am spawning 10 vehicle nodes at the same time and position for reinforcement learning. I would like the vehicles to not interact with each other (collision and LIDAR) but still have physics to collide with the environment.

So my question:

Is there provision in Webots for disabling collision and LIDAR point cloud between certain nodes (so that they can pass through each other), but retain physics to interact with the environment?

答:
Yes, it is possible to disable collision and LIDAR detection with a specific node in Webots.

These are two different tasks that can be achieved with the following solutions.

Disable collision

To control the collision detection you have to write a physics plugin.
At this link you can find all the information about how to set it up.

The main point is to overwrite the webots_physics_collide(dGeomID g1, dGeomID g2) callback function, and if both the geometries g1 and g2 belong to a vehicle node then you just have to return 1 to tell the physics engine to ignore the collision.

The biggest development effort in your physics plugin will be to detect if an ODE geometry belongs to a vehicle.
In a physics plugin you can use all the ODE API functions (ODE is the Webots' physics engine).
In particular you might be interested in these funtions to get the ODE body from a dGeomId, check if two bodies are connected by a joint, and get a ODE body/geom given a Webots DEF name.

// ODE functions
dBodyID dGeomGetBody (dGeomID)
int dAreConnected (dBodyID, dBodyID);
// Webots utility functions
dBodyID dWebotsGetBodyFromDEF(const char *DEF);
dGeomID dWebotsGetGeomFromDEF(const char *DEF);
```
<p>Here you can find an example of pyhsics plugin just to have an idea about how to structure it.
However, note that the functionality implemented in the<code> webots_physics_collide(dGeomID g1, dGeomID g2)</code> differs from what you would like to achieve.</p>
<hr />
<p><strong>Disable LIDAR detetion</strong></p>
<p>You can use the Supervisor function <code>wb_supervisor_node_set_visibility</code>.</p>
<p>So, in the controller where you are spawning the vehicles you would have to add something like this (just pseudo code)</p>
<pre class="lang-c prettyprint-override"><code>foreach (string vehicle_name, vehicles) {
  WbNodeRef vehicle_node = wb_supervisor_node_get_def(vehicle_name);
  foreach (string other_vehicle_name, vehicles) {
    if (vehicle_name == other_vehicle_name)
      continue;
    // assuming expects the vehicle has a unique DEF name and
    // the LIDAR is visible in the Webots scene tree with DEF name 'LIDAR'
    WbNodeRef lidar_node = wb_supervisor_node_get_def(vehicle_name + '.LIDAR');
    wb_supervisor_node_set_visibility(vehicle_node, lidar_node);
}
</code>```

Disclaimer: I am a Webots developer working at Cyberbotics.

Body:
Yes, it is possible to disable collision and LIDAR detection with a specific node in Webots.

These are two different tasks that can be achieved with the following solutions.

Disable collision

To control the collision detection you have to write a physics plugin.
At this link you can find all the information about how to set it up.

The main point is to overwrite the webots_physics_collide(dGeomID g1, dGeomID g2) callback function, and if both the geometries g1 and g2 belong to a vehicle node then you just have to return 1 to tell the physics engine to ignore the collision.

The biggest development effort in your physics plugin will be to detect if an ODE geometry belongs to a vehicle.
In a physics plugin you can use all the ODE API functions (ODE is the Webots' physics engine).
In particular you might be interested in these funtions to get the ODE body from a dGeomId, check if two bodies are connected by a joint, and get a ODE body/geom given a Webots DEF name.

// ODE functions
dBodyID dGeomGetBody (dGeomID)
int dAreConnected (dBodyID, dBodyID);
// Webots utility functions
dBodyID dWebotsGetBodyFromDEF(const char *DEF);
dGeomID dWebotsGetGeomFromDEF(const char *DEF);

Here you can find an example of pyhsics plugin just to have an idea about how to structure it.
However, note that the functionality implemented in the webots_physics_collide(dGeomID g1, dGeomID g2) differs from what you would like to achieve.

Disable LIDAR detetion

You can use the Supervisor function wb_supervisor_node_set_visibility.

So, in the controller where you are spawning the vehicles you would have to add something like this (just pseudo code)

foreach (string vehicle_name, vehicles) {
  WbNodeRef vehicle_node = wb_supervisor_node_get_def(vehicle_name);
  foreach (string other_vehicle_name, vehicles) {
    if (vehicle_name == other_vehicle_name)
      continue;
    // assuming expects the vehicle has a unique DEF name and
    // the LIDAR is visible in the Webots scene tree with DEF name 'LIDAR'
    WbNodeRef lidar_node = wb_supervisor_node_get_def(vehicle_name + '.LIDAR');
    wb_supervisor_node_set_visibility(vehicle_node, lidar_node);
}

Disclaimer: I am a Webots developer working at Cyberbotics.


问:
How to measure pull force on a (kite) rope?
I'm building a kite flying robot, in which I've already got some sensors built in. I now also want to measure the pulling force on the rope which is attached to the kite. So I thought of attaching a hanging digital scale to the rope which can then measure the pulling force in kilograms. I of course need to read out the data from the scale using GPIO pins or USB.

Furthermore, I also have a raspberry pi installed within the kite and I want to measure some pull on ropes in the air. So if possible, I also need very small/light scales from which I can read out the data. It would be great if the scales can measure up to about 50kg, but with some pulleys about 20kg would be fine as well.

Unfortunately I can't find any digital hanging scale with a possibility to read out the data and which is reasonably light. There are some simple USB powered hanging scales, but I think they just use USB for charging, not for reading out data. And I also found this one, but that's a bit overkill (too heavy and too expensive).

So question #1: Does anybody know where I can get a simple existing hanging scale from which I can read out data?

If needed I can also build something, but I just wouldn't know where to start. I did find this page on Alibaba, where they show the contents of the scale they offer:

So as far as I understand I need the component which I highlighted. But I have no idea how that component is called (what do I search for?) and whether it is actually doable to read it out from a Raspberry Pi.

Question #2: does anybody know what the highlighted component is called, where I could possibly get it and if I can read it out from a Raspberry Pi?

In conclusion; can anybody point me in the right direction?

答: That is a load cell, comprised of four strain gages in a Wheatstone bridge, probably mounted in sheer mode to cancel out bending forces.</p>


答: You can examine Linorobot2 and see what he uses, as he provides code for 4x4 Mechanum drive with PID controller.</p>

Title: How to control a 4 wheel mechanum wheel robot with PID?

Body:
I have a robot with 4 mechanum wheels. I implemented a pid controller on each wheel and as a result it doesn't drive straight. When I send a command to simply move in the x direction forward, it ends up doing an arc.

Obviously my wheels/motors aren't identical and I can't have the same PID tuning parameters for all of them. I tried to tune all 4 controllers manually and gave up, because I wasn't able to get a consistent result.

So now I am trying a different approach where I want to tune the linear and angular velocities individually. I also split up the linear velocity into x and y. The parts in green are the ones that I'm currently working on.

Does this approach make sense?
One thing that doesn't seem right to me is that in the inverse kinematic equation I convert the set point to a PWM value that I then write to the robot. Should there be an additional PID controller that is only working on setting a predetermined RPM to the motor?

Also is there a way to avoid including a low pass filter? I had to implement one because the speed is calculated from quadrature encoders and it was too noisy.

If anyone has any thoughts on this mechanum wheel pid control, any help would be greatly appreciated. Thank you.


问:
Robot loses proper localization
Having trouble getting robot to stay localized in map. The lidar maps the correct position of robot but not the odometry or ekf tf. One can see the two positions on graphic below.

The robot base is Linorobot2 with differential drive and I'm using Nav2 and Humble. I'm running Rtabmap on the robot without the rtabmapviz node and it publishes the Velodyne puck ICP odometry. The host runs the complete Nav2 stack and SLAM Toolbox with Rviz2.

The Gazebo sim works well for mapping and navigation but the real robot has this trouble, as shown in graphic.

I tried feeding the ICP odom into the EKF node also but it just made things worse, I had to change it back to just the IMU and wheel odometry going into the EKF node.

What is the correct way to troubleshoot this setup? Should I adjust the encoder ticks per meter? I have never seen the amcl arrows, though it seems to be running, as it sometimes posts warnings.

From launch file:

```<code>    Node(
        package='rtabmap_ros', executable='icp_odometry', output='screen',
        parameters=[{
          'frame_id':'velodyne',
          'odom_frame_id':'odom',
          'wait_for_transform':0.2,
          'expected_update_rate':15.0,
          'deskewing':deskewing,
          'use_sim_time':use_sim_time,
        }],
        remappings=[
          ('scan_cloud', '/velodyne_points'),
          ('odom', 'odom_icp')
        ],
        arguments=[
          'Icp/PointToPlane', 'true',
          'Icp/Iterations', '10',
          'Icp/VoxelSize', '0.1',
          'Icp/Epsilon', '0.001',
          'Icp/PointToPlaneK', '20',
          'Icp/PointToPlaneRadius', '0',
          'Icp/MaxTranslation', '2',
          'Icp/MaxCorrespondenceDistance', '1',
          'Icp/Strategy', '1',
          'Icp/OutlierRatio', '0.7',
          'Icp/CorrespondenceRatio', '0.01',
          'Odom/ScanKeyFrameThr', '0.6',
          'OdomF2M/ScanSubtractRadius', '0.1',
          'OdomF2M/ScanMaxSize', '15000',
          'OdomF2M/BundleAdjustment', 'false',
        ]),
</code>```

ekf_filter_node:
ros__parameters:

```<code>    frequency: 50.0

    two_d_mode: true

    publish_tf: true

    use_control: false

    map_frame: map
    odom_frame: odom
    base_link_frame: base_footprint
    world_frame: odom

    #x     , y     , z,
    #roll  , pitch , yaw,
    #vx    , vy    , vz,
    #vroll , vpitch, vyaw,
    #ax    , ay    , az

    # from Robbie's encoders
    odom0: odom/unfiltered
    odom0_config: [false, false, false,
                   false, false, false,
                   true, true, false,
                   false, false, true,
                   false, false, false]

    # from ICP module of Rtabmap
    odom1: odom_icp
    odom0_config: [true, true, false,
                   false, false, true,
                   false, false, false,
                   false, false, false,
                   false, false, false]

    # from MPU 9250
    imu0: imu/data
    imu0_config: [false, false, false,
                  false, false, false,
                  false, false, false,
                  false, false, true,
                  true, false, false]
</code>```

[component_container_isolated-1] [INFO] [1677951250.137225667] [amcl]: Message Filter dropping message: frame 'velodyne' at time 1677951249.140 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[component_container_isolated-1] [INFO] [1677951250.208334876] [local_costmap.local_costmap]: Timed out waiting for transform from base_link to odom to become available, tf error: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist
[component_container_isolated-1] [INFO] [1677951250.237334995] [amcl]: Message Filter dropping message: frame 'velodyne' at time 1677951249.240 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[component_container_isolated-1] [INFO] [1677951250.366020161] [amcl]: Message Filter dropping message: frame 'velodyne' at time 1677951249.340 for reason 'discarding message because the queue is full'
[component_container_isolated-1] [INFO] [1677951250.456398646] [amcl]: Message Filter dropping message: frame 'velodyne' at time 1677951249.441 for reason 'discarding message because the queue is full'
[component_container_isolated-1] [INFO] [1677951250.541983751] [amcl]: Message Filter dropping message: frame 'velodyne' at time 1677951249.540 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[component_container_isolated-1] [INFO] [1677951250.666734227] [amcl]: Message Filter dropping message: frame 'velodyne' at time 1677951249.643 for reason 'discarding message because the queue is full'
[component_container_isolated-1] [INFO] [1677951250.708331804] [local_costmap.local_costmap]: Timed out waiting for transform from base_link to odom to become available, tf error: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist
[component_container_isolated-1] [INFO] [1677951250.744652632] [amcl]: Message Filter dropping message: frame 'velodyne' at time 1677951249.739 for reason 'discarding message because the queue is full'

答:
I don't see green points from AMCl, are you sure it is running correctly?
What type of warning do you see from it? Can you add them to the post?

What are your parameters for ICP and the rest in the EKF? Can you show them? Maybe something there could be tinkered with to help.

Title: Robot loses proper localization


问:
SCARA robot configuration subtype (first axis vertical)
I'm working on a project where I need a SCARA where the first axis is the linear / vertical one (as often seen in hobbyist configurations). Something like this:

https://www.igus.eu/product/21104?C=DE&L=en&artNr=RL-SCR-0101

I'm trying to find similar robots (or even a horizontal arm by itself - e. g. this https://www.igus.eu/product/21105?C=DE&L=en&artNr=RL-SCR-0102) with similar specs to compare. It won't be lifting heavy loads (max. 500 g) and won't need to go fast at all. It will need a pretty large vertical range though (up to 2 m depending on the final specs of the project). That's why I was thinking about going with an arm like the mentioned one bolted to a linear rail.

Does anyone have some recommendations or maybe just some keywords with which I could find something similar.
If you need any additional information let me know.

Thanks in advance.

答:
Staubli used to make a robot arm called the TS20 or earlier RS20 which was a small 20 cm reach scara with the vertical axis over the base instead of on the end of the arm. It was small for a commercial arm and had a light payload, nominal 0.5 KG maximum 1 KG. It was intended for the education market, schools and universities. The sales numbers were low and Staubli stop making them a couple of years ago.

You see an image at https://www.automate.org/products/staubli-robotics/ts20-scara-robot


问:
What's the difference between a sonar and ultrasonic sensor?
After having read the introductions of the Wikipedia articles about "sonar" and "ultrasonic sensor", it is still not clear to me what the differences between the two are. In the context of robotics, is there any difference between these two sensors (terms)?</p>

答:
SONAR was to find submarines under water. It used low frequency sound that was readily conducted by water. The sound was audible and could be heard. Similar to RADAR with SONAR the sound reflected from the submarine and the delay in the return allowed to determine the range or distance to the target. The emitter and microphones were directional and allow you to determine the bearing or direction to the target.

The low frequency sound did not work well in air and the emitter and microphones were not very directional. so newer versions where developed that used high frequency or ultrasonic sounds, beyond the frequencies humans can hear. These were more directional and worked better in air that older audible sonar.

More solid state emitters, microphones, and microprocessors have greatly lowered the cost making them common for small mobile robots to detect obstacles.


问:
Ros control pid gives different input than expected
I am trying to match the trajectory i am getting from gazebo simulation using ros_control and matlab for a simple pendulum which rotates in the horizontal plane (so no gravity, no friction and no mechanical reductions).

The Dynamics are governed by the following equation:
$$ I\ddot{q} = u $$
where u is the input signal, and $I = 2.4667 \ kgm^2$.

When $u(t) = u(0)$ (step input. I am using the joint_effort_controller in gazebo in this case) the simulations are close enough, as it can be seen from the following graph.

However when i use a pid to control the position (using an effort_controllers/JointPositionController) there are major differences. They can been seen in the following graph:

The culprit seems to be the input, as it is considerably different:

The pid gains are chosen as follows:

```<code>    pid: {p: 15, i: 0.00, d: 5.0}
</code>```

In theory, the initial command is ($q_d=1$, and $\{ q_0,\dot{q}_0 \} = \{0,0\}$ )
$$ u_0 = Kp(q_d-q_0) + Kd(0-\dot{q}_0) = 15+0 = 15Nm $$

Matlab gives the expected initial input, however, the commanded torque from ros_control is $u_0 \approx 4 Nm$.

Question
I cannot explain how the input command gets that value. And as a result how ros_control calculates the input signal. Any help, hint or idea about what causes this difference will be of great help.

Update
Without the derivate term, and only $K_p = 15$ the initial input matches the theoretical one, so $u_0  = 15Nm$. However, from the implementation as it is discussed below:

$$ p_{error} = p_{state}-p_{desired} -> e_{position,0} = -1;$$
$$ d_{error} = (p_{error}-p_{error_last})/T -> e_{velocity,0} = -1/T;$$
I would expect the initial input to be something like that:

$$ u_{ros,0} = -15(-1) -5(-1/T) = 15+5/T$$

Update 2
If the derivate gain is set to zero, and instead i add damping of 5Ns/rad, then the results between matlab and gazebo match. It can be seen in the next figure.

Extra1
I have to mention that looking the control tooblox API which is used by ros_control for the pid controller, the from of the PID controller is of the following form:

(see lines 331 in the pid.cpp where the function computeCommand is defined. )

Extra2
$$\dot{e}_{position} = \frac{d}{dt}(q_d-q) =  -\dot{q} $$

Extra3
The settings for the controller are:

```<code>  joint1_position_controller:
    type: effort_controllers/JointPositionController
    joint: base_to_link1
    pid: {p: 15, i: 0.00, d: 5.0}
    publish_rate: 50

</code>```

答:
There are two wrong things in the initial post:

1st The pid controller publishes its state every 10 calculations.
Inside the update command, there is the following code:

```<code>// publish state
   if (loop_count_ % 10 == 0)
   {
     if(controller_state_publisher_ && controller_state_publisher_->trylock())
     {
       controller_state_publisher_->msg_.header.stamp = time;
       ...// rest of the message
     }
   }
   loop_count_++;
</code>```

So the plot i was getting from the topic controller/state/ doesn't necessarily  give me the first command of the system. And from the maths in my question, it can be deduced that i wasn't getting the first command.

Actually, after a while, $|p_{error,i}| < |p_{error,i-1}|$ so $d_{error}>0$ so the $d_{term}$ was positive. Due to the timestep being $Dt=1ms$( something that you can get from the /controller/state/time_step), the $d_{term}$ was also quite large. These reasons resulted in the vastly different input histories of my original post.

2nd Ros control is a discrete digital controller, so the correct way to simulate it is not by using a continuous controller, like:
$$ u = K_p e + K_d \dot{e} = K_p (q_d-q) - K_d \dot{q}  $$

Using matlab simulink, i used a discrete pid controller (without filtering the derivative) and as expected the simulation results matched.

Also, the imput histories are very simillar (again we can see that i didn't get the initial values from the ros topics):

I also put the controller settings of simulink (K = [15,5]):

Body:
There are two wrong things in the initial post:

1st The pid controller publishes its state every 10 calculations.
Inside the update command, there is the following code:

// publish state
   if (loop_count_ % 10 == 0)
   {
     if(controller_state_publisher_ && controller_state_publisher_->trylock())
     {
       controller_state_publisher_->msg_.header.stamp = time;
       ...// rest of the message
     }
   }
   loop_count_++;

So the plot i was getting from the topic controller/state/ doesn't necessarily  give me the first command of the system. And from the maths in my question, it can be deduced that i wasn't getting the first command.

Actually, after a while, $|p_{error,i}| < |p_{error,i-1}|$ so $d_{error}>0$ so the $d_{term}$ was positive. Due to the timestep being $Dt=1ms$( something that you can get from the /controller/state/time_step), the $d_{term}$ was also quite large. These reasons resulted in the vastly different input histories of my original post.

2nd Ros control is a discrete digital controller, so the correct way to simulate it is not by using a continuous controller, like:
$$ u = K_p e + K_d \dot{e} = K_p (q_d-q) - K_d \dot{q}  $$

Using matlab simulink, i used a discrete pid controller (without filtering the derivative) and as expected the simulation results matched.

Also, the imput histories are very simillar (again we can see that i didn't get the initial values from the ros topics):

I also put the controller settings of simulink (K = [15,5]):


问:
How to ensure test-dependencies declared in a colcon.pkg file are installed?
I was trying to build Micro-XRCE-DDS-Client library on develop hash bc6e9c361a87f71272cb2033a899a09f0c49f638.

It is a Tier 1 package that can be build with colcon and has tests, but there is no package.xml file. How should I should install dependencies?

According to colcon, it's outside of the design scope of colcon to fetch dependencies.
According to rosdep, it only works with package.xml files.

When I build on my Ubuntu 22.04 system, it cannot find GTest, but it's already installed with apt using libgtest-dev.

```<code>libgtest-dev is already the newest version (1.11.0-3).
</code>```
```<code>colcon build --cmake-args -DUCLIENT_BUILD_TESTS=ON
...
CMake Error at /usr/share/cmake-3.22/Modules/FindPackageHandleStandardArgs.cmake:230 (message):
  Could NOT find GMock (missing: GMOCK_LIBRARY GMOCK_INCLUDE_DIR
  GMOCK_MAIN_LIBRARY)
Call Stack (most recent call first):
  /usr/share/cmake-3.22/Modules/FindPackageHandleStandardArgs.cmake:594 (_FPHSA_FAILURE_MESSAGE)
  cmake/modules/FindGMock.cmake:122 (FIND_PACKAGE_HANDLE_STANDARD_ARGS)
  CMakeLists.txt:447 (find_package)
</code>```

I have tried editing the supplied colcon.pkg file to declare a dependency with test-dependencies, since it was not declared there, however it has no effect on colcon build. I even tried putting in a bogus fooBar in test-dependencies, nothing happens.

What is the recommended way to install dependencies if you are using colcon.pkg and is there any command in colcon to validate you have dependencies installed prior to calling colcon build?

Edit, sure, I can install libgmock-dev, which fixes the problem, but that a fair bit of googling to find was the right way. There are misleading outdated answers saying this won't work.

答:
As you've noted. This is out of scope of colcon. It is a build tool and it's job is to iterate over packages in your workspace and build them in the appropriate order.

Colcon discovers packages by finding resource files such as package.xml or setup.py which it can attempt to parse. the colcon.pkg file is a way to give it explicit information so it doesn't have to guess/interpret.

In terms of dependencies though, it will assume that any not available in the current workspace are already available on the system and iterate the available packages.

You're right that the standard way in the ROS ecosystem is to use rosdep. This leverages the package.xml files and the rosdep database to manage dependencies.

It is the responsibility of the package maintainer to define the dependencies in a way that you as the user can download and install them. The use of a package.xml is an easy well defined cross platform way to do so. However if the package doesn't provide a package.xml file the best practice is to clearly define the dependencies required. And it's even more helpful to document how to install them. This is usually in the README. There appears to be a link to installation instructions in the README but that doesn't include the dependency resolution. So I would suggest ticketing it as a documentation issue there or considering suggesting adding a package.xml.

Note: that this is a Tier 1 support from Eprosima it's not in the ROS Tier 1 support list.


问:
Solving for rotation matrix: Aligning vector a with vector b and then rotating around vector b
Currently working on a quadcopter simulation. I have a desired thrust vector,

```<code>t =sin(30)cos(45)a1+sin(30)sin(45)a2+cos(30)a3
</code>```

and desired yaw angle,

```<code>psi = 45.
</code>```

Because we are working with an under-actuated system, I am trying to solve for the rotation matrix that aligns the vector:b3 = [0,0,1] (direction of thrust in the body-fixed frame of reference) with the direction of t. Here is my code:

```<code>t = [sind(30) * cosd(45), sind(30) * sind(45), cosd(30)];

b3 = [0;0;1];

psi = 45;

syms phi theta;

eqn = [[cosd(psi)*cosd(theta)-sind(phi)*sind(psi)*sind(theta), -
cosd(phi)*sind(psi), cosd(psi)*sind(theta)+cosd(theta)*sind(phi)*sind(phi);
cosd(theta)*sind(psi)+cosd(psi)*sind(phi)*sind(theta), cosd(phi)*cosd(psi),
sind(psi)*sind(theta)-cosd(theta)*sind(phi)*cosd(psi);
 -cosd(phi)*sind(theta), sind(phi), cosd(phi)*cosd(theta)]
* b3 == t/norm(t)];

S = solve(eqn,[phi theta]);
disp(S.phi)
disp(S.theta)`
</code>```

The problem is that it outputs an empty symbolic struct, with no warnings or errors otherwise:

What I am doing wrong? It is an issue with my theory, my implementation, or both?

答: I made the same mistake, actually u need to write the thrust vector t as column vector instead of the row vector. u can simply replace t with t' at all occurrences, it will solve the issue.</p>


答:
I've done a similar project before where I wrote the control program with .Net.

I think the first step is deciding how much of a role do you want the computer to play in controlling the helicopter.  If you want the helicopter to be rather autonomous and simply receive commands from the computer but decide how to execute those commands you will likely need to use a microcontroller on-board the helicopter.

If you want to control everything via transmitting sensor data from the helicopter and then having the computer do all the control, you won't need as much hardware on the helicopter itself. You will just need a wireless data link.

When I worked on my project, I used the first architecture because I didn't want loss of communication with the computer to result in a total loss of control.

Do you have microcontroller or assembly language experience?

Title: RC Helicopter Connect with computer


问:
Writing multi-process Python tests in ROS2
I have a large number of tests in ROS2 in Python I would like to write that are essentially

Publish a message

Subscribe and do something

Check what happens during the "do something"

I can do this by creating the publisher and subscriber in the same piece of code. However, it's closer to what happens "for real" if I publish in one process and subscribe in another. But that doesn't fit in well with pytest. Is there a ROS2 "way" to do this - something like the equivalent of ROS1's rostest?

Thanks!

答:
Generally speaking, you should try to make it so that your software isn't heavily reliant on ROS, to the point where you can test the core, nontrivial logic of your code without such an invasive dependency. This often means writing a library that does not rely on ROS at all and then writing a thin ROS wrapper around its runtime execution. The library's behaviour can be tested independently. This might be one way to get around your problem.

If that isn't possible, you should look into pytest's mock testing extension pytest-mock. Mock testing involves being able to make "mock" objects that isolate your dependencies; you can assign expected behaviour like method calls on them and match whether it works how you expect.

You can also use the launch testing package to run ROS 1-like rostests


问:
Rotation of a point in 3D space
I want to rotate a point (3,4,5) by an angle 45 degrees(0.785398 rad) in 3D space. I have used the standard DCM matrix in the order Rx, Ry and Rz.
Where,
theta = 0.785398,
Rx= ( [ 1, 0, 0]
[ 0, cos(theta), sin(theta)]
[ 0, -sin(theta), cos(theta)])

Ry= ([ cos(theta), 0, -sin(theta)]
[ 0, 1, 0]
[sin(theta), 0, cos(theta)])

Rz= ([ cos(theta), sin(theta), 0 ]
[ -sin(theta), cos(theta) , 0]
[ 0,0,1 ])

R = Rx(0.785398) * Ry(0.785398) * Rz(0.785398)

x = R * x
We get a 3 x 1 matrix after this multiplication. The resultant matrix for the value of X,Y and Z is found by adding all the values in this case I got
X = [[ 1.50000049]
[-0.43934]
[ 2.56065985]]
therefore X= 3.62

y = R * y

Y = [[ 2]
[ 3.41421352]
[-0.58578667]]
therefore Y= 4.82

z = R * z

Z = [[-3.53553333]
[ 2.5  ]
[ 2.50000082]]
therefore Z= 1.46

Ideally even after the rotation of the point the length of the point from origin should remain the same but here it is varying. Length before rotation is 7.071 and length after rotation is 6.210.Could you tell me if anything is wrong.

My basic idea is to rotate the 3D point independently along the three axis by a certain angle to get the new position of of the same point now when the axis are rotated by certain angle.Pls suggest some approach.

答:
1st Rotation of 45 degrees about what axis?
$$x_{new} = R_x(45)R_y(45)R_z(45) x_{old}$$
describes 3 rotations of 45 degrees in all the fixed axis. In that order ($R_x(45)R_y(45)R_z(45)$), you first rotate the point around z axis, then rotate the new point around the fixed y axis, and finally you rotate the new point around the fixed x axis.

If you want to rotate around a random axis, you should use rodriguez formula .

2nd In the case you describe in the post, as you know the angle, you can calculate the R matrix, it is:

$$ R = \begin{bmatrix} 0.5000 &  -0.5000 &   0.7071 \\
    0.8536  &  0.1464 &  -0.5000\\
    0.1464   & 0.8536  &  0.5000 \end{bmatrix} $$

so then you can get the new position of the point. It is:
$x_{new} = Rx = [    3.0355, \ 0.6464,\ 6.3536]^T$

extra I suggest you to look in some relavant theory. Here are some good sources online:

mecharithm,
Modern Robotics

Title: Rotation of a point in 3D space


答:
In addition to the aforementioned launch_testing of the launch project whose goal is to provide "tools for launching multiple processes and for writing tests involving multiple processes" there is the launch_testing_ros of the launch_ros project. This is about "tools for launching ROS nodes and for writing tests involving ROS nodes".

So as long as you don't combine multiple nodes into a single process launch_testing_ros is probably the best solution for testing node interactions based on topics.

Title: Writing multi-process Python tests in ROS2

Body:
I have a large number of tests in ROS2 in Python I would like to write that are essentially

Publish a message

Subscribe and do something

Check what happens during the "do something"

I can do this by creating the publisher and subscriber in the same piece of code. However, it's closer to what happens "for real" if I publish in one process and subscribe in another. But that doesn't fit in well with pytest. Is there a ROS2 "way" to do this - something like the equivalent of ROS1's rostest?

Thanks!


问:
Azure Kinect with ROS2 Humble
I have been trying to configure my Azure Kinect to be used in ROS2 Humble but I haven't had success.
Here are a few links I found with good information

https://gist.github.com/madelinegannon/c212dbf24fc42c1f36776342754d81bc

https://learn.microsoft.com/en-us/windows-server/administration/linux-package-repository-for-microsoft-software#ubuntu

Any help would be awesome!

Update (still not working)

Following the comments below (thank you)

Manually downloaded libsoudio and installed

Downloaded the k4a files and installed (from here)

Made a ros workspace and cloned the Azure demo project

rosdep install --from-paths src --ignore-src -r -y

```<code>ERROR: the following packages/stacks could not have their rosdep keys resolved
to system dependencies:
azure_kinect_ros_driver: Cannot locate rosdep definition for [K4A]
Continuing to install resolvable dependencies...
#All required rosdeps installed successfully
</code>```

If I just ignore this a colcon build the package, this is the result:

```<code>Starting >>> azure_kinect_ros_driver
--- stderr: azure_kinect_ros_driver
Finding K4A SDK binaries
!!! Body Tracking SDK not found: body tracking features will not be available !!!
K4A Libs: k4a::k4a;k4a::k4arecord
K4A DLLs: /usr/lib/x86_64-linux-gnu/libk4a.so.1.4.1;/usr/lib/x86_64-linux-gnu/libk4arecord.so.1.4.1
K4A Install Needed: FALSE
---
Finished <<< azure_kinect_ros_driver [6.39s]

Summary: 1 package finished [6.51s]
</code>```

Doesn't seem a terrible output but it still doesn't work.

I also followed the instructions here and created the /etc/udev/rules.d/99-k4a.rules file with the following content:

```<code># Bus 002 Device 116: ID 045e:097a Microsoft Corp.  - Generic Superspeed USB Hub
# Bus 001 Device 015: ID 045e:097b Microsoft Corp.  - Generic USB Hub
# Bus 002 Device 118: ID 045e:097c Microsoft Corp.  - Azure Kinect Depth Camera
# Bus 002 Device 117: ID 045e:097d Microsoft Corp.  - Azure Kinect 4K Camera
# Bus 001 Device 016: ID 045e:097e Microsoft Corp.  - Azure Kinect Microphone Array

BUS!="usb", ACTION!="add", SUBSYSTEM!=="usb_device", GOTO="k4a_logic_rules_end"

ATTRS{idVendor}=="045e", ATTRS{idProduct}=="097a", MODE="0666", GROUP="plugdev"
ATTRS{idVendor}=="045e", ATTRS{idProduct}=="097b", MODE="0666", GROUP="plugdev"
ATTRS{idVendor}=="045e", ATTRS{idProduct}=="097c", MODE="0666", GROUP="plugdev"
ATTRS{idVendor}=="045e", ATTRS{idProduct}=="097d", MODE="0666", GROUP="plugdev"
ATTRS{idVendor}=="045e", ATTRS{idProduct}=="097e", MODE="0666", GROUP="plugdev"

LABEL="k4a_logic_rules_end"
</code>```

But k4aviewer gives me this error:

```<code>[ error    ] : usb_cmd_io(). Error calling libusb_bulk_transfer for initial tx, result:LIBUSB_ERROR_IO
[ error    ] : usb_cmd_io(usbcmd_handle, cmd, p_cmd_data, cmd_data_size, p_data, data_size, NULL, 0, bytes_read, &cmd_status) returned failure in usb_cmd_read()
[ error    ] : usb_cmd_read(depthmcu->usb_cmd, DEV_CMD_DEPTH_READ_PRODUCT_SN, NULL, 0, (uint8_t *)temp_serial_number, sizeof(temp_serial_number) - 1 , &bytes_read) returned failure in depthmcu_get_serialnum()
[ error    ] : depthmcu_get_serialnum(device->depthmcu, serial_number, &serial_number_size) != K4A_BUFFER_RESULT_SUCCEEDED returned failure in k4a_device_open()

</code>```

答:
Install Ubuntu 22.04.

Install the Azure Kinect Sensor SDK as per official documentation stated here

Install ros2-humble-desktop

Setup your ros2 workspace

Download the humble branch of the official driver from here into the src folder of your workspace

Run rosdep rosdep install --from-paths src --ignore-src -r -y

Build the workspace with colcon build

Source the workspace source install/setup.bash

Run the launch file ros2 launch Azure_Kinect_ROS_Driver driver.launch.py

Title: Azure Kinect with ROS2 Humble


问:
What should I use for a Visual+IMU+GPS fusion? A Madgwick or a Kalman?
I am new to robotics, and recently I am involving in a sensor fusion task using visual input (binocular at present), an IMU, and a GPS module. I have searched for related journal papers for a reasonable fusion method. Well, a Kalman-type algorithm definitely seems to be a popular one used in sensor fusion. But, I also read articles used the Madgwick orientation filter to fix the IMU data and feed-forward to visual odometry input (see: https://www.researchgate.net/publication/342732957_Stereo_Visual_Inertial_Pose_Estimation_Based_on_Feedforward-Feedback_Loops). Now I am getting confused: what should I use? or say, when should I use a Kalman and when should I use a Madgwick? Thx.

Or it's true to say when we get a 9 DoF IMU with magnetometer, Madgwick is better for taking magnetic field/gravitational direction into account, and Kalman performs better or easier to implement when using 6 DoF IMU?

答:
I think you need to focus on what you want to estimate.

Usually, a Madgwick filter is an optimization based algorithm targeting the estimation of the quaternion representing the attitude and heading (i.e. the 3d orientation, roll+pitch+yaw) of your robot. In its traditional formulation you will not be able to extend it to camera info fusion, just imu and magnetometer. It's simple, fast, easy to tune and performs quite well given calibrated sensors input data (magnetometer!!!).

Kalman filters are a more general, probabilistic approach to estimation. You can easily extend your state to estimate both position and orientation, and from a range a various sensors, including camera, provided a gaussian sampling model. They are more tricky to formulate (you must derive a dynamic model of your robot) , to tune and are computationally more expensive, especially in high dimensional space and in its nonlinear variants.

If you just need to estimate orientation and you're without a camera, go for Madgwick. If a camera module is available, go for a Kalman filter. If you feel inspired, you may implement both and feed the output of Magdwick to Kalman as external orientation measurement.


问:
Sending Universal Robots UR5e to specific place Using Camera
I have a UR5e installed with a simple camera on its end effector. The robot is at a known position (Cartesian pose/angles and joint angles)

Using the camera I took a picture of a QR code and got the object's real life coordinates to the camera using OpenCV's SolvePNP. This gives me a 3x1 translation vector and a 3x1 rotation vector.

I'd like to tell the robot to move to that position in translation and angles.

I guess first step would be to inverse that result so I have the QR code's position from the camera's axes point of view.

How do I "add" this movement to the robot's arm?

(Using Python and ut_rtde)

Thank you

答: If extrinsic parameters of camera (Transformation between end-effector and camera) are known, you know the transformation matrix between camera and robot base. So, definitely you know the T matrix between base and QR code in cartesian space, so you coulduse a function to move robot respect base using cartesian movement or by joints (calculating IK knowing T matrix between base and QR)</p>


问:
Visualize state of gamepad/joystick in ros in rviz or in an Image?
A non-ros but python or C++ solution like https://github.com/KernelZechs/open-joystick-display (this video shows it off) is what I was originally looking for, and I'll possibly still that use if it immediately works- but it is no longer being maintained and given the language and build process I'm unlikely to be able to quickly make any needed alterations (or discern which is best of all the forks).

But the other route is to do something quickly in a ros node from scratch using the Joy message as an input topic, and publish a crude image or turn all of the Joy message into JointStates and combine with a simple gamepad urdf (and maybe find a free mesh file of an xbox controller to use with it later)- unless something like that already exists?

答:
https://github.com/lucasw/rqt_virtual_joy/tree/joy_monitor shows a single analog stick and all the buttons in rqt under Robot Tools | Joystick Monitor, in the image below I'm pressing left stick up and right and the x and right shoulder buttons:

Adding another analog stick wouldn't be hard

(from comment above)

There's a ros2 package joy_tester that uses tk: https://github.com/joshnewans/joy_tester (via https://discourse.ros.org/t/new-package-joy-tester/30036)


问:
Quadrocopter first build: how to tell if components play well together?
I'm building my first quadrocopter, and I'm trying to come up with a parts list that is suitable for a first build.

I will use this to learn how to fly a quadrocopter manually (lots of crashes!), and to do some experiments with running AI for piloting it.

A couple questions about the below list of parts:

Is this a good choice for a first build? Are we missing any crucial parts?

Do these components work together? Is this battery strong enough to fuel all the components that need power?

Here's the current list of parts:

Frame - 450 mm

Propellers - 10x4.5", two pairs

Motor (4x) - 900kv brushless outrunner motor; max current: 18A; ESC: 25-30A; cell count:3s-4s lipoly

Electronic speed controllers (4x) - 20A constant current, 25A burst current; battery: 2-4S lipoly

Battery - 3300mAh lipoly, 11.1v, 3 cell; constant discharge: 30C, peak discharge: 40C. charge plug: JST-XH. 3300 mAh x 30C = 99 amps?

Charger - lipoly, 50W, 6A, 12v power supply

Power supply - input: AC 100-240v 50/60Hz; output: DC15v 5A

Arduino board

Gyroscope for arduino

Accelerometer for arduino

GPS sensor for arduino?

RC transmitter for arduino

RC controller

答:
You part list is fine as this is your first build. However I would suggest you to use ready made flight controller instead of buying arduino and program it yourself. Once you are comfortable flying quad rotor, it will be easier for you to test your code and adjust controller gains which is a very crucial step to control quad rotor as per your requirement. There are good flight controller available for quad rotor such as Ardupilotmega or KKMulticopter.

Apart from this, you should add lipo alarm like this to your list which will monitor lipo voltage during flight and tell you when to land. Also you should have spare propellers as propeller are most likey to get damage when you crash. You should also consider propeller balancer like this to reduce vibration due to imbalanced propeller.