Humble 4.4 update broke joints

[mr337]

I recently upgraded ROS packages for my system. Upon launching the simulation a few of the joints are broken. After trying a few things I have pinned down the issue to the ros-humble-gazebo-ros2-control package update. At any time I can roll back to version 0.4.2 and all problems are gone. I don't have access to the 0.4.3 package but looking at the changelog looks like 4.4 introduced a few changes.

ros-humble-gazebo-ros2-control:  0.4.2-1jammy.20230309.194017 -> 0.4.4-1jammy.20231003.230725

With version 0.4.2-1jammy.20230309.194017

Everything works as expected

I am also able to publish an array to the controller and get control of the joint. Here is moving to 1 radian.

After update with version 0.4.4-1jammy.20231003.230725

RVIZ startups up showing this. I don't get any controller errors or repeated messages. I am unable to control the joint via publishing and no errors when publishing the array. It acts like everything is fine except joint broken in RVIZ.

I have a few other joints that are broken as well, looks like things rotated by 90 degrees, so far they all seem to be of type position_controllers/JointGroupPositionController. I was thinking maybe the issue #240 was related but no luck as some of the joints are not passive.

Link details:
URDF

<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

  <xacro:macro name="wheel_bracket" params="name parent *origin *axis">

    <link name="${name}_link">
      <inertial>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <mass value="0.01" />
        <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001" />
      </inertial>
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <mesh
            filename="file://$(find test_model)/meshes/wheel_bracket.stl" scale="0.001 0.001 0.001" />
        </geometry>
        <material name="">
          <color rgba="0.10 0.50 0.14 1" />
        </material>
      </visual>
    </link>

    <joint name="${name}_joint" type="revolute">
      <parent link="${parent}"/>
      <child link="${name}_link"/>
      <xacro:insert_block name="axis" />
      <xacro:insert_block name="origin" />
      <dynamics damping="10.0" friction="1.0"/>
      <limit lower="-0.698132" upper="0.698132" effort="1000" velocity="10.0" />
    </joint>

    <transmission name="trans_${name}">
      <type>transmission_interface/SimpleTransmission</type>
      <actuator name="motor_${name}">
        <mechanicalReduction>10000</mechanicalReduction>
        <hardwareInterface>EffortJointInterface</hardwareInterface>
      </actuator>
      <joint name="${name}">
        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      </joint>
    </transmission>

    <gazebo reference="${name}_link">
      <visual>  
        <material>  
          <ambient>0.10 0.25 0.14 1.0</ambient>  
          <diffuse>0.10 0.25 0.14 1.0</diffuse>  
          <specular>0.10 0.25 0.14 1.0</specular>  
          <emissive>0.10 0.25 0.14 1.0</emissive>  
        </material>  
      </visual>
    </gazebo>

  </xacro:macro>

</robot>

controllers_sim.yaml

controller_manager:
  ros__parameters:
    update_rate: 100
    use_sim_time: true

    wheel_brackets_cont:
      type: position_controllers/JointGroupPositionController

    joint_broad:
      type: joint_state_broadcaster/JointStateBroadcaster
wheel_brackets_cont:
  ros__parameters:
    use_sim_time: true
    base_frame_id: base_link
    joints:
      - wheel_bracket_driver_joint
      - wheel_bracket_passenger_joint
    interface_name: position

Hardware interfaces

$ ros2 control list_hardware_interfaces                                                              
command interfaces
        wheel_bracket_driver_joint/position [available] [claimed]
        wheel_bracket_passenger_joint/position [available] [claimed]
state interfaces
        wheel_bracket_driver_joint/position
        wheel_bracket_driver_joint/velocity
        wheel_bracket_passenger_joint/position
        wheel_bracket_passenger_joint/velocity

System details:
OS: Ubuntu Jammy
ROS Version: Humble

[christophfroehlich]

There were quite a lot of changes to 0.4.4, I don't have a glue now where this could come from. Could you try to compile it from source, and use git bisect to find the commit introducing your issue?

[mr337]

No problem, after working git bisect what appears to be the offending commit is d18a887#diff-e14963d1c2ba3a31138bda99525946c355191fe361859b5fb7247fddad6c3a2a

I believe some of this control code is a little over my head. What is odd is I compared the full ROS log line for line with version 0.4.2 and 0.4.4 expecting some kind of error or something crashing/segfaulting. I did not get any warning as defined here https://github.com/ros-controls/gazebo_ros2_control/pull/177/files#diff-e14963d1c2ba3a31138bda99525946c355191fe361859b5fb7247fddad6c3a2aR138

Looking at the code I don't see much difference since the larger if else blocks seem to cover Position controllers just like I am using. The only difference is they have an additional true. Still splunking around in the codebase.

[christophfroehlich]

I don't see what #208 should change with your setup. Could you provide a full example of your robot including the meshes? I'm curious..

[mr337]

That is what I keep thinking 😆

Let me provide a Minimal Reproducible Example of the robot

[mr337]

@christophfroehlich Here is an example using the above meshes in the original post of this issue https://github.com/mr337/control_mre Instructions and such on how to produce in ROS2 Humble.

[christophfroehlich]

Sorry, I haven't had time earlier to dig into this.

The problem is not a broken joint-state, but your gazebo model explodes after touching the ground.

The reason why this didn't happen before is that now as a fallback every joint velocity is set to zero (this happens always at the first write until the controllers get activated)

gazebo_ros2_control/gazebo_ros2_control/src/gazebo_system.cpp

Lines 604 to 607 in 3c8f5bb

	} else if (this->dataPtr->is_joint_actuated_[j]) {
	// Fallback case is a velocity command of zero (only for actuated joints)
	this->dataPtr->sim_joints_[j]->SetVelocity(0, 0.0);
	}

Without forcing it to zero they are "not actuated"/compliant and the impact is solved stable.

I have to try if this is a problem of your specific robot (falling down, four wheels might not get stable contact points..) or happens also with others.

[christophfroehlich]

OK, your model sometimes also explodes with the version before the commit, after it has already been standing stable on the ground. By increasing physics/solver/iterations to 500 make the simulation stable, even with the latest version of the HEAD.

This is an indication that your model or the physics solver is not well configured.

A good starting point is always to have a look at the inertia, masses, and contact points (visualize them from the view menu).

• Having a look at the inertias: None has a infeasible high magnitude, but it does not really coincide with your meshes. Pay also attention that every link being in the kinematic chain with non-fixed joints has reasonable high mass+inertia. (setting inertia of any link to zero -> your model explodes).
• The contact points are flickering. This is a well-documented issue of gazebo classic ODE solver, but there are some solver options to play around with.

I hope I could help with this. I'm pretty sure now that this is not a problem with gazebo_ros2_control, the new version just reveals the problem immediately. I close this issue now, feel free to reopen if you don't agree.

[mr337]

Just wanted to add that our issue was related to the mass of the wheels. We also did some further work on masses and inertias to get a model that is happy.

Thanks again for pointing us in the right direction @christophfroehlich

[christophfroehlich]

you're very welcome, glad you fixed it!