问:
covariance matrix in EKF?
I'm struggling with the concept of covariance matrix.
$$
\Sigma
=
\begin{bmatrix}
\sigma_{xx} & \sigma_{xy} & \sigma_{x \theta} \\
\sigma_{yx} & \sigma_{yy} & \sigma_{y \theta} \\
\sigma_{\theta x} & \sigma_{\theta y} & \sigma_{\theta \theta} \\
\end{bmatrix}
$$
Now, my understanding for $\sigma_{xx}$, $\sigma_{yy}$, and $\sigma_{\theta \theta}$ that they describe the uncertainty. For example, for $\sigma_{xx}$, it describes the uncertainty of the value of x. Now, my question about the rest of sigmas, what do they represent? What does it mean if they are zeros? I can interpret that if $\sigma_{xx}$ is zero, it means I don't have uncertainty about the value of x.

Note, I'm reading Principles of Robot Motion - Theory, Algorithms, and Implementations by Howie Choset et. al., which states that

By this definition $\sigma_{ii}$ is the same as $\sigma_{i}^{2}$ the variance of $X_{i}$. For $i ≠ j$, if $\sigma_{ij} = 0$, then $X_{i}$ and $X_{j}$ are independent of each other.

This may answer my question if the rest of sigmas are zeros however, I'm still confused about the relationship between these variables for example $x$ and $y$. When does this happen? I mean the correlation between them. Or in other words, can I assume them to be zeros?

Another book namely FastSLAM: A Scalable Method ... by Michael and Sebastian which states

The off-diagonal elements of the covariance matrix of this
  multivariate Gaussian encode the correlations between pairs of state
  variables.

They don't mention when the correlation might happen and what does it mean?

答:
To get a feeling for the covariance matrix - without getting into the math details here - its best to start with a 2x2 matrix. Then remember that the covariance matrix is an extension of the concept of variance into the multivariate case. In the 1D case, variance is a statistic for a single random variable. If your random variable has a Gaussian distribution with zero mean, its variance can precisely define the probability density function.

Now, if you extend this to two variables instead of one, you can differentiate between two cases. If your two variables are independent, which means the outcome of one value has no relation to the other value, its basically the same as in the 1D case. Your $\sigma_{xx}$ and your $\sigma_{yy}$ give the variance of the $x$ and $y$ part of your random variable, and $\sigma_{xy}$ will be zero.

If your variables are dependent this is different. Dependent means that there is a relation between the outcome of $x$ and $y$. For example you could have that whenever $x$ is positive, $y$ is in general more likely to also be positive. This is given by your covariance value $\sigma_{xy}$.

Giving an example for a robot in a 2D case without orientation is a bit contrived, but lets say you have a random component along the travel direction on the $x$-axis and you know that this component also generates a drift on your lateral axis ($y$). This could for example be a faulty wheel. This will result in a rotated uncertainty ellipse. Now for e.g. when you later have something that measures your actual $x$ position, you can estimate the uncertainty distribution on your $y$ component.

A more relevant example is in the 3D case, where usually you have a different uncertainty along the transversal direction compared to the lateral direction. When you rotate your system (so changing $\theta$) this will also rotate your uncertainty ellipse. Note that the actual representation is usually some banana shape, and the Gaussian is only an approximation. In the EKF case its a linearization around the mean.

One really good way to visualize this is to use the concept of the uncertainty ellipse. It basically shows the $1 \sigma$ boundary for a multivariate Gaussian distribution, and can be used to visualize a Covariance matrix. A quick search brought up this demo which will also provide you with some additional insight into how the covariance is built. In essence, the diagonal entries define the extents of the axis, while the off-diagonal entries relate to the rotation of the entire ellipse.

This is also true in the 3D case. I would love to get more mathematical here, but maybe some time later.

Title: covariance matrix in EKF?


问:
why custom generated pointcloud2 is displayed as line on rviz?
I am new in ROS, We are developing LiDAR camera and I am trying to interface out LiDAR camera with ROS. I am using ROS 1 noetic on windows 10, I wrote a python script to convert the depth image generated from out camera to PointCloud2 format and then publish the generated PointCloud2 format.

When I try to display the PiontCloud2 on Rviz , the pointcloud is displayed as line on screen as here

or nothing is dispalyed at all

here is a single PointCloud2 sample

```<code>header:
  seq: 8
  stamp:
    secs: 1661780280
    nsecs: 110538482
  frame_id: "LiDAR1"
height: 80
width: 260
fields:
  -
    name: "x"
    offset: 0
    datatype: 7
    count: 20800
  -
    name: "y"
    offset: 4
    datatype: 7
    count: 20800
  -
    name: "z"
    offset: 8
    datatype: 7
    count: 20800
is_bigendian: False
point_step: 12
row_step: 3120
data: [130, 115, 134, 191, 118, 171, 231, 190, 23, 217, 78, 64, 103 ....]
is_dense: True
---
</code>```

I generate the Point Cloud by converting my depth image using camera parameters, applying the following command and then convert it to string

```<code>#cx shift of camera center from sensor center in x direction measured in pixels
   #cy shift of camera center from sensor center in y direction measured in pixels
   #fx=fy focal length in  meters
   #rows no of rows per frame
   #cols no of columns per frame

    c, r = np.meshgrid(np.arange(-cols/2,cols/2), np.arange(-rows/2,rows/2), sparse=True)
    z = np.float32(depth/1000)
    x = np.float32(z * (c - cx) * pixel_width / fx )
    y = np.float32(z * (r - cy) * pixel_higth / fy )
    ptcloud = np.dstack((x, y, z)).tostring()
</code>```

my question is where's my mistake, is it in the PointCloud2 Format, or data or in Rviz configuration?

答:
To answer your question directly I suspect that it's an error in your population of the point cloud.

I'd recommend that you make a simple publisher that just publishes a known pattern that's easily debugged first to make sure that you understand the point cloud format correctly. rviz uses a bunch of these for tests such as here: https://github.com/ros-visualization/rviz/blob/noetic-devel/src/test/send_point_cloud_2.py

And then you afterwards leverage that knowledge to fill in the data from your sensor.

I don't think that there's any rviz settings that will significantly warp or reduce a dimension or two of your data.

ContentLicense: CC BY-SA 4.0


答:
I think it's in your conversion. Why does the depth/height of the content affect the x/y position? Your code has:

```<code>c, r = np.meshgrid(np.arange(-cols/2,cols/2), np.arange(-rows/2,rows/2), sparse=True)
z = np.float32(depth/1000)
x = np.float32(z * (c - cx) * pixel_width / fx )
y = np.float32(z * (r - cy) * pixel_higth / fy )
</code>```

but why are x and y dependent on z there? I would think you're looking for something like the Matlab surf command, but the example in the Matlab documentation looks like:

```<code>[X,Y] = meshgrid(1:0.5:10,1:20);
Z = sin(X) + cos(Y);
surf(X,Y,Z)
</code>```

In this example, the x/y points are fixed and only the z-values vary. I would urge you to really consider what the x/y values are supposed to be and whether or not depth actually affects those values.

Title: why custom generated pointcloud2 is displayed as line on rviz?

Body:
I am new in ROS, We are developing LiDAR camera and I am trying to interface out LiDAR camera with ROS. I am using ROS 1 noetic on windows 10, I wrote a python script to convert the depth image generated from out camera to PointCloud2 format and then publish the generated PointCloud2 format.

When I try to display the PiontCloud2 on Rviz , the pointcloud is displayed as line on screen as here

or nothing is dispalyed at all

here is a single PointCloud2 sample

```<code>header:
  seq: 8
  stamp:
    secs: 1661780280
    nsecs: 110538482
  frame_id: "LiDAR1"
height: 80
width: 260
fields:
  -
    name: "x"
    offset: 0
    datatype: 7
    count: 20800
  -
    name: "y"
    offset: 4
    datatype: 7
    count: 20800
  -
    name: "z"
    offset: 8
    datatype: 7
    count: 20800
is_bigendian: False
point_step: 12
row_step: 3120
data: [130, 115, 134, 191, 118, 171, 231, 190, 23, 217, 78, 64, 103 ....]
is_dense: True
---
</code>```

I generate the Point Cloud by converting my depth image using camera parameters, applying the following command and then convert it to string

```<code>#cx shift of camera center from sensor center in x direction measured in pixels
   #cy shift of camera center from sensor center in y direction measured in pixels
   #fx=fy focal length in  meters
   #rows no of rows per frame
   #cols no of columns per frame

    c, r = np.meshgrid(np.arange(-cols/2,cols/2), np.arange(-rows/2,rows/2), sparse=True)
    z = np.float32(depth/1000)
    x = np.float32(z * (c - cx) * pixel_width / fx )
    y = np.float32(z * (r - cy) * pixel_higth / fy )
    ptcloud = np.dstack((x, y, z)).tostring()
</code>```

my question is where's my mistake, is it in the PointCloud2 Format, or data or in Rviz configuration?

Body:
I think it's in your conversion. Why does the depth/height of the content affect the x/y position? Your code has:

c, r = np.meshgrid(np.arange(-cols/2,cols/2), np.arange(-rows/2,rows/2), sparse=True)
z = np.float32(depth/1000)
x = np.float32(z * (c - cx) * pixel_width / fx )
y = np.float32(z * (r - cy) * pixel_higth / fy )

but why are x and y dependent on z there? I would think you're looking for something like the Matlab surf command, but the example in the Matlab documentation looks like:

[X,Y] = meshgrid(1:0.5:10,1:20);
Z = sin(X) + cos(Y);
surf(X,Y,Z)

In this example, the x/y points are fixed and only the z-values vary. I would urge you to really consider what the x/y values are supposed to be and whether or not depth actually affects those values.


问:
How to control end-effector orientation with vector part of quaternion?
I was reading this paper: Closed-Loop Manipulator Control Using Quaternion
Feedback, and it mentioned that you could control the end-effector orientation using a resolved rate controller where the orientation part of that controller is:
$$\tau_\theta = \mathbf{K}_d(\mathbf{J}_o^\dagger(\omega_d - \mathbf{K}_o\mathbf{e}_o) - \dot{\theta}).$$

See Eq.(20) of the linked paper. My understanding of the terms is that $\omega_d$ is the desired angular velocity, $\mathbf{J}_o$ is the orientation part of the geometric Jacobian, $\dot{\theta}$ is the angular velocities of the joints, $\mathbf{e}_o$ is the orientation error, and both $\mathbf{K}_d$ and $\mathbf{K}_o$ are gain matrices.

In that paper, it mentioned that $\mathbf{e}_o$ could be represented by $\delta{\mathbf{q}}$, which is the vector part of the error quaternion. I did try this. However, it did not work. Instead, if I set $\mathbf{e}_o$ to be the orientation error in the axis-angle form, it was able to give me a controller that works.

So I was wondering if I did something wrong when using the vector part of the error quaternion as $\delta{\mathbf{q}}$. I guess that one possible place I might have gotten wrong is $\mathbf{J}_o$, I am using the rotation part of the geometric Jacobian, is this the right choice?

The portion of my code that computes the control is

import pinocchio as pin
from scipy.spatial.transform import Rotation as R

# get joint states
q, dq = get_state()

# Get end-effector position
ee_position = get_ee_position()

# Get end-effector orientation in quaternions
ee_orientation = get_ee_orientation()

# [x, y, z, w]
ee_quaternion = R.from_matrix(ee_orientation).as_quat()

# Orientation error in quaternion form
quat_err = compute_quat_vec_error(target_quaternion, ee_quaternion)

# Get frame ID for grasp target
jacobian_frame = pin.ReferenceFrame.LOCAL_WORLD_ALIGNED

# Get Jacobian of grasp target frame
jacobian = robot.getFrameJacobian(FRAME_ID, jacobian_frame)

# Get pseudo-inverse of frame Jacobian
pinv_jac = np.linalg.pinv(jacobian)

# Compute Gravitational terms
G = robot.gravity(q)

# Compute controller
target_dx = np.zeros((6, 1))
target_dx[:3] = 1.0 * (target_position - ee_position)
target_dx[3:] = np.diag([3.0, 3.0, 3.0]) @ quat_err[:, np.newaxis]

# Compute torque commands
tau = (pinv_jac @ target_dx - dq[:, np.newaxis]) + G[:, np.newaxis]
```
<p>the function that I used to compute the quaternion error is</p>
<pre class="lang-py prettyprint-override"><code>def compute_quat_vec_error(quat_desired, quat_measured):
    eta_d = quat_desired[-1]
    eta = quat_measured[-1]

    q_d = quat_desired[:3]
    q = quat_measured[:3]

    delta_quat_vec = eta_d * q - eta * q_d - np.cross(q_d, q)
    return delta_quat_vec
</code>```

答:
The error with the above code when compared to the axis-angle representation method is that this quaternion method returns its values still in a quaternion representation. In other words, we have:

$$ \mathbf{e}_{q} = [q_{1}\text{sin}(\frac{\psi}{2}), \ q_{2}\text{sin}(\frac{\psi}{2}), \ q_{3}\text{sin}(\frac{\psi}{2})]$$

whereas the angle axis representation appears to return (from our discussions in chat):

$$ \mathbf{e}_{a} = [q_{1}\psi, \ q_{2}\psi, \ q_{3}\psi]$$

So, this leaves us with two different values for what conceptually represents the same information. Now, we should keep in mind that $\mathbf{J}_{o}$ is formulated (as far as I am aware) to represent the axis-angle change in orientation with respect to joint velocities. Thus, the pseudo-inverse of $\mathbf{J}_{o}$ will return proper joint velocities when multiplied with the axis-angle representation of angular change. To accomplish this given $\mathbf{e}_{q}$, one could apply the following:

$$ \text{let} \  r_{i} \triangleq q_{i}\text{sin}(\frac{\psi}{2}), \  \text{for} \ i \in \{1,2,3\} \\
||\mathbf{e}_q|| = \sqrt{r_{1}^{2} + r_{2}^{2} + r_{3}^{2}} \\
$$

Then, as $||\mathbf{q}|| = \sqrt{q_{1}^{2} + q_{2}^{2} + q_{3}^{2}} = 1$ by the definition of a quaternion (versor), we can find the rotation angle from the following result:

$$ ||\mathbf{e}_{q}|| = ||\mathbf{q} \cdot \text{sin}(\frac{\psi}{2})|| = | \text{sin}(\frac{\psi}{2})| \cdot ||\mathbf{q}|| = | \text{sin}(\frac{\psi}{2}) | \cdot 1$$

Where we can pull the $| \sin({\frac{\psi}{2}}) |$ out of the norm by the definition of the Euclidian norm. Now, we need to deal with the absolute value portion of the $| \sin(\frac{\psi}{2}) |$ term. So, choose an arbitrary $r_{j}, \text{where} \ 1 \le j \le 3$, and we can now define:

$$\psi \triangleq 2 \sin^{-1}(||\mathbf{e}_{q}||) \cdot \text{sgn}(r_{j})$$

In other words, we assume $q_{j}$ is always positive and determine the correct sign of our angle with the direction of $r_{j}$. Note that this should not cause any issues as we will be multiplying $q_j$ and $\psi$ resulting in the same original sign (we know this is true as $\sin(x) \gt 0 \ \text{for} \ 0 \lt x \lt \frac{\pi}{2}$ and $\sin(x) < 0 \ \text{for} \ -\frac{\pi}{2} \lt x \lt 0$). The final step of converting the error in the quaternion representation to error in the angle-axis representation is:

$$\mathbf{e}_{a} = \mathbf{e}_{q} \cdot \frac{\psi}{\sin(\frac{\psi}{2})} $$

Note that a check should be done to see if $|| \mathbf{e}_{q} ||$ is close to zero to avoid division by zero - as $\psi$ would then also be close to zero.

As a final point not really related to solving your problem here, I would recommend sticking with angle-axis representations instead of quaternions. The only clear advantage of quaternions is there computational efficiency, but given the speed of modern computer hardware you are unlikely to see any real benefit there unless you are simulating many, many objects simultaneously. Angle-axis representation has the same ability to represent rotations without representational singularities just as quaternions (they are isomorphic). Personally, I find angle-axis more intuituve as well - given the exponential map from a unit rotation vector and scalar angle to a rotation matrix here and the rotation of a vector about an arbitrary vector by a scalar angle here. Quaternions do have simple equations, but I have found the headaches associated with their nuances to be much greater than the utility gained from slightly faster computations.

Finally, note that most arguments around quaternions vs. rotation matrices are based on compositions of Euler angles - which are inferior to both quaternions and axis-angle.


答:
Comparing my code to the link sent by @Tully above, I could found the mistake.
Here's the corrected code

```<code>c, r = np.meshgrid(np.arange(-cols/2,cols/2), np.arange(-rows/2,rows/2), sparse=True)
z = np.float32(depth) #depth/1000 ==> float point cloud represent points in meters
x = np.float32(z * (c - cx) * pixel_width / fx )
y = np.float32(z * (r - cy) * pixel_higth / fy )
z = z.reshape(1,-1)
x = x.reshape(1,-1)
y = y.reshape(1,-1)
brightness = brightness.reshape(1,-1)
ptcloud = np.dstack([x, y, z, np.float32(brightness)])
return ptcloud.reshape(-1,4).tolist()
</code>```

and changed msg.fields count to 1 instead of 20800

```<code>    msg.fields = [
        PointField('x', 0, PointField.FLOAT32, 1),
        PointField('y', 4, PointField.FLOAT32, 1),
        PointField('z', 8, PointField.FLOAT32, 1),
        PointField('intensity', 12, PointField.FLOAT32, 1) ]
</code>```

and creaded the pointcloud2 using

```<code>pc2 = point_cloud2.create_cloud(msg.header, msg.fields, data)
</code>```

Body:
Comparing my code to the link sent by @Tully above, I could found the mistake.
Here's the corrected code

c, r = np.meshgrid(np.arange(-cols/2,cols/2), np.arange(-rows/2,rows/2), sparse=True)
z = np.float32(depth) #depth/1000 ==> float point cloud represent points in meters
x = np.float32(z * (c - cx) * pixel_width / fx )
y = np.float32(z * (r - cy) * pixel_higth / fy )
z = z.reshape(1,-1)
x = x.reshape(1,-1)
y = y.reshape(1,-1)
brightness = brightness.reshape(1,-1)
ptcloud = np.dstack([x, y, z, np.float32(brightness)])
return ptcloud.reshape(-1,4).tolist()

and changed msg.fields count to 1 instead of 20800

    msg.fields = [
        PointField('x', 0, PointField.FLOAT32, 1),
        PointField('y', 4, PointField.FLOAT32, 1),
        PointField('z', 8, PointField.FLOAT32, 1),
        PointField('intensity', 12, PointField.FLOAT32, 1) ]

and creaded the pointcloud2 using

pc2 = point_cloud2.create_cloud(msg.header, msg.fields, data)


问:
rosrun Couldn't find executable named node_name.py
I am using ROS Noetic on Windows 10, and I created a python node using this directory structure

```<code>\catkin_ws\src\package_name\scripts>
</code>```

and my node's Python files are inside scripts folder.
I can run the nodes from within the scripts folder using

```<code>python node_name.py
</code>```

and can run the node within scripts folder using

```<code>rosrun package_name node_name.py
</code>```

but when I try to run the node from the \catkin_ws\ folder using

```<code> rosrun package_name node_name.py
</code>```

I get this error

```<code>[rosrun] Couldn't find executable named node_name.py
</code>```

I have already run

```<code>\ros\noetic\x64\setup.bat
\devel\setup.bat
</code>```

Why are my nodes are not recognizable by rosrun outside the scripts folder and how do I solve this problem?

答:
You haven't provided enough information, but my first guess is that you did not make your script executable on the system. rosrun looks for executables available in each packages.

There's an FAQ in the Python Documentation about Windows

In ROS 2 we typically don't just write scripts and expect them to run, but leverage official python entry points in the setup.py which the python installer will correctly setup for execution on Windows.


问:
How to Implement a Physically Human-Controlled Robotic Arm?
I'm a complete beginner in robotics other than FIRST competitions at my school. I wanted to make a robotic arm that works by a person physically moving the end-effector in 3D space and the robot staying in place even when the person removes their hand. Similar to https://youtu.be/RLxIg69GWqA?t=465. The robot arm would also apply force-feedback when it gets close to some predefined threshold.

If I wanted to use servos (because I really need the angle data for force feedback), I would need a certain gear ratio for torque (there is no payload to the end-effector; the only loads would be for all other axes' connections), and this would make it hard for someone to move the end-effector fast and efficiently.

To offset this problem, I have found online that I can use force control on the end-effector. The problem with this is that most 6-axis force/torque sensors are seriously expensive and way too industrial for my needs. I also have no idea how to implement this kind of sensor to the arm.

Are there any simpler ways to accomplish the task above? Any changes/recommendations to how I'm going about this? I plan for this to be a 5 DoF arm with a 16" reach and, as i said before, no end-effector payload.

答:
Fully integrated highly calibrated force torque sensors are complex and priced accordingly. However you do not need that level of precision for your application. If you're willing to do a little bit of electronics yourself you can make your own force feedback circuits using strain gauges.

If they're mounted on or near your end effector you can measure the strain on the link/mount and then use that as your force feedback. It will give you direct feedback on the forces applied to the end effector and you can align them with your primary axes to simplify the math for providing the feedback to your controller potentially. Alternatively you can possibly have one at each joint to provide the feedback necessary.


问:
A single screw axis of a rolling wheel - seems impossible...?
Chasles' theorem:

"Any rigid-body motion can be specified by a rotation about a screw axis and a translation parallel to that axis."

For a rolling wheel, the axis of rotation is perpendicular to the direction of translation. How do we reconcile these facts ?

答:
All velocities are relative to a specific frame.

You can describe the wheels motion relative to the vehicle body as purely rotation and no parallel translation.

You can alternatively describe the motion of the vehicle to the ground (assuming a basic cart going straight forward) as purely translation along a forward facing axis of the vehicle without rotation.

If you want the velocity of the wheel with respect to the ground. The vehicle is moving forward at velocity $v$ then the top of the wheel is moving at $2*v$ and the bottom of the wheel is moving at zero velocity then the axis is at the contact point on the ground. (The place where it has zero velocity.) And the rotational velocity is $v/r$ where $r$ is the radius of the wheel. And there's no translational component.

One critic part of the above theorem is that the axis of choice of the representation is not tied to the physical mechanisms causing the motion. In fact it may diverge significantly and also change moment to moment for a more complex trajectory.


问:
How to calculate serial speed and buffer requirements for PC to microcontroller communications?
A common scenario is to have a PC that sends commands to a microcontroller via RS232.  My PC program is sending commands (each of which are composed of multiple bytes) as fast as it can to a small robot.  The microcontroller on the robot is a Parallax Propellor.

I have noticed that if I don't process bytes quickly enough on the microcontroller side of things, it can very quickly overflow the default buffers in the popular serial port drivers that are available for the Propellor. (The buffers are generally anywhere from 16 to 256 bytes).  I can arbitrarily increase these buffers or create my own larger circular buffer, but I would like to have a more methodical approach to determining appropriate size requirements and/or the minimal amount of time I can wait before pulling bytes out of the serial port driver buffer.

At 1st glance:

115200 == 115.2 bits per millisecond == ~12.8 bytes per millisecond (assuming 1 stop bit)

1) Is that a valid way to calculate timing for serial transmissions?

Also, given my specific setup:

PC Program <--> Bluetooth Serial Profile Driver <--> Bluetooth Transceiver <-*-> BlueSMIRF Wireless Modem <--> Parallax Propellor Program

2) What is the maximum amount of data I can send for a given period of time consistently without eventually running in to problems?

Maybe I'm over complicating things, but it seems like there are potentially multiple buffers involved in the transmission chain above.  How do others commonly deal with this? Do they throttle the PC sending to a known safe rate? Implement flow control?  If implementing flow control, how does that affect bandwidth and response times?

(If it matters, my experiment is to use a joystick on the PC to control multiple servos with instant reaction to the joystick movements. So every small movement of the joystick results in multiple commands being sent to the microcontroller. The commands are not just simple positional commands though, they also involve acceleration/deacceleration of servos over time and this is the reason that the microcontroller spends a significant amount of clock cycles before processing new bytes.)

答:
There is no right answer to this, and I'm sure you already know (or can guess) all you need to solve this problem. However

The first obvious thing to say is that downstream devices must be capable of dealing with the flow of data, both in the long term and in the short term. In the short term, devices use buffering to cope with the flow. In the long term, the need processing power to act on the data.

One problem you have is that you don't control all of the steps in your chain, so if one of them is causing delays, there might not be much you can do about it, short of replacing it. Hoever, I'd have thought that the Bluetooth serial driver would have a nice big buffer, and would play nice with the transceiver and BlueSMIRF, so it's probably safe to ignore them. I guess the real problem is between the Propeller and the PC.

What seems to be happening in your case is that the interaction between the data producer and the consumer is producing unpredictable queue lengths, and you would like to apply some proper theory to that.

Interestingly, studies have been done on exactly this problem with regards to queuing at restaurants, E.G. Case Study for Restaurant Queuing Model and Equilibrium in Queues under Unknown Service Times and Service Value. But there are easier to understand resources online, like Queueing Theory Queueing Theory for Dummies. But the one you really want is probably How to size message queues.

You have a few options:

Just make sure that the PC sends data at an orderly rate. Decide exactly what rate you really need, and don't go above that.

Use a Realtime Operating System on the MCU to make sure that the bytes are being dealt with in a timely manner.

Implement flow control.

But here's my preferred solution. Spacewire! Or at least use the flow control system from it.

Essentially the downstream device sends bytes to the upstream device stating the number of empty places in its FIFO. This way the data producer only sends what the consumer can cope with.

If you're interested, you can read the full Spacewire standard: IEEE 1355.

ContentLicense: CC BY-SA 3.0


答:
Here is one toy case where off-diagonal elements are non-zero.

Consider a state vector that includes the position of both the left and right wheels instead of just a single position for the robot. Now if the left wheel has a position of 100m then you know the right wheel will also have a position of roughly 100m (depending on the axle length). As the left wheel increases position so will the right wheel, in general. It's not an exact 1:1 correlation, e.g. it doesn't hold exactly when the robot is turning, but overall it holds.

So here the off-diagonal entry between left wheel x-position and right wheel x-position would be close to 1.

Title: covariance matrix in EKF?

Body:
I'm struggling with the concept of covariance matrix.
$$
\Sigma
=
\begin{bmatrix}
\sigma_{xx} & \sigma_{xy} & \sigma_{x \theta} \\
\sigma_{yx} & \sigma_{yy} & \sigma_{y \theta} \\
\sigma_{\theta x} & \sigma_{\theta y} & \sigma_{\theta \theta} \\
\end{bmatrix}
$$
Now, my understanding for $\sigma_{xx}$, $\sigma_{yy}$, and $\sigma_{\theta \theta}$ that they describe the uncertainty. For example, for $\sigma_{xx}$, it describes the uncertainty of the value of x. Now, my question about the rest of sigmas, what do they represent? What does it mean if they are zeros? I can interpret that if $\sigma_{xx}$ is zero, it means I don't have uncertainty about the value of x.

Note, I'm reading Principles of Robot Motion - Theory, Algorithms, and Implementations by Howie Choset et. al., which states that

By this definition $\sigma_{ii}$ is the same as $\sigma_{i}^{2}$ the variance of $X_{i}$. For $i ≠ j$, if $\sigma_{ij} = 0$, then $X_{i}$ and $X_{j}$ are independent of each other.

This may answer my question if the rest of sigmas are zeros however, I'm still confused about the relationship between these variables for example $x$ and $y$. When does this happen? I mean the correlation between them. Or in other words, can I assume them to be zeros?

Another book namely FastSLAM: A Scalable Method ... by Michael and Sebastian which states

The off-diagonal elements of the covariance matrix of this
  multivariate Gaussian encode the correlations between pairs of state
  variables.

They don't mention when the correlation might happen and what does it mean?


问:
cartesian velocity control loop implementation
I'm using ROS (noetic) to intuitively control a franka manipulator using the panda_robot package for the simulation.

I've set up an extended kalman filter which fuses the following measures:

IMU data: the sensor is attached to my arm and reports the orientation of my hand wrt the (virtual) base link of the manipulator, along with the angular velocity

position data coming from my mouse. The z coordinate is controlled through the mouse wheel

linear velocity obtained by differencing the position of the mouse. As for the position data, the velocity is with respect to the (virtual) base link of the manipulator, which corresponds to the origin of the world in the Gazebo simulator.

The filter outputs an odometry message, which I'm trying to exploit to implement the following cartesian velocity control loop:

where $\dot{x}^*$ is the target velocity in cartesian space, while $x^*$ is the target position in the cartesian space. Both quantities are obtained from the kalman filter. Essentially, the filter acts as the reference generator depicted on the image, sending commands at about 100 Hz.

The code for the robot control loop is the following (using rospy):

```<code>    import rospy
    import numpy as np
    import tf
    ...

    def on_cmd(self, cmd):

      # cartesian speed and position obtained by the kalman filter
      tgt_pose = np.array(cmd.tgt_pose)
      tgt_twist = np.array(cmd.tgt_twist)

      # get current end-effector position and orientation in cartesian space
      ee_position, ee_orientation = self._panda_arm.ee_pose()
      ee_orientation = self._quat2array(ee_orientation)         # w,x,y,z --> x,y,z,w
      ee_orientation_rpy = tf.transformations.euler_from_quaternion(self._ee_orientation)
      ee_pose = np.concatenate([ee_position, ee_orientation_rpy])

      # get the jacobian for the current joint configuration
      J = self._panda_arm.zero_jacobian()
      J_inv = np.linalg.pinv(J)

      pose_error = tgt_pose - ee_pose

      # self._K is a 6x6 diagonal positive-definite matrix
      twist_error = tgt_twist + self._K @ pose_error

      joint_velocity_cmd = J_inv @ twist_error

      # finally execute the command
      self._panda_arm.exec_velocity_cmd(joint_velocity_cmd)
</code>```

$K$ is chosen to be a 6x6 diagonal matrix with values of 0.01.

By means of rviz, I can see that the cartesian speed and velocity produced by the kalman filter seem to be correct. Still, I'm struggling to make the manipulator move accordingly to my mouse motion and my arm orientation. The robot is currently able to just move back and forth following the mouse, but it is basically ignoring the orientation. The result is that it appears to move randomly when I attempt to perform some more advanced moves (like rotating the gripper of the end effector).
I've come to the assumption that the commands value provided by the filter are correct, so I think that there should be some error in my implementation of the control loop. I'd like to ensure that the above code snippet does make sense, at least conceptually.

答:
The orientation error is not simply subtraction of your current pose and the desired pose. Given $\mathbf{R}_{a}, \ \mathbf{R}_{d} \in \text{SO}(3) $, where $\mathbf{R}_{d}$ is your desired orientation in matrix form and $\mathbf{R}_{a}$ is your current orientation in matrix form, you can compute the orientation error as:

$$ \mathbf{e}_{o} = 0.5 \cdot\sum_{i=1}^{3} \mathbf{r}_{a,i} \times \mathbf{r}_{d,i} $$

where $\mathbf{r}_{a,i}$ denotes the $i^{\text{th}}$ column vector of $\mathbf{R}_{a}$, likewise for $\mathbf{r}_{d,i}$ and $\mathbf{R}_{d}$, and $\times$ denotes the cross product. This will convert any rotation to the appropriate axis-angle representation of error - capable of being used in conjunction with the end-effector Jacobian.

The following image details converting from RPY velocity to angular velocity:


问:
Simulator that allows your to test your real firmware inside a simulated world?
I am interested in learning more about how robotics are tested in simulators. Reading the Wikipedia article on robotics simulators I can't tell how the onboard firmware (the "brains" of the robot) are actually tested inside the simulator.

Every simulator I've found from my online searches seems to want you to program the robot inside the simulator, and then export that code as firmware to the actual robot. Or even worse, maintain duplicate codebases (one codebase for use inside the simulator, and another one for the actual firmware flashed to the robot and used in "production").

Are there any simulators that allow you to upload your firmware (and a 3D model of your robot) into a simulated world, and see how your actual, production firmware behaves in that simulated world?

To be clear, I'm talking about the following general setup:

The firmware is running on some type of production-like MCU/SOC

Instead of hardware inputs (cameras, microphones, lidar, etc.) being wired to the MCU/SOC, the machine that the simulator is running on (a.k.a. the "simulator machine") is wired up to those same ports/pins

Instead of hardware output (motors, servos, speakers, etc.) being wired up to the MCU/SOC, the simulator machine is wired up to those same ports/pins

So essentially the firmware is running in environment where its hardwired to speak to the simulator machine

The simulator renders the 3D world for the robot-under-test and sends each input frame over the wire to the MCU/SOC/firmware

The firmware processes the input as it normally would, in production (the real world)

The firmware generates outputs to its output pins/ports, which get picked up and interpreted by the simulator

So for example, when the test starts, the firmware is waiting for video frames from a camera. The simulator renders those frames and sends them over the wire to the MCU/SOC where the firmware is running. The firmware decides it wants to explore the world, so it sends commands that would normally turn its motors on to move forward. But because the associated pins/ports are connected to the simulator machine, the simulator receives those commands, and (inside the 3D world) moves the robot forward accordingly, and re-renders its view, sending new video frames back to the firmware's input pins/ports.

Update in response to Chuck's answers below

I'm not understanding why I would need to plop my firmware onto a hardware emulator for such a hypothetical system. Say I am running on firmware (in production) on some MCU/SOC, say the BuggyBoard 9000. And lets say my robot is super simple: it hooks up to an RGB camera as its only input, processes video frames from it, and outputs different sounds (based on what it "sees" in the video captures) to a speaker (its only output) that is connected via a few wires soldered onto some I/O pins on the board. So in production, I have a USB camera connected to the Buggboard 9000 at one of its USB serial ports. The firmware is using USB device drivers to read the camera's video frames and do visual processing with them. When it detects something and decides it needs to make a sound, it uses its own algorithms and the speaker's device driver to send sound to the connected speaker. But this is in production. In my simulator/test harness however, I simply have the BuggBoard 9000 connected to a power source and its running my firmware. Instead of a USB camera I have a USB cable connected to the BuggBoard 9000, where the other end is connected to the simulator machine. Instead of the soldered-on speaker, I have those same I/O pins soldered to wires going to a dongle that also connects to another port on the simulator machine. The simulator renders some video frames and sends them to the USB port connected to the BuggyBoard 9000. The firmware uses device drivers to read the video frames into memory and do its visual processing. It sends sounds through the device drivers, down into the I/O wires, the dongle and back into the simulator, where perhaps something in the simulated environment will respond to certain sounds being broadcasted.

答:
:EDIT: - In response to the question edit,

In my simulator/test harness however, I simply have the BuggBoard 9000 connected to a power source and its running my firmware. Instead of a USB camera I have a USB cable connected to the BuggBoard 9000, where the other end is connected to the simulator machine.

Yeah, but here you need to code the camera-side of the USB protocol. You can probably get the camera USB driver from the manufacturer, but you're 99.99% guaranteed to not be able to get a copy of the camera-side USB code, which means you get to reverse engineer the communication protocol that the camera is using. You might "luck out" in that it's some kind of standard protocol, like RTSP, but could also wind up being something that's specific to that particular camera model. This also means you need to at least verify and probably re-write the whole camera-side USB spoofing code any time you change camera models.

Likewise for the speaker, you write an analog-to-digital converter to capture the waveform and then need to implement some streaming protocol (your choice!) to encode it, transmit it to the simulator, then you need to decode it in the simulator.

The time you spend designing the spoofing hardware, software, simulator interface, etc. is time that you will never get back, and doesn't directly help your project beyond being able to test your device without making changes.

If instead you modularized your code, then you could do something like run ROS connections between the different aspects of your code. If you had a USB camera connected to a dedicated microcontroller that encoded the images to a ROS video stream, now your core logic isn't doing anything with USB drivers. It subscribes to the ROS image topic and now your system becomes cleanly testable. You can easily spoof the ROS message and test your business logic without an actual camera. You can create a debug subscriber and test your camera-to-ROS encoding.

You can do the same for playing the sound - you can create the speaker driver as a ROS subscriber, and your business logic can send a ROS message with an enum or similar that would indicate to the speaker driver which sound to make. Again, that's really easy to spoof and again breaks the hardware dependence. You can run your business logic on the BuggyBoard 9k, you can do it as a desktop application, in a docker container running on AWS, etc. Doesn't really matter because it's modular.

== Original Post ==

simulation! This is exactly my domain.

Firmware implies hardware. The firmware that runs on a TV remote isn't the same as the firmware that runs on a network router, and you couldn't run firmware from one device on the other.

There are, generally speaking, two levels of simulation:

Software-in-the-loop (SIL), where you execute your program on some platform that's NOT the actual device you're going to use in production, and

Hardware-in-the-loop (HIL), where you ARE using the production hardware.

A simulator has some virtual environment, and then you would add virtual sensors to that environment. You create a kind of avatar that is the simulated device, and as your program runs it signals the avatar to take some action.

I've done every level of testing here, from a basic simulator that is capable of testing some subsystem on a product, to full-scale hardware-in-the-loop testing that has multiple racks of PLCs and network switches setup as they'll be configured at a customer site.

The real difficulty with HIL testing is replicating the sensor OEM packet structure. For example, at a previous employer we did full-scale HIL testing with a bank of SICK brand lidars. Those devices have an internal state machine and a particular boot sequence, complete with a handshake protocol that steps through that internal state machine. In order to run our software with no changes, we had to fully implement the internal state machine, all the status messages, etc., in addition to "just" the raw lidar packets.

Depending on how "low" you want to get with the HIL testing, you'll also need to recreate all the motor encoders, limit/whisker switches, circuit breaker and contactor status, bus voltage readings, etc.

Part of what you can do for yourself is to structure your hardware in such a way that you segregate your core program logic from the I/O handling. If you had one board that handles I/O and forwards that I/O data as an Ethernet message, then you get the opportunity to just send I/O packets from your simulator that are structured in the same way as the physical I/O handler and life gets dramatically easier. If your core logic and I/O handling are on the same board then you'll wind up needing a kind of "translator" board. This translator would receive I/O status data from the simulator and would generate digital and analog signals that you can wire to your production hardware to simulate all those encoders and other feedbacks.

Kind of similarly from the other side, if your core logic is also directly interacting with I/O pins or other hardware-specific aspects of your platform then you're going to have a really hard time performing SIL testing, because that code won't work as-is if you run it as a desktop application. You can't just change your compiler options and get it to run, and this is probably where you've read about some people/companies that need to keep a "production" and "test" version of their code, where the difference is whether or not the code is tethered to the hardware platform. Again, careful structuring on your part can mitigate this and really let your exercise your core logic on an arbitrary platform.

Regarding your question,

Are there any simulators that allow you to upload your firmware

The thing you're probably looking for is "hardware emulation," but the results there are pretty limited. I can find stuff for the Raspberry Pi or Texas Instruments' MSP-430 (no affiliation to either company), but beyond those you're probably going to be pretty limited on what you can find for hardware emulation. Even then, I don't know that you're going to be able to easily interface the emulator to a virtual environment.

Back to your question again:

So for example, when the test starts, the firmware is waiting for video frames from a camera. The simulator renders those frames and sends them over the wire to the MCU/SOC where the firmware is running.

Yup, your simulator captures the scene, encodes those captures appropriately, and then sends that data over the appropriate connection - TCP/IP, RealTime Streaming Protocol (RTSP), etc. Again, the difficulty isn't so much with encoding the data and sending it but (for example, with RTSP) the state machine and handshake associated with making the connection, handling messages beyond frame transmission like DESCRIBE, SETUP, PLAY, TEARDOWN, etc.

The firmware decides it wants to explore the world, so it sends commands that would normally turn its motors on to move forward.

Yeah, and again you need a way to receive those commands, maybe also virtual encoders to report wheel speeds or motor speeds, etc. All those also need to be packaged correctly.

But because the associated pins/ports are connected to the simulator machine, the simulator receives those commands

This is the hand-waving part where the real effort happens. If your production platform's software is using hardware I/O to generate outputs, then you'll need a way for the simulator to read that hardware I/O. As I mentioned above, this is typically some OTHER piece of hardware that will make up part of your test stand. The other hardware will read the System Under Test (SUT) outputs, convert to some network message the simulator can read (ROS message, TCP/IP socket connection, etc.). The simulator runs, generates its own outputs (like wheel encoders), sends those outputs to the translator, which then generates the square wave, analog signal, gray code, etc. - whatever the appropriate representation is that your SUT is expecting.

These are generally all specific to whatever system you're trying to test and I think everyone wants to make their own Simulator. Gazebo is supposed to be a simulator that has a lot of stock sensors, but (1) probably won't have all the sensors you're using, and (2) almost certainly won't "just work" with your production hardware out of the box.

You'll need to tailor the simulator to your needs and if you're that work then I'd highly recommend you evaluate a suite of simulation environments to determine what's going to work best for your application. Pre-built sensors aren't going to cut it if what you really need is photorealism, etc.


问:
AWS RoboMaker simulations + testing explained
I am trying to understand how AWS RoboMaker and WorldForge work, and am having trouble "seeing the forest through the trees," and might need someone to "explain it to me like I'm 5" (ELIF).

Do you:

Write your robot's firmware (in any language/library/RTOS you choose) and upload it to AWS and run it through their simulations/tests/workbenches? or...

Write your robot's firmware in Python (which seems to be the only language they currently support, at least in the examples I can find), use their Python libraries/modules for interacting with their WorldForge simulations, and upload your Python code to AWS and run it through their simulations/tests/workbenches; or...

Something else completely different?

Thanks for any clear + succinct (good step-by-step examples would be amazing!) steering here!

答:
If you take a look at their Getting started > Concepts section, they mention

A typical simulation might use Robot Operating System (ROS) with one container simulating the environment in Gazebo, and a second container simulating the robot.

As I discuss in my answer to your other question, your robot needs some way to interact with the virtual environment. By the way it's described above, Amazon is assuming you're going to have some network I/O that will bridge your robot to the simulated environment. The simulated environment would be what hosts your virtual robot, with all the virtual sensors and encoders, etc. The virtual environment would all be put on one docker container.

The robot (as described by Amazon in the quote above) would run on another docker container. Your robot runs on whatever software you choose to write, Python, C++, etc., and it communicates over that network I/O to the simulator, which will generate synthetic feedback based on whatever your virtual sensors are programmed to do.


问:
Sensors, ports, pins, drivers and firmware by example
I am looking to just make sure I correctly understand the fundamentals for how robotic systems typically receive and process inputs from sensors.

It is my understanding that a sensor peripheral, such as a camera or a temperature gauge, will be built to either send its data over port (perhaps a serial port such as USB) or over individual wires that must be soldered/connected to the I/O pins of the controller running the robot's firmware/software. So perhaps the camera is a USB camera that expects to send RGB video frames over the attached USB cable to whatever is listening at that USB port. Or perhaps the temperature gauge is sending real-time temp readings over a few wires to whatever those wires are connected to.

It is my understanding that the robot's firmware must use device drivers to read/write values over its I/O pins for talking with these sensors. So both the camera and temperature gauge must have device drivers so that the firmware must talk to it. In the case of a USB camera, the USB drivers almost assuredly already exist for the OS, unless its a real-time OS that for whatever reason doesn't have USB drivers written for it. And in the case of the temperature gauge, either the manufacturer would write and provide device drivers for the gauge's use on numerous/common OSes, or I would have to write my own device driver for the gauge myself. In that case I could write a device driver to expect listen and read for signals coming from, say, pins 9-14, and to send any values written to those pins (by the temp gauge) on to a memory buffer that my firmware could read them out of.

So I ask: how'd I do? Is my understanding of this process correct? Or is it grossly midled or off-base somehow? Thanks in advance for any steering or course correction!

答:
Yes.  Your understanding is correct.  But I would like to clarify some things.

The "brains" of the robot: software, controller, firmware, etc. on the robot can come in many forms.

Simple robots might just have a bare-metal microcontroller.  An Arduino for example.  Which typically have direct control over GPIO digital and analog pins.  As well as communication buses such as RS232, I2C, etc.  There is usually nothing like an OS here.

More advanced robots might run a full microprocessor like a laptop.  In this case there is most definitely an OS and the hardware interface is restricted to the standard communication protocols: USB, RS232, ethernet, etc.

Lastly, there are things like RaspberryPi which are kind of a mix between the two.  The processor is powerful enough to run a "real" OS, support a wide range of communication buses/protocols, as well as provide some low-level GPIO.

On the other side, sensors come in a wide range as well.

A temperature sensor can be as simple as a variable resistor.

Or as complicated as a WIFI connected IOT device.

So you see that connecting one to the other is highly dependent on your hardware / application.

Interfacing hardware with the computer is a common problem in robotics.  (Note that this goes for motor outputs as well as sensor inputs).  Usually what is done is to have an intermediate processor.  For example a servo controller, or a sensor hub.  Which is basically just a microcontroller to interface with the hardware, and some communication protocol with API to read the senor or drive the motor.


问:
How to distribute ros package developed on windows?
I am using ros1 neotic on windows 10 to develop ros package with python. I checked this link Packaging your ROS project as a snap but it's telling only how to share packages developed on linux machine. How can can I share my package developed on Windows?</p>

答:
Unfortunately without a well established package manager there's no canonical way to make releases there. You can tag and tell people to build from source. And you can build binaries that you redistribute via windows installers as long as you make sure that they bundle in the necessary dependencies.

The primary ways to release ROS 1 are documented here: http://wiki.ros.org/ROS/ReleasingAPackage (For others future reference ROS 2 release documentation is here: https://docs.ros.org/en/rolling/How-To-Guides/Releasing/Releasing-a-Package.html )

If you release packages into the main distributions and there are two mechanisms to also build and release them onto Windows.

The one where you can most actively participate is via conda forge and the RoboStack project. It was announced on ROS Discourse here

Microsoft also periodically builds a number of packages distributed via the ROS on Windows project where you could contribute to as well.

And if your package is well developed and becomes a core package in the ROS ecosystem it may also be promoted to the main development bundles. But that's a much longer process than what it sounds like you're looking for and would require cross platform capabilities and a large user base.


问:
Compensating for Yaw in Lateral Quadcopter Movement
I'm trying to make a quadcopter move laterally at a certain angle. I've been able to find the proper roll and pitch angles for this (that work with a yaw of 0°); how would I adjust these values to compensate for a different yaw?</p>

答:
Think of it this way, if you had 90$^\circ$ yaw you would swap roll and pitch, and maybe switch the sign depending on your coordinate system definition. Once you think through that, you just use the yaw angle to interpolate for arbitrary yaw angles.

$$
\begin{align}
\phi' &= \cos(\psi) \phi + \sin(\psi) \theta
\\
\theta' &= -\sin(\psi) \phi + \cos(\psi) \theta
\end{align}
$$

The $\pm$ signs might be off, but you get the idea.


问:
Euler Angles Order For Quadrotor Modelling
I am modelling a quadrotor and I need to choose an order for the rotations that transfer vectors which are represented in Earth Frame to the Body Frame.

what is the most logical order for these rotations?

which order is likely used?

does the order have a big effect on the control of the quadrotor?

Thanks in advance for any answers

答: Yes, it has an effect on the controller if you are going to take advantage of the simple model and small angles assumption. What matter most is that the yaw should go first in the order. because the yaw angle unlike the other two is not going to be held close to zero. Imagine we are at a pitch angle of 10deg and then a 180deg of yaw is commanded, if a rotation order of YZX (for example) is defined, things become upside down and a 10deg of pitch would be in the wrong direction making the quad title toward the wrong direction if not going unstable. I was suffering from inability to control the quad in simulation every time I set the yaw to far from zero angle until I figured out I was setting the wrong rotation order.</p>


问:
Stabilizing water bucket with two retractable cables and motors
I am trying to design a system in which a bucket of water is held by two cables, lifted up/down while remaining stable. The cables can be retracted/detracted by two individual motors, controlled by their own PID system. The distance the cables travel is measured by an encoder that provides a discrete signal.

However, when I set a target distance (e.g., lift 10 cm), the output of the PID system controlling the motor speeds are different due to a difference in load on each motor. As a result, the bucket is destabilizing and water is leaking out.

What is the best way to ensure that the distance traveled by the bucket follows a smooth curve (i.e. no large changes in motor speed) and still remains stable? I am not looking for a complete solution to my problem, just a direction of terms and theory to look for. Thank you.

答:
Tully's response is spot on

I'd only like to add a few comments:

Improving the hardware setup as Tully suggested is definitely the first way to go. However, the controllers certainly need some careful tuning.

It's important to select the right reference position trajectory to your system to minimize the jerk. To this end, you should rely on the well-known 5th-order polynomial (see also https://robotics.stackexchange.com/a/21571/6941).

There are MIMO approaches that are suitable to this case, but the simplest way of coupling the two PIDs is:

Pick one PID as the primary and keep using it standalone.

Send the exact output of the primary PID to the secondary motor as the feed-forward term and provide a further feedback term with the secondary PID that is instructed to reduce the error $\text{distance}_1 - \text{distance}_2$.   If the PIDs (especially the secondary) are properly tuned, then the former error is kept low so that the bucket will remain stable.


问:
Nvidia Jetson Nanon GPIO c++ direct memory interfacing
Is there's any library to interface the Jetson Nano GPIO?
There's the official Python interface, and there is an unofficial c++ interface which is working through the filesystem, and I'm interested in the direct register access.
There is a post here by someone who claims he has achived it, but before I re-imlement the reference manual, It would be great if someone already did it.</p>

答: I created a library that does just that, mapping the tegra CPU registers in user space. My lib is here: https://github.com/Rubberazer/JETGPIO it can be called from C or C++</p>


问:
Finding a point in a new reference frame <modern robotics>
From modern robotics chapter 3.2 and figure 3.7, how do we obtain $p_{b}=\begin{bmatrix}1 & -1 & 0\end{bmatrix}^{T}$ from $R_{b}$ and $p_{a}$? Because $R_{b}*p_{a} =\begin{bmatrix}-1 & 1 & 0\end{bmatrix}^{T}$ which is not $p_{b}$...

答: One of the usage of rotation matrices is to represent the orientation of one frame relative to another. If we would like to represent the orientation of frame {$b$} relative to frame {a}, we need $R^a_b$, so if we have a vector in frame {$b$} (i.e. $p_b$) then the same vector expressed in frame {$a$} is $p_a=(R^a_b) p_b \implies p_b=(R^a_b)^{-1} p_a$. Notice $(R^a_b)^{-1} = (R^a_b)^T$ which is more efficient than the inverse.


问:
angle rates wrt another frame
My manipulator has a gyroscope attached to its end-effector (ee). The sensor provides angular velocity in the form of angular rates. Those rates are wrt fixed axis, so they effectively represent roll-pitch-yaw extrinsic sequence of angular rates. Let's call this angular velocity $\dot{\gamma}$.
Now, $\dot{\gamma}$ is expressed with respect to the end-effector frame.
For my control scheme I have to compute the joint velocity command, $\dot{q}$, as follow:
$$\dot{q} = J_{A}^{-1}\dot{x}$$
where

$J_{A}^{-1}$ is the inverse of the analytical Jacobian, which relates cartesian velocities with respect to the base link to joint velocities

$\dot{x}$ is the cartesian velocity, whose angular component is not the angular velocity vector commonly denoted as $\omega$, but it's the time derivative (i.e. angle rates) of a minimal representation of orientation of the end-effector with respect to the base link. In this application, the angular component should be the above mentioned $\dot{\gamma}$

Here's the issue: $\dot{\gamma}$ is the angular rates expressed in the end-effector frame. Let's denote it as $^{ee}\dot{\gamma}$. For my computation, I need it to represent the angular rates wrt the base link of the manipulator (i.e. $^{base}\dot{\gamma}$).

How can I achieve this change of reference frame for the angular rates in order to use it with the analytical jacobian?

I'm also given $^{base}R_{ee}$, which is the matrix expressing the orientation of the end-effector frame wrt the base link frame. If $^{ee}\dot{\gamma}$ was an element of a vector space, it would be simple to change the frame of reference:
$$^{base}\dot{\gamma} \text{  =  }  ^{base}R_{ee} \text{  } ^{ee}\dot{\gamma}$$
But since that's not the case, I don't think that multiplication is even feasible from a mathematical point of view.

答:
It sounds like part of the problem here is what frame the RPY derivative, $\dot{\mathbf{x}}_{RPY}$, is in. Considering the fact that the RPY values, lets call them $\psi, \theta, \phi$ respectively, are used to parameterize the rotation matrix between the world frame and the end-effector frame - it is unlikely then that $\dot{\mathbf{x}}_{RPY}$ is defined in the end-effector frame. Consider

$$ ^{base}R_{ee}(\psi, \theta, \phi) = \ R_{z}(\psi) R_{y}(\theta) R_{x}(\phi)  $$

So a change in these parameters is perhaps best not viewed as being in one frame vs another, but instead as changes in elementary parameters that effect the final orientation. That being said, if you use the analytical Jacobian you should not have to perform any change of bases for $\dot{\mathbf{x}}_{RPY}$ as these variables are the change in bases.

Now, for my personal schpeal on why the geometric Jacobian is better here. RPY is traditionally used for aircrafts and the like (the naming convention being a clear indicator) and they generally perform well in this arena. This is because it is highly unlikely that a passenger jet reaches the RPY singular configurations ($\theta = \pm \frac{\pi}{2}$) - as this would mean the plane is pointed "upwardly" or "downwardly" or perhaps "about-to-crashedly". So, for objects who are generally constrained to certain orientation ranges this should not be a problem. However, revolute-joint robots are not at all orientationally challenged - and as such find themselves in a wide range of orientations. In fact, it is within reason to assume that every orientation (i.e. all of $\text{SO}(3)$) is reachable for specific locations in a 6 DoF robot's workspace (up to self-collision and joint limits of course).

That being said, using a representation that contains singularites should be avoided if it can. Also note, the analytical Jacobian is not technically valid at these representational singularities (if you have access to Spong's robotics textbook check out section 4.8). We know how to convert RPY derivatives to angular velocity (here), the geometric Jacobian is required to compute the analytical Jacobian, and the analytical Jacobian can be undefined - angular velocity may be difficult to conceptualize at first but the math is better defined than its parameterized counterparts. As a final note, once the angular velocity has been computed from the RPY derivatives, this will be in the base frame - so no re-framing necessary.

Title: angle rates wrt another frame


问:
Does PID controller work on every actuator
I am new to PID controller, so I got very confused. I am trying to solve the Cartpole problem. I saw this simple solution for that. I don't understand why that guy used sigmoid to determine which action to take, does the PID controller work on every actuator? (in this case, he used sigmoid, which is so confusing)</p>

答:
does the PID controller work on every actuator?

The short answer to this is yes.

PID controllers are used to stabilise a plant/system. They are heavily used in different industries, though they can be a bit costly to implement sometimes. Drone Industry (UAV / drones) is one of them. PID controllers would help them to stabilize and auto-hold (or altitude-hold some might say).

why that guy used sigmoid to determine which action to take

Now to answer this question we have to look into the code posted in the GitHub link. Let's look into the code snippets from HenryJia's repository.

```<code>def sigmoid(x):
return 1.0 / (1.0 + np.exp(-x))
</code>```

and,

```<code>pid = np.dot(P * error + I * integral + D * derivative, desired_mask)
action = sigmoid(pid)
</code>```

In these snippets, he/she/they used the sigmoid function to limit the coefficients of the PID controller within the predefined range of the sigmoid function. This would help them to keep the PID coefficients within the stable range so that it doesn't get too high or too low to reach the verge of instability.

Body:
does the PID controller work on every actuator?

The short answer to this is yes.

PID controllers are used to stabilise a plant/system. They are heavily used in different industries, though they can be a bit costly to implement sometimes. Drone Industry (UAV / drones) is one of them. PID controllers would help them to stabilize and auto-hold (or altitude-hold some might say).

why that guy used sigmoid to determine which action to take

Now to answer this question we have to look into the code posted in the GitHub link. Let's look into the code snippets from HenryJia's repository.

def sigmoid(x):
return 1.0 / (1.0 + np.exp(-x))

and,

pid = np.dot(P * error + I * integral + D * derivative, desired_mask)
action = sigmoid(pid)

In these snippets, he/she/they used the sigmoid function to limit the coefficients of the PID controller within the predefined range of the sigmoid function. This would help them to keep the PID coefficients within the stable range so that it doesn't get too high or too low to reach the verge of instability.


问:
Bluetooth module HC-05 giving ERROR :(0)
I am working right now with Arduino UNO and HC-05 bluetooth module.I followed the instruction given on this link for wiring. So there are 2 mode of working with this HC-05 module

Simple serial communication

Working in AT command mode so as to change the parameters of HC-05 module

As long as I work in simple serial communication mode, everything works fine but when I tried to change the parameters of module, it didn't work out. For working in At command mode, PIN NO 34 of HC-05 module needs to be high which I had taken care of. Lately I found that in mu module they had knowingly not connected the Berg strip to PIN 34, so I connected the PIN directly, even though I am not able to change the parameters of module and when I write any command on COM port of arduino IDE, I get this response

```<code>Enter AT commands:
ERROR:(0)
ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿõÿýì¢^
ERROR:(0)
ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿõÿýì¢^
ERROR:(0)
ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿõÿýì¢^
ERROR:(0)
ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿõÿýì¢^
ERROR:(0)
ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿõÿýì¢^
ERROR:(0)
ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿõÿýì¢^
</code>```

I think that garbage is due to my code

Here is my code:

```<code>#include <SoftwareSerial.h>

SoftwareSerial BTSerial(10, 11); // RX | TX

void setup()
{
  pinMode(9, OUTPUT);
  digitalWrite(9, HIGH);
  Serial.begin(9600);
  Serial.println("Enter AT commands:");
  BTSerial.begin(38400);
}

void loop()
{
  uint8_t x;
  char CommandFromSerial[50]=" ";
  char ResponseFromBluetooth[50]= " ";

  if ((Serial.available())){
    if(Serial.available()>0){
      for(x=0;x<50;x++)
        CommandFromSerial[x]=Serial.read();
      BTSerial.println(CommandFromSerial);
    }
  }

  if ((BTSerial.available())){
    if(BTSerial.available()>0)
      for(x=0;x<50;x++)
        ResponseFromBluetooth[x]=BTSerial.read();
    Serial.println(ResponseFromBluetooth);
  }
}
</code>```

I am not able to figure out what I am doing wrong. I used this command on COM port
AT\r\n and many other commands but every time I get the same response.

Did I mess up with my bluetooth module unknowingly?

答:
You're getting garbage in because the module is running at 9600 baud. In order for the module to operate at 38400 baud, pin 34 must be set high before the module is powered on. It looks like you are trying to set the pin high in Setup(), but it's already too late by then.

Edit: I see now that he hardwired pin 34 high and he is getting valid data read back from the module.  The following fixes still apply if you need a way to ensure the baud rate of the module.

You can fix this in a few ways:

use 9600 baud in your code

wire vcc directly to pin 34 so it's always in 38400 baud mode

change the default uart speed using the AT command that does this (can't remember)

tie the hc-05 reset pin to an Arduino output pin so you can set pin 34 high and then toggle the reset which will ensure the module goes into 38400 mode.

Edit:  Assuming you have everything connected properly and the baud rates set correctly, the following code is known to work on the Arduino Uno.

```<code>/* Serial Loop */

#include <SoftwareSerial.h>

#define rxPin 8
#define txPin 9

SoftwareSerial mySerial(rxPin, txPin); // RX, TX
char myChar;

void setup() {
  Serial.begin(9600);
  Serial.println("AT");

  mySerial.begin(38400);
  mySerial.println("AT");
}

void loop() {
  while (mySerial.available()) {
    myChar = mySerial.read();
    Serial.print(myChar);
  }

  while (Serial.available()) {
    myChar = Serial.read();
    Serial.print(myChar); //echo
    mySerial.print(myChar);
  }
}
</code>```

Here is the circuit that goes with the code. The pin labled "Key" is pin 34.

Body:
You're getting garbage in because the module is running at 9600 baud. In order for the module to operate at 38400 baud, pin 34 must be set high before the module is powered on. It looks like you are trying to set the pin high in Setup(), but it's already too late by then.

Edit: I see now that he hardwired pin 34 high and he is getting valid data read back from the module.  The following fixes still apply if you need a way to ensure the baud rate of the module.

You can fix this in a few ways:

use 9600 baud in your code

wire vcc directly to pin 34 so it's always in 38400 baud mode

change the default uart speed using the AT command that does this (can't remember)

tie the hc-05 reset pin to an Arduino output pin so you can set pin 34 high and then toggle the reset which will ensure the module goes into 38400 mode.

Edit:  Assuming you have everything connected properly and the baud rates set correctly, the following code is known to work on the Arduino Uno.

/* Serial Loop */

#include <SoftwareSerial.h>

#define rxPin 8
#define txPin 9

SoftwareSerial mySerial(rxPin, txPin); // RX, TX
char myChar;

void setup() {
  Serial.begin(9600);
  Serial.println("AT");

  mySerial.begin(38400);
  mySerial.println("AT");
}

void loop() {
  while (mySerial.available()) {
    myChar = mySerial.read();
    Serial.print(myChar);
  }

  while (Serial.available()) {
    myChar = Serial.read();
    Serial.print(myChar); //echo
    mySerial.print(myChar);
  }
}

Here is the circuit that goes with the code. The pin labled "Key" is pin 34.


问:
definition of self motion
From my understanding, self motion refers to a set of joint velocities which doesn't cause any cartesian motion on the end-effector. Here, by motion it is meant both linear and angular components or just the linear one?

For example, in the Franka Emika Panda manipulator (7 DOF), if I move only the last revolut joint, I make the end-effector rotating in-place (i.e. its orientation changes while its position not). The cartesian linear velocity is obviously null, but the end-effector is subject to a non-zero angular velocity. Can we still talk about self motion?

答:
The example you have given is not self-motion - and I believe you understand why already. Even though the end-effector does not change position, it does change orientation. Mathematically speaking, this means the output of our end-effector forward kinematics function has changed with our most recent motion. So - we can simply define null-motion (self-motion) as any joint velocity which once applied to our robot's joints do not change the output of our forward kinematics. This is equivalent to the following constraint:

$$ \dot{\mathbf{x}} = \mathbf{J}(\mathbf{q})\dot{\mathbf{q}} = 0 $$

where $\dot{\mathbf{x}}$ is the end-effector task space velocity, $\mathbf{q}$ is the current joint configuration, $\dot{\mathbf{q}}$ is the current joint velocity, and $\mathbf{J}(\cdot)$ is the geometric Jacobian. Now, what aspects of the forward kinematics function you care about are up to you. If you do not care about orientation, one might relax null-motion to the following constraint:

$$ \dot{\mathbf{x}} = \mathbf{J}(\mathbf{q})\dot{\mathbf{q}} = \begin{bmatrix} 0 \\ \omega \end{bmatrix} $$

where $\omega$ is some arbitrary angular velocity.

Some notes:

If the number of joints your robot has is equal to the number of task space degree's of freedom, null-motion is only achievable at singular configurations.

Given $n$ joints, $m$ task space DoFs, and $n \gt m$ - The null space or null space projection matrix can be solved as $(I - \mathbf{J}^{\dagger}\mathbf{J})$, where $\mathbf{J}^{\dagger}$ is the pseudo-inverse of the geometric Jacobian. Any $\dot{\mathbf{q}}$ such that $\dot{\mathbf{q}} = (I - \mathbf{J}^{\dagger}\mathbf{J})\dot{\mathbf{r}}$, where $\dot{\mathbf{q}}, \dot{\mathbf{r}} \in \mathbb{R}^{n}$, will result in no end-effector motion. see here.

When $n \gt m$, null-motion lies on what are referred to as self-motion manifolds. These manifolds are typically of $n-m$ dimension and act as continuous solutions to the inverse kinematics problem. This also means that we can continuously move through our null-space without discontinuity - although for the case of revolute joints we will return to our original starting configuration eventually.

Title: definition of self motion


答:
Don't think roll and pitch.  think of the level flight case where the vertical axis of the 'copter is vertical.  Now if you want to move in any direction at all create a plane that contains your vertical axis (so this is a vertical plane) and also contains your desired new direction.  two line now define a plane.   You need to tip the 'copter's vertical axis while keeping ti on that plane

Looking down on this plane from high up on your z-axis the plane looks line a line drawn on the x,y plane pointing to your desired new heading.  The line is has some angle which equals the change in heading you want call it theta.

the rotation will have a pitch and roll component and you must distribute the total amount of tilt between pitch and roll.   that is sin(theta) and cos(theta)
Does this make sense?  Try it: what if you want to turn 90 degrees then sin(90)=1 and cos(90)=0 this means to so "due left" you roll.   To move forward is is pitch one because sin(0)=0 and cos(0) = 1.    At 45 degree heading sin(45)=cos(45) so equal amount of pitch and roll.

Now you can go any direction take the total rotation and proportion it between pitch and roll be multiplying by (total rotation)(sin(theta)) and cos for roll.

Title: Compensating for Yaw in Lateral Quadcopter Movement

Body: I'm trying to make a quadcopter move laterally at a certain angle. I've been able to find the proper roll and pitch angles for this (that work with a yaw of 0°); how would I adjust these values to compensate for a different yaw?</p>


问:
Lawn Mower Robot Boundary Wire Current
Robot Lawn Mowers have those Boundary Wires where a weak current runs through. This creates a magnetic field around it. The Robot then detects the magnetic field and knows it has reached the end of the lawn.
I want to use the same system to navigate my robot.

I did some calculation which I'm not sure about if I did it right.

If I run 100mA through the wire I create a magnetic field of $1.33*10^{-7}T$ at a distance of 15cm away from the wire. Isn't this far to little to get detected by any magnetic field sensor?

How are the robot lawn mowers able to measure that magnetic field?

答: A quick internet search for "how does Robot Lawn Mower detect Boundary Wire" turned up a patent which suggests that the mower knows what it's looking for. That is, the transmitter sends a signal with fancy coding. The receiver can use its knowledge of the coding scheme, plus its own fancy signal processing, to pull the signal out of the noise. Of course, no particular mower has to use exactly that scheme.</p>

Title: Lawn Mower Robot Boundary Wire Current


问:
Extended Kalman Filter and PID controller
I'm trying to build a model based on tracking an object in 2-D using MATLAB (time-varying system).
I built the model using an Extended Kalman Filter and a PID controller.

I have a doubt regards the relation of the reference with. I think that it is a theoretical doubt but, after some research I still do not understand.

In your opinion what are the signal that must follow my reference?
The state ("real" position x and y) or the estimated state (estimated position x and y")?

Thank you in advance.

答:
You always act on the estimated state, because if you knew the actual state then you wouldn't need an estimator!

Since you're in simulation, you could run the same test several times, running the PID controller with the real state one time and with the estimated state the other time. The results should be basically identical, and it's a good way to prove to yourself (and others!) that your estimator and complete system (estimator + controller) are working well.

I'll add though that you should run whatever tests you can with the actual system (step test, disturbance injection, etc.) and re-run the same tests against your modeled system. A great estimator and great controller don't really matter if your underlying model doesn't actually reflect reality!


问:
Water depth arduino sonar sensor
Just like a fish finder finds the depth of the water directly beneath it, Im trying to find a sensor that I can purchase for the Arduino that does the same. would like it to check up to 20 ft at least, with high accuracy, +/- 10 or 15 cm. All the threads and info I've been finding are water level sensors not for water depth. So does anyone know of a sensor like this and where I can find one?</p>

答:
The simplest sensor you can build is a weight on a string. Lower it down until the tension of the string is reduced. I assume you want sonar. So just go buy one they are dead easy to interface to.

You can buy these at marine hardware stores. They are not cheap. Look for a depth finder that has "NMEA" output. You may have heard of NMEA as the data format that GPS receivers use to send location. But it is a common format used by wind, depth and speed sensors.

The NMEA format is just 4800 baud serial ASCII data with end of lines and the data is typically sent once per second. You can read the data in a PC if it has a serial port and a terminal program.

The sensors itself looks like a plumbing fitting. You drill a 1 or 2 inch hole in the boat and install the sensor facing down then a wire runs to the main unit. In theory you could create electronics that "ping" the sensor and wait for a return but it's easier to buy a complete unit with NMEA output.

If you try and roll your own you have to deal with thermoclines, soft bottoms, rocks and even fish and do a lot of signal processing to find a reliable depth. This is not like sonar in air where there is a huge difference between a solid wall and air. Soft mud is not so much unlike water. But a NMEA sensor would be very easy to use. You pay the big price for the DSP.

My favorite store is West Marine. Google it and I bet there is one near you if you live near ocean. I recommend them because of their high level of customer support. They will point you at the best product and answer questions like "what's NMEA?"

Title: Water depth arduino sonar sensor


问:
rosrun doesn't appear to do anything
I'm new to ROS and I'm following the next tutorial (Don't worry there is time stamps bellow to see the full code in the video).

https://www.youtube.com/watch?v=yqYvMEYJoTk&list=PLAjUtIp46jDcQb-MgFLpGqskm9iB5xfoP&ab_channel=EmilVidmark

the thing is that my code is identical to the python executable (11:10) and I wrote the same thing in the CMakeLists.txt under line 111 (12:34) additionally I granted an execute permission to the python file with the command.

chmod +x publisher_node

but when I run the rosrun command it does nothing, it's not freezing but it is not under the rostopic list.

rosrun tutorials publisher_node.py

i'm running with another terminal running the roscore and I've the Ubuntu 18.04 distro.
Thanks in advance for your help!.

Edit:

The question was closed because I didn't add enough detail, I didn't know if I should add more detail because there is a warning that I should wrap things up, so here is my full procedure

I ran the following commands from the base folder of Ubuntu Terminal:

```<code>usr@PC:~<span class="math-container">$ mkdir python_catkin
usr@PC:~$</span> cd python_catkin
usr@PC:~/python_catkin<span class="math-container">$ mkdir src
usr@PC:~/python_catkin$</span> catkin_make
Base path: /home/usr/python_catkin
Source space: /home/usr/python_catkin/src
Build space: /home/usr/python_catkin/build
Devel space: /home/usr/python_catkin/devel
Install space: /home/usr/python_catkin/install
####
#### Running command: "cmake /home/usr/python_catkin/src -DCATKIN_DEVEL_PREFIX=/home/usr/python_catkin/devel -DCMAKE_INSTALL_PREFIX=/home/usr/python_catkin/install -G Unix Makefiles" in "/home/usr/python_catkin/build"
####
-- Using CATKIN_DEVEL_PREFIX: /home/usr/python_catkin/devel
-- Using CMAKE_PREFIX_PATH: /opt/ros/melodic
-- This workspace overlays: /opt/ros/melodic
-- Found PythonInterp: /usr/bin/python2 (found suitable version "2.7.17", minimum required is "2")
-- Using PYTHON_EXECUTABLE: /usr/bin/python2
-- Using Debian Python package layout
-- Using empy: /usr/bin/empy
-- Using CATKIN_ENABLE_TESTING: ON
-- Call enable_testing()
-- Using CATKIN_TEST_RESULTS_DIR: /home/usr/python_catkin/build/test_results
-- Found gtest sources under '/usr/src/googletest': gtests will be built
-- Found gmock sources under '/usr/src/googletest': gmock will be built
-- Found PythonInterp: /usr/bin/python2 (found version "2.7.17")
-- Using Python nosetests: /usr/bin/nosetests-2.7
-- catkin 0.7.29
-- BUILD_SHARED_LIBS is on
-- BUILD_SHARED_LIBS is on
-- ~~~~~~~~~~~~~~~~~
-- ~~  traversing 1 packages in topological order:
-- ~~  - tutorials
-- ~~~~~~~~~~~~~~~~~
-- +++ processing catkin package: 'tutorials'
-- ==> add_subdirectory(tutorials)
-- Configuring done
-- Generating done
-- Build files have been written to: /home/usr/python_catkin/build
####
usr@PC:~/python_catkin<span class="math-container">$ cd src
usr@PC:~/python_catkin/src$</span> catkin_create_pkg tutorials rospy std_msgs
Created file tutorials/package.xml
Created file tutorials/CMakeLists.txt
Created folder tutorials/src
Succesfully created files in home/usr/python_catkin/src/tutorials. Please adjust the values in package.xml.
</code>```

Later I opened Visual Studio Code under the tutorials folder and created the folder scripts and I created a file under the scripts folder named publisher_node.py, then I added this code:

```<code>#!/usr/bin/env
import rospy
from std_msgs.msg import String

def talk_to_me():
    pub = rospy.Publisher('talking_topic', String, queue_size=10)
    rospy.init_node('publisher_node', anonymous=True)
    rate = rospy.Rate(1)
    rospy.loginfo("Publisher Node Started, now publishing messages")
    while not rospy.is_shutdown():
        msg = "Hello Emil - %s" % rospy.get(time)
        pub.publish(msg)
        rate.sleep()

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

later under the CMakeLists.txt file I added the following line:

```<code>catkin_install_python(PROGRAMS scripts/publisher_node.py
  DESTINATION %{CATKIN_PACKAGE_BIN_DESTINATION}
)
</code>```

later I went to the folder of the ROS proyect on a terminal and runned the following command:

Terminal1:

```<code>usr@PC:~/python_catkin$ roscore
... logging to /home/usr/.ros/log/710a4f1c-3c3e-11ed-8cef-409f3849b403/roslaunch-PC-9343.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://IP/
ros_comm version 1.14.13

SUMMARY
========

PARAMETERS
 * /rosdistro: melodic
 * /rosversion: 1.14.13

NODES

auto-starting new master
process[master]: started with pid [9355]
ROS_MASTER_URI=http://IP/
</code>```

Terminal2:

```<code>usr@PC:~/python_catkin$ rosrun tutorials publisher_node.py
█
</code>```

The Terminal2 is running but it doesn't print anything, it doesn't print the line 9 nor the line 11 from the publisher_node.py, it just stays the same when I give it excecution permissions but if I don't it shows the error message:

Terminal2:

```<code>usr@PC:~/python_catkin$ rosrun tutorials publisher_node.py
[rosrun] Couldn't find executable named publisher_node.py below /home/usr/python_catkin/src/tutorials
[rosrun] Found the following, but they're either not files,
[rosrun] or not executable:
[rosrun]   /home/usr/python_catkin/src/tutorials/scripts/publisher_node.py
</code>```

I hope is well explained, if you have any question please answer this post and I will respond ASAP.

Thanks for your help!

答:
Looking at your roscore log, I see that your ROS_MASTER_URI is malformed. You have not included how your set up your environment, but I think that your have accidentally copied something verbatim which should have been substituted for your IP and it's missing the port number.

Make sure to check your environment setup

And see this related question

Title: rosrun doesn't appear to do anything


问:
What could be the unit ( dimension) of the vectors spanning the Jacobian matrix?
Let the general forward rate kinematics equation for a serial mechanism be given as follows:

$$ \begin{bmatrix}  \boldsymbol{v} \\ \boldsymbol{\omega} \end{bmatrix} =  \begin{bmatrix}  \boldsymbol{J}_1 & \boldsymbol{J}_2 & \cdots &\boldsymbol{J}_n \\  \end{bmatrix} \begin{bmatrix} \dot{\boldsymbol{q}}_1 \\ \dot{\boldsymbol{q}}_1 \\ \vdots \\ \dot{\boldsymbol{q}}_n \end{bmatrix}$$

The Jacobian, $\boldsymbol{J}_i \in \mathbb{R}^{6}$,columns are formed by a direction and moment vectors, i.e., $\boldsymbol{s}_{i\parallel} $ and $(\boldsymbol{s}_{i\parallel} \times \boldsymbol{l}_i)$, respectively. I know $\boldsymbol{s}_{i\parallel}$ is a direction vector which is only dependent on the joint angles and $\boldsymbol{l}_i$ is a position vector to the location of $\boldsymbol{s}_{i\parallel}$. If $\dot{\boldsymbol{q}}_i$ has a unit of $rad/sec$, what would be the unit of $\boldsymbol{s}_{i\parallel} \times \boldsymbol{l}_i$ to get an approperiate dimension for $\boldsymbol{v}$ and $\boldsymbol{\omega}$?

Thank you in advance

答:
The units of the linear velocity portions of your Jacobian (what you have denoted as $\boldsymbol{s}_{i \parallel} \times \boldsymbol{l}_{i}$) are whatever distance unit you have used to define $\boldsymbol{l}_{i}$ "divided by" radians. To some extent this may rely on the parameterization method of your robot's kinematic structure (i.e. DH parameters). Given that you are not mixing units when you calculate your Jacobian, it will be the same units as were used to calculate $\boldsymbol{l}_{i}$ - which physically represents the distance from the $i^\text{th}$ frame of your robot to the end-effector.

Assuming you use meters in your model parameterization, we have:

$$ \mathbf{J}(\mathbf{q}) = \frac{\partial f(\mathbf{q})}{\partial \mathbf{q}} $$

where $f(\cdot)$ is the forward kinematics map. So, since our forward kinematics have been defined in meters using an appropriate parameterization method, the linear velocity here is really not velocity as much as change in position of our end-effector with respect to change in joint angle. It should be noted that the units for our orientation space and individual joint angles are radians. Thus, our units for the linear Jacobian terms would simply be $\frac{\text{m}}{\text{rad}}$, and likewise the units of each $\boldsymbol{s}_{i \parallel}$ term are $\frac{\text{rad}}{\text{rad}}$. So, once multiplied by $\dot{\mathbf{q}}$, we have instantaneous linear velocity in terms of $\frac{\text{m}}{\text{sec}}$ and angular velocity in terms of $\frac{\text{rad}}{\text{sec}}$.


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

答:
"Please be as through as you possibly can"

This is a book length subject.  To cover it completely would required hundreds of pages of text.

The best way to learn, and I'm just starting to learn is to start out very simple.  Make a filter where the state is just a 2D vector and the measurements look a LOT like the state.

Then you try a harder problem and so on.   I doubt you can learn by reading.  I can't.  I have to use the theory to learn it.  This is a very good and simple explanation, about as simple as in can get: Kalman tutorial


问:
homogeneous transformation matrix only given sensor extrinsic values
I'm currently dealing with a dataset with points clouds collected by a lidar sensor and radar sensor. Unlike datasets like kitti however, they only provided info about the positions of the sensor relative to the origin of a car. For example:

sensor
x(m)
y(m)
z(m)
yaw(°)
pitch(°)
roll(°)

Lidar
-1.5
0
1.5
5
-2
0

Radar
0.05
-0.5
1
0
5
180

The problem is I'm not sure how I could calculate the homogenous transformation matrices for each sensor to transform points in each sensor frame into the origin (0,0,0) reference frame.

I tried finding the Denavit–Hartenberg parameters. But due to the two rotations of each sensor, I cant seem to match the coordinates frames and get the corresponding parameters. Any advice would be appreciated!

答:
You can do this easily just by using homogeneous transformation.

Attach a local frame to each sensor , and make sure your sensor measurements can be represented in this frame. For example, I would suggest to align the z-axis w.r.t sensor axis.
In your case sensors' positions and orientations are already given. But generally, get the positions and orientations of each of these frames w.r.t global frame. You can have a global frame anywhere you want , but ideally at the geometric center of the robot .

Now just by using matrix multiplication , you can represent the sensor values of local frame to global frame. For example, if your sensor measurements in local frame are defined by P , and homogeneous transformation of sensor frame in global frame by H . then the sensor readings in global frame are H.P .

Edit : I seem to have missed the explanation about computing the Homogeneous matrix. Updating that part here.

Homogeneous matrix is combined way of representing overall rotation and translation. For representing the rotation effectively , we need more information than what you have provided in the question. For now , I am assuming the ZYX Euler representation . That means , your overall rotation would be : $R_z \times R_y \times R_x $ . Here , X, Y ,Z rotations are roll , pitch and yaw respectively. Translation component is just added to your overall rotation matrix in the right , along with homogeneous component.
Homogeneous matrices for each sensor can be computed as given in the following python program. You can read more about the theory behind it here.

```<code>from scipy.spatial.transform import Rotation
import numpy as np

class Transformation:
    def __init__(self,x=0,y=0,z=0,alpha=0,beta=0,gamma=0) -> None:
        self.alpha =alpha  #rotation w.r.t x in degrees
        self.beta = beta   #rotation w.r.t y in degrees
        self.gamma = gamma  # rotation w.r.t z  in degrees
        self.x = x  # X translation
        self.y = y  # Y translation
        self.z = z  # Z translation
    def xRotation(self):
        return Rotation.from_euler("X",self.alpha,degrees=True).as_matrix()
    def yRotation(self):
        return Rotation.from_euler("Y",self.beta,degrees=True).as_matrix()
    def zRotation(self):
        return Rotation.from_euler("Z",self.gamma,degrees=True).as_matrix()
    def xyzRotation(self):
        return Rotation.from_euler("ZYX",[self.alpha,self.beta ,self.gamma],degrees = True).as_matrix()
        # return np.linalg.multi_dot([self.xRotation(),self.yRotation(),self.zRotation()])  #self.xRotation().dot(self.yRotation()).dot(self.zRotation())  #
    def translation(self):
        return np.array([[self.x],[self.y],[self.z]])
    def homogeneous(self):
        rot_trans = np.concatenate((self.xyzRotation(),self.translation()),1) #np.concatenate
        bottom_row = np.array([[0,0,0,1]])  # this is required to convert to homogeneous matrix
        return np.concatenate((rot_trans,bottom_row),0)  # use this as result.dot([point,1]). Here result is  4x4 numpy array containing homogenous matrix

if __name__ == "__main__":
    # First computing the homogeneous matrix for Lidar
    x,y,z,alpha,beta,gamma = -1.5,0,1.5,0,-2,5
    t = Transformation(x,y,z,alpha,beta,gamma)
    print("H matrix for Lidar sensor")
    print(np.round(t.homogeneous(),2))

    # Homogenous matrix for Radar

    x,y,z,alpha,beta,gamma = 0.05,-0.5,1,180,5,0
    t = Transformation(x,y,z,alpha,beta,gamma)
    print("H matirx for Radar Sensor")
    print(np.round(t.homogeneous(),2))
</code>```

The result of it is :

```<code>H matrix for Lidar sensor
[[ 1.   -0.   -0.03 -1.5 ]
 [ 0.    1.   -0.09  0.  ]
 [ 0.03  0.09  1.    1.5 ]
 [ 0.    0.    0.    1.  ]]

H matirx for Radar Sensor
[[-1.   -0.   -0.09  0.05]
 [ 0.   -1.    0.   -0.5 ]
 [-0.09  0.    1.    1.  ]
 [ 0.    0.    0.    1.  ]]
</code>```

After obtaining the H matrix , you can multiply the corresponding measurement with it to obtain the measurement in global frame.

For example, if your Lidar measurement is :$[1,1.5,-1.3]^T $ , then its representation in global frame would be:

```<code>H_lidar = np.round(t.homogeneous(),2)
lidar_measurement = np.array([[1,1.5,-1.3,1]]).T
print(np.dot(H_lidar,lidar_measurement))
</code>```

This prints :

```<code>[[-0.461]
 [ 1.617]
 [ 0.365]
 [ 1.   ]]
</code>```

The one is added in the last row to represent the homogeneous transformation.

Concepts to be careful about:

Multiplication of rotation matrices is not commutative

Know the difference between fixed angle rotation and current ange rotation . They are not same!

Body:
You can do this easily just by using homogeneous transformation.

Attach a local frame to each sensor , and make sure your sensor measurements can be represented in this frame. For example, I would suggest to align the z-axis w.r.t sensor axis.
In your case sensors' positions and orientations are already given. But generally, get the positions and orientations of each of these frames w.r.t global frame. You can have a global frame anywhere you want , but ideally at the geometric center of the robot .

Now just by using matrix multiplication , you can represent the sensor values of local frame to global frame. For example, if your sensor measurements in local frame are defined by P , and homogeneous transformation of sensor frame in global frame by H . then the sensor readings in global frame are H.P .

Edit : I seem to have missed the explanation about computing the Homogeneous matrix. Updating that part here.

Homogeneous matrix is combined way of representing overall rotation and translation. For representing the rotation effectively , we need more information than what you have provided in the question. For now , I am assuming the ZYX Euler representation . That means , your overall rotation would be : $R_z \times R_y \times R_x $ . Here , X, Y ,Z rotations are roll , pitch and yaw respectively. Translation component is just added to your overall rotation matrix in the right , along with homogeneous component.
Homogeneous matrices for each sensor can be computed as given in the following python program. You can read more about the theory behind it here.

from scipy.spatial.transform import Rotation
import numpy as np

class Transformation:
    def __init__(self,x=0,y=0,z=0,alpha=0,beta=0,gamma=0) -> None:
        self.alpha =alpha  #rotation w.r.t x in degrees
        self.beta = beta   #rotation w.r.t y in degrees
        self.gamma = gamma  # rotation w.r.t z  in degrees
        self.x = x  # X translation
        self.y = y  # Y translation
        self.z = z  # Z translation
    def xRotation(self):
        return Rotation.from_euler("X",self.alpha,degrees=True).as_matrix()
    def yRotation(self):
        return Rotation.from_euler("Y",self.beta,degrees=True).as_matrix()
    def zRotation(self):
        return Rotation.from_euler("Z",self.gamma,degrees=True).as_matrix()
    def xyzRotation(self):
        return Rotation.from_euler("ZYX",[self.alpha,self.beta ,self.gamma],degrees = True).as_matrix()
        # return np.linalg.multi_dot([self.xRotation(),self.yRotation(),self.zRotation()])  #self.xRotation().dot(self.yRotation()).dot(self.zRotation())  #
    def translation(self):
        return np.array([[self.x],[self.y],[self.z]])
    def homogeneous(self):
        rot_trans = np.concatenate((self.xyzRotation(),self.translation()),1) #np.concatenate
        bottom_row = np.array([[0,0,0,1]])  # this is required to convert to homogeneous matrix
        return np.concatenate((rot_trans,bottom_row),0)  # use this as result.dot([point,1]). Here result is  4x4 numpy array containing homogenous matrix

if __name__ == "__main__":
    # First computing the homogeneous matrix for Lidar
    x,y,z,alpha,beta,gamma = -1.5,0,1.5,0,-2,5
    t = Transformation(x,y,z,alpha,beta,gamma)
    print("H matrix for Lidar sensor")
    print(np.round(t.homogeneous(),2))

    # Homogenous matrix for Radar

    x,y,z,alpha,beta,gamma = 0.05,-0.5,1,180,5,0
    t = Transformation(x,y,z,alpha,beta,gamma)
    print("H matirx for Radar Sensor")
    print(np.round(t.homogeneous(),2))

The result of it is :

H matrix for Lidar sensor
[[ 1.   -0.   -0.03 -1.5 ]
 [ 0.    1.   -0.09  0.  ]
 [ 0.03  0.09  1.    1.5 ]
 [ 0.    0.    0.    1.  ]]

H matirx for Radar Sensor
[[-1.   -0.   -0.09  0.05]
 [ 0.   -1.    0.   -0.5 ]
 [-0.09  0.    1.    1.  ]
 [ 0.    0.    0.    1.  ]]

After obtaining the H matrix , you can multiply the corresponding measurement with it to obtain the measurement in global frame.

For example, if your Lidar measurement is :$[1,1.5,-1.3]^T $ , then its representation in global frame would be:

H_lidar = np.round(t.homogeneous(),2)
lidar_measurement = np.array([[1,1.5,-1.3,1]]).T
print(np.dot(H_lidar,lidar_measurement))

This prints :

[[-0.461]
 [ 1.617]
 [ 0.365]
 [ 1.   ]]

The one is added in the last row to represent the homogeneous transformation.

Concepts to be careful about:

Multiplication of rotation matrices is not commutative

Know the difference between fixed angle rotation and current ange rotation . They are not same!