ControlBoard: fix torque feedback

[xela-95]

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 Control single pendulum with idle and torque modes #77 (comment))
• 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]

Past gz issues for reference:

• Get joint torques gazebosim/gz-physics#124
• [dart] Compute Inverse Dynamics for all skeletons to populate joint forces gazebosim/gz-physics#143
• Add GetJointTransmittedWrench feature gazebosim/gz-physics#283

[xela-95]

Gazebo-yarp-plugin issues:

• Implement properly simulated joint torque control mode gazebo-yarp-plugins#471
• SEA joints simulation using gazebo-yarp-plugins  gazebo-yarp-plugins#192

Gazebo classic issues:

• Proper joint torque feedback gazebosim/gazebo-classic#1321

[xela-95]

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]

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

[xela-95]

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]

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]

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]

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]

Visualizations as quick remainders:

URDF

joint frame == child link frame

SDF 1.6

SDF 1.7 (relative_to attribute)

[xela-95]

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]

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 #81 (comment) is equal to the one I could compute analytically from the single pendulum model?

[traversaro]

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]

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]

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

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]

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]

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]

@traversaro I've a doubt regarding on how to setup a device driver for a unit test. By looking at the forcetorque test:

gz-sim-yarp-plugins/tests/forcetorque/ForceTorqueTest.cc

Lines 21 to 24 in b9c56cb

	option.put("device", "multipleanalogsensorsclient");
	option.put("remote", "/forcetorque");
	option.put("timeout", 1.0);
	option.put("local", "/ForceTorqueTest");

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]

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

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

  gz-sim-yarp-plugins/tests/controlboard/ControlBoardTest.cc

  Lines 157 to 159 in 9886fbb

  	auto expectedJointTorque
  	= linkMass * gravity.Z() * linkLength / 2.0 * std::sin(jointPosition)
  	+ linkInertiaAtLinkEnd * jointAcceleration;

• The test perform 1000 iterations.

A visualization of the test in the following:

2024-02-19_13-50-51.mp4

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]

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

2024-02-19_14-45-13.mp4

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

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

[xela-95]

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]

After investigating with @traversaro, we found an error in the expected torque equation (#81 (comment)) in the sign of the term related to the gravity torque. Now with commit 0bac02e 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 9ef9126

[xela-95]

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 1298c81, has a joint framed defined with respect an arbitrary frame with a custom orientation:

gz-sim-yarp-plugins/tests/controlboard/pendulum_joint_relative_to_child_link.sdf

Lines 116 to 123 in 1298c81

	<frame name='arbitrary_frame' attached_to='__model__'>
	<pose>0 0 0 0 0 1.570796</pose>
	</frame>
	<joint name='upper_joint' type='revolute'>
	<!-- Joint axis pose relative to child link -->
	<pose relative_to='arbitrary_frame'>0 -0.15 2.0 0 0 0</pose>
	<parent>base_link</parent>
	<child>upper_link</child>

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]

@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]

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 717929c 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]

What is the failure message?

[xela-95]

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]

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]

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 13f35ba