Add support for direct kinematic coupling in gazebo-fmi-actuator by providing Gazebo acceleration

[traversaro]

At the moment, the actuator plugin only supports providing to the actuator FMI the position and velocity joint values from Gazebo.

Depending on the actual simulation model, it may make sense to also provide the joint acceleration as read from Gazebo (even if I guess it could be particularly noisy when using ODE or Bullet).

[traversaro]

To make an example from Slide 5 of https://github.com/robotology-playground/gazebo-fmi/files/2195839/11_TigtlyConnectedFMUs.pdf , we have these two system that can be co-simulated using FMUs :

In our case the main Gazebo physics engine is equivalent to the FMU2, while the actuator plugin is simulating FMU1 . It is interesting to note that in this case FMU1 is an instantaneous system that takes in input the acceleration from FMU2:

and the tau output of FMU1 is computed by subtracting from the input tau the product between the rotational inertia and the acceleration provided by FMU2 .

If instead there is a spring between the inertia and the FMU1 output, it is interesting to note that the velocity and acceleration feedback from FMU2 is completely ignored.

[traversaro]

How to compute joint acceleration from Gazebo APIs

Most of the notation is based on https://pure.tue.nl/ws/files/25753352/Traversaro_en_Saccon_DC_2016.064.pdf . A indicated the inertial/world frame of the simulation.

Revolute joints

Note: the specific semantics of the Gazebo methods need to be checked.

For a link L, the method
ignition::math::Vector3d WorldAngularAccel () const
returns the angular acceleration $ {}^A \omega_{A,L} $.
Given a hinge/revolute joint ${P, C}$ connecting the links $P$ (parent) and $C$ (child), the method
virtual ignition::math::Vector3d GlobalAxis( unsigned int _index) const returns the angular part of the joint motion subspace $ ^A s_{P,C} \in \mathbb{R}^6 $ , that we indicate with $ {}^A a_{P, C} \in \mathbb{R}^3 $.

The joint acceleration $\ddot{\theta} \in \mathbb{R}$ can then be computed as:
$
\ddot{\theta} = \left( ^C s_{P,C} \right)^T {}^C \dot{\omega}_{P,C} = \left( ^C s_{P,C} \right)^T ( {}^C \dot{\omega}_{A,C} - {}^C \dot{\omega}_{A,P} ) =
\left( ^A s_{P,C} \right)^T ( {}^A \dot{\omega}_{A,C} - {}^A \dot{\omega}_{A,P} )
$

Prismatic Joints

TODO. The following methods can be used:

• ignition::math::Vector3d WorldAngularAccel () const
• virtual ignition::math::Vector3d GlobalAxis( unsigned int _index) const
• ignition::math::Vector3d WorldLinearAccel( ) const
• ignition::math::Vector3d WorldLinearVel( ) const
• ignition::math::Vector3d WorldAngularVel( ) const