ControlBoard: fix torque feedback

xela-95 commented 4 months ago

We want to handle torques correctly in the control board plugin, by this we mean:

Project the 6D wrench that Gazebo sim returns to the joint axis, so that in control we can deal only with a torque scalar value for each joint (see the comments related to https://github.com/robotology/gz-sim-yarp-plugins/issues/77#issuecomment-1933858542)
Manage losses in the transmission: i.e. Gazebo returns the joint torque $\tau_o$ while from the controller side we will set the input torque $\tau_i$.

CC @traversaro

xela-95 commented 4 months ago

Past gz issues for reference:

xela-95 commented 4 months ago

Gazebo-yarp-plugin issues:

Gazebo classic issues:

https://github.com/gazebosim/gazebo-classic/issues/1321

xela-95 commented 4 months ago

Definition of JointTransmittedWrench from gz-sim: https://github.com/gazebosim/gz-sim/blob/633ce72171e27e83bf2e0292c9998e036d5da3fc/include/gz/sim/components/JointTransmittedWrench.hh#L36-L48

Joint Transmitted wrench in SI units (Nm for torque, N for force).

The wrench is expressed in the Joint frame and the force component is applied at the joint origin. Note The term Wrench is used here to mean a pair of 3D vectors representing torque and force quantities expressed in a given frame and where the force is applied at the origin of the frame. This is different from the Wrench used in screw theory. The value of force_offset in msgs::Wrench is ignored for this component. The force is assumed to be applied at the origin of the joint frame.

xela-95 commented 4 months ago

Definition of JointAxis class in SDF specification: https://github.com/gazebosim/sdformat/blob/f3607761d3846dba0b660880484ff1e5eb1e9425/include/sdf/JointAxis.hh#L121-L353

xela-95 commented 4 months ago

Definition of JointAxis class in SDF specification: https://github.com/gazebosim/sdformat/blob/f3607761d3846dba0b660880484ff1e5eb1e9425/include/sdf/JointAxis.hh#L121-L353

Inside the Xyz method it states:

The axis is expressed in the frame named in XyzExpressedIn() and defaults to the joint frame if that method returns an empty string. The vector should be normalized.The default value is gz::math::Vector3d::UnitZ which equals (0, 0, 1).

From here I have to understand how the joint frame is defined in URDF an SDFs.

traversaro commented 4 months ago

The axis is expressed in the frame named in XyzExpressedIn() and defaults to the joint frame if that method returns an empty string. The vector should be normalized.The default value is gz::math::Vector3d::UnitZ which equals (0, 0, 1).

From here I have to understand how the joint frame is defined in URDF an SDFs.

At least in URDF, joint_frame == child_frame, see the image in http://wiki.ros.org/urdf/XML/joint .

traversaro commented 4 months ago

In SDF, it is not true in general that joint_frame == child_frame . I think you can find useful pointers in http://sdformat.org/tutorials?tut=pose_frame_semantics&cat=specification&#pose-frame-semantics-tutorial, https://vimeo.com/378683328 and https://roscon.ros.org/2019/talks/roscon2019_poseframespecificationforsdformat1_7.pdf .

xela-95 commented 4 months ago

In SDF, it is not true in general that joint_frame == child_frame . I think you can find useful pointers in http://sdformat.org/tutorials?tut=pose_frame_semantics&cat=specification&#pose-frame-semantics-tutorial, https://vimeo.com/378683328 and https://roscon.ros.org/2019/talks/roscon2019_poseframespecificationforsdformat1_7.pdf .

Thank you, this documentation was super useful to understand the different ways in which the frames can be specified!

BTW I think it's interesting also for @Gio-DS

xela-95 commented 4 months ago

Visualizations as quick remainders:

URDF

joint frame == child link frame Screenshot from 2024-02-14 15-13-32

SDF 1.6

SDF 1.7 (`relative_to` attribute)

xela-95 commented 4 months ago

If I have understood well the problem of getting the scalar joint torque value for a joint can be solved easily by just projecting the 6D wrench returned by TransmittedWrenchonto the joint motion subspace, an easy to do task when we know the axis of the (e.g. revolute) joint expressed in the parent link frame:

\tau_o = \mathrm{f} \cdot \begin{bmatrix}
0_3 & a
\end{bmatrix}

with $a$ being the joint axis expressed in the parent link frame and $\mathrm{f}$ being the 6D transmitted wrench:

\mathrm{f}:=\left[ \begin{array}{l}
f \\
\tau
\end{array} \right] \in \mathbb{R}^6

Now, since as we said in the previous comments, in the SDF specification there are multiple way to define the joint axis, we must always resort to have the axis represented in the parent link frame. I think this can be done using the SDF API ResolveXyz method of JointAxis class, by giving as the frame name to which the joint axis coordinates will be resolved, the parent link name, obtained by calling the gz-sim Joint class method ParentLinkName method.

Is it correct, @traversaro ?

xela-95 commented 4 months ago

In order to test the correctness of the implementation, I would like to setup some unit tests, using the single pendulum as discussed in the lab, maybe something similar to what the developers of gz-sim had done here: https://github.com/gazebosim/gz-physics/pull/283/files#diff-d47feea35356bed8ccbd38262debeedb3d69aa7c1476744096daabc850340fb8R1160-R1209

In my case, should I assert that the torque computed as in https://github.com/robotology/gz-sim-yarp-plugins/issues/81#issuecomment-1944030733 is equal to the one I could compute analytically from the single pendulum model?

traversaro commented 4 months ago

Is it correct, @traversaro ?

It make sense! The only way to be sure is to have tests, but for now we can proceed with what you wrote that seems to make sense.

traversaro commented 4 months ago

In my case, should I assert that the torque computed as in #81 (comment) is equal to the one I could compute analytically from the single pendulum model?

Exactly, this is a good example of a check.

xela-95 commented 4 months ago

I summarize here the test I want to perform to validate the joint torque computation.

Pendulum drawio

The idea is to compare the joint torque obtained with my computations and the one coming from ther analytical equations of motion of the pendulum given an arbitrary motor torque $\tau_i$, and verify this equality holds true for pendulum models defined in different ways (joint frame pose relative to child link, model link, parent link, ...).

The equations of motions are:

\tau_o - \tau_{weight} = I \cdot \ddot{\theta}

\tau_o = \frac{1}{2} m g L \sin{\theta} + \frac{1}{3} m L^2 \ddot{\theta}

with L the length of the link.

xela-95 commented 4 months ago

Right now I'm dealing with two issues:

I'm not succeeding in resolving the joint axis for the parent link frame, using the SDF API ResolveXyz():
```
gzJoint.Axis(ecm).value().at(0).ResolveXyz(axis, gzJoint.ParentLinkName(ecm).value())
```
When running this I get the error Error while resolving joint axis in parent link frame: JointAxis has invalid pointer to PoseRelativeToGraph.
To implement the unit tests I'm trying to understand if it is possible to obtain the joint acceleration from Gazebo, since in the Documentation are listed on ly position and velocity.

traversaro commented 4 months ago

To implement the unit tests I'm trying to understand if it is possible to obtain the joint acceleration from Gazebo, since in the Documentation are listed on ly position and velocity.

We can also think to obtain this information by numerical differentation if necessary.

xela-95 commented 4 months ago

@traversaro I've a doubt regarding on how to setup a device driver for a unit test. By looking at the forcetorque test: https://github.com/robotology/gz-sim-yarp-plugins/blob/b9c56cbe8c4a905997a5f8192f6423a58f965f5e/tests/forcetorque/ForceTorqueTest.cc#L21-L24 I see there a number of options configured, but in the ForceTorqueDriver open() these options are not used. In the end, what options should I configure for my controlboard tests?

xela-95 commented 4 months ago

@traversaro I'm facing an unexpected behavior when comparing the joint torques with the unit test I've written (see 9886fbbb1424252637930d46a4cde2e0c046a0e2).

The setup of the test is the following:

The pendulum starts from the stable position ($\theta=0$);
at each time instant a reference constant torque is applied to the joint (in this case $\tau_i=0.5 Nm$);
at each time instant the joint torque is computed by projecting the Transmitted wrench on the joint axis (in this case $a=[1 0 0]'$) and it is compared with the expected joint torque computed at lines https://github.com/robotology/gz-sim-yarp-plugins/blob/9886fbbb1424252637930d46a4cde2e0c046a0e2/tests/controlboard/ControlBoardTest.cc#L157-L159
The test perform 1000 iterations.

A visualization of the test in the following:

https://github.com/robotology/gz-sim-yarp-plugins/assets/57228872/2e0fa02d-3c9d-4a60-8a75-9032ba2d7ae1

What I get is that the joint torque I obtain by projecting the wrench is always equal to the torque I've set as reference, i.e. 0.5 Nm, while the expected joint torque from the eq. of motion starts close to 0.5 and then it decreases becoming also negative.

I'm trying to understand what's missing: one of the terms could be the joint friction, but I though it would be automatically subtracted to the torque I've set at input, while it seems not the case.

LastTest.log

xela-95 commented 4 months ago

If the number of iterations are increased this is the resulting behavior:

https://github.com/robotology/gz-sim-yarp-plugins/assets/57228872/f7f03268-f733-4baf-951f-f4ee3cd61f23

With respect to this test I've parsed the log file and plotted the expected torque values:

expected_torque_from_eq_motion

(Image edited from previous for a bug in code parsing the log)

xela-95 commented 4 months ago

I'm trying to understand what's missing: one of the terms could be the joint friction, but I though it would be automatically subtracted to the torque I've set at input, while it seems not the case.

Related to the friction I've seen in SDF specification that the static and viscous friction of the joint can be explicitly defined with the tags friction and damping, both having a default value of zero. Hence I think joint friction could be excluded from the analysis.

xela-95 commented 4 months ago

After investigating with @traversaro, we found an error in the expected torque equation (https://github.com/robotology/gz-sim-yarp-plugins/issues/81#issuecomment-1945794432) in the sign of the term related to the gravity torque. Now with commit 0bac02e35ec3999874074d2f12dfb41a6a871187 the test is passing and is producing coherent results.

We also added a fixed joint to the sdf model in order to maintain fixed the pendulum base: see 9ef9126ca7e5505ca821338d1f1549872e8208d3

xela-95 commented 4 months ago

Now that the unit test is working as expected I've tried to run it with a new SDF file defining the same pendulum model but using different frame conventions.

In particular, my goal was to define the joint frame relative to an arbitrary frame different from the parent link frame used in the current SDF model, which represents the case adherent to theory, in which the joint frame and child frame coincide and are defined with respect to the predecessor link.

The SDF pendulum_joint_relative_to_parent_link.sdf, defined in 1298c81f79a92795761f6d6beec227959c5ef95a, has a joint framed defined with respect an arbitrary frame with a custom orientation: https://github.com/robotology/gz-sim-yarp-plugins/blob/1298c81f79a92795761f6d6beec227959c5ef95a/tests/controlboard/pendulum_joint_relative_to_child_link.sdf#L116-L123

Also in this case the code to return the joint torque is working as expected, without modifications. In this case the joint axis is defined as 0 -1 0 relative to arbotrary_frame and the joint TransmittedWrench is always defined in the joint frame, hence the product of joint axis by the torque component of the wrench is the expected joint torque.

@traversaro I think this validates the method for returning the joint torque even when the joint axis is defined in a different frame than the parent link, let me know if you have in mind other test cases to add for this topic.

traversaro commented 4 months ago

@traversaro I think this validates the method for returning the joint torque even when the joint axis is defined in a different frame than the parent link, let me know if you have in mind other test cases to add for this topic.

Perfect! I think we can proceed!

xela-95 commented 4 months ago

The last thing I wanted to do in this issue is to refactor the unit test to make use of a parameterized test (see http://google.github.io/googletest/advanced.html#how-to-write-value-parameterized-tests).

In 717929c3d411009de175fcd2f79f453fcd5a67e7 I refactored my unit test to make use of this kind of parametrized test in order to use the same code for different input models. The tests are being correctly recognized but the second one is failing (this does not happen when it runs alone).

@traversaro have you any experience with this?

traversaro commented 4 months ago

What is the failure message?

xela-95 commented 4 months ago

What is the failure message?

I get sometimes a SEGFAULT and sometimes a thread_mutex_lock.c:94: ___pthread_mutex_lock: Assertion mutex->__data.__owner == 0 failed. exception. I think this could be caused by the missing logic to correctly close drivers and remove entities from the singletons we use or maybe by the fact that in the test I'm accessing joint properties without using locks?

traversaro commented 4 months ago

It is useful to report the returned backtrace in the context of the segfault. If just running the second test works fine, and the problem just occurs if running both tests one after the other, probably you are right that this may be related to the missing closing of the drivers or anyhow some problems of that kind.

xela-95 commented 4 months ago

It is useful to report the returned backtrace in the context of the segfault. If just running the second test works fine, and the problem just occurs if running both tests one after the other, probably you are right that this may be related to the missing closing of the drivers or anyhow some problems of that kind.

Unfortunately I wasn'able to get a stack trace for such exception apart from the messages I wrote in the comment above, even with the extra-verbose argument of ctest. Luckily the error was the missing removal from the singleton of the controlboard entity upon destruction of ControlBoard class. Fixed in 13f35ba27775ee76cc5416c84a932c67b1f7eb4b

robotology / gz-sim-yarp-plugins