Open jacknlliu opened 5 years ago
But this is what we want, to remove gravity moment. or am I somewhere wrong?
@destogl
offset_.torque.x -= (gravity_transformed.vector.y * cog.z - gravity_transformed.vector.z * cog.y);
the right hand of the code is -r x G
, not r x G
.
Then the result is:
offset_.torque = offset_.torque - (-rxG) = offset_.torque + r x G
In other words, the offset_.torque
is to add the gravity moment, not to remove it. Right?
Yes you are right. I will check once again what we do in our code to check why is this like that...
@jacknlliu: Did you correct this for you? Can you make pull request or give a correct code snippet?
@destogl Yes, I'll check it in these days.
https://github.com/KITrobotics/force_torque_sensor/blob/1065d1405a0c415fd3230d61d3e2600f1b093ef6/src/force_torque_sensor_handle.cpp#L359
This computes the gravity moment not follow![\mathbf{r} \times \mathbf{G} = \begin{pmatrix} cog_x \\ cog_y \\ cog_z \end{pmatrix} \times \begin{pmatrix} g_x \\ g_y \\ g_z \end{pmatrix} = \begin{pmatrix} 0 & -cog_z & cog_y \\ cog_z & 0 & -cog_x \\ -cog_y & cog_x & 0 \end{pmatrix} \begin{pmatrix} g_x \\ g_y \\ g_z \end{pmatrix}](https://latex.codecogs.com/gif.latex?\mathbf{r}&space;\times&space;\mathbf{G}&space;=&space;\begin{pmatrix}&space;cog_x&space;\\&space;cog_y&space;\\&space;cog_z&space;\end{pmatrix}&space;\times&space;\begin{pmatrix}&space;g_x&space;\\&space;g_y&space;\\&space;g_z&space;\end{pmatrix}&space;=&space;\begin{pmatrix}&space;0&space;&&space;-cog_z&space;&&space;cog_y&space;\\&space;cog_z&space;&&space;0&space;&&space;-cog_x&space;\\&space;-cog_y&space;&&space;cog_x&space;&&space;0&space;\end{pmatrix}&space;\begin{pmatrix}&space;g_x&space;\\&space;g_y&space;\\&space;g_z&space;\end{pmatrix})
but using
-r x G