Support on development of computed torque-control

[qleonardolp]

Introduction
Hello. I've been developing a ros2 controller using computed torque-control. My goal is to develop an impedance controller. However, I'm facing an issue to implement the gravity compensation (GC). I'm trying to leverage the capability of DART library to compute the manipulator Jacobian, inertia tensor, Coriolis and Gravity effects in joint space. Also, in this way both the simulation (Gazebo) and my controller uses the same robot description (URDF).

One can see how to implement the gravity compensation as present in one some of the DART examples:

    Eigen::VectorXd C = mRobot->getGravityForces();

    mRobot->setForces(C);

However, the values returned does not match with the torques at the joints for the same manipulator configuration. I could input the torques manually through a ForwardCommandController and the values are diferent.

Q1: In the controller loop I update the robot skeleton positions, velocities and torques from the Gazebo simulation. Is it the proper way to feedback the robot data to the dart skeleton class?

Q2: I've been studying the Dart code base and I'm aware of methods such as computeForwardDynamics for the Skeleton. Should my controller invoke this function? (edit: I tested it and is not working too)

Q3: Moreover, my scenario is different than the examples/tutorials because the "World" is Gazebo. In this setup, Gazebo will integrate the states in the same way World::Step() does?

To Reproduce
Steps to reproduce the behavior:

1. Launch my simulation ur5_simulation.launch.py
2. Check joint efforts in /joint_states
3. Activate my controller ros2 control set_controller_state impedance_controller active
4. See error: robot falls down.

Expected behavior
When writing on the effort command interfaces the values from the getGravityForces() method the robot stand still.

Screenshots

Efforts before activating my controller (I guess it is the efforts that the gz_ros2_control sets to hold position):

These values makes some sense for this spatial configuration:

Efforts when activate my controller (using only the getGravityForces() vector):

Environment:

• OS: Ubuntu Noble
• Version Jazzy

[qleonardolp]

Update
It became clear to me it is a Dart related issue, then the details related to ros2_control here may not be helpful.