[Harmonic] There is an error in the force/torque computation on the model when it is on the ground

[federicociresola]

Environment

OS Version:

• Ubuntu 22.04 LTS

Binary build:

• Gz Harmonic (Lasst verion)
• ROS2 Humble
• ros-gz

Description

My colleague and I developed an easy PD controller that controls the position of the joints using torque force.

Expected behavior:

Sim 1 - The simulation with only the arm shows that when the effort controller is enabled, the arm reaches the initial condition (The video shows the correct behavior)

only_arm.webm

Sim 2 - Attach the arm to the drone and use the same controller. The arm reaches the initial condition, just as it did in the previous simulation.

Actual behavior:
Sim 2 - The arm cannot reach the correct initial condition as if the inertia matrix or center of mass (CoM) were changed or others

uav_with_Arm.webm

Additionally, I noticed that this behavior doesn't appear if I spawn the drone with an arm and a fixed joint in the middle.

This problem occurs with PX4, ROS2_GZ_Control, and DetachableJoint. I removed each element piece by piece to understand where the problem was, and it seems to be an incorrect computation of force between the model and the ground plane. Additionally, this problem appeared after an update to ros2/GzHarmonic in April, but I could not determine what changed.

Steps to reproduce

Unfortunately, I don't have a way to reproduce the problem without the Simulink model. I have attached the SDF and URDF files.

• add open_manipualtor_x_description to the GZ_SIM_RESOURCE_PATH

Sim with only arm

• run gz sim -r only_arm_world.sdf
• run ros2 run ros_gz_bridge parameter_bridge --ros-args -p config_file:=bridge_only_arm.yaml

Sim with UAM+Arm:

• run gz sim -r uav_with_arm_world.sdf
• run ros2 run ros_gz_bridge parameter_bridge --ros-args -p config_file:=bridge_uav_with_arm.yaml

UAM_issue_gz.zip