Add conditional joint friction to stabilize Gazebo Classic simulation.

[danzimmerman]

This PR follows on the discussion at UniversalRobots/Universal_Robots_ROS2_Gazebo_Simulation#19

Summary

Simulated UR robots in Gazebo Classic in ROS 2 are not stable as described in the issue above.

This PR stabilizes that behavior (only tested with ODE so far) by adding friction in the joints' <dynamics> tags if sim_gazebo is true.

Details

There are a few discussions of similar issues I've found. In particular gazebo_ros2_control/issues/54 suggests a couple of different approaches:

• Explicitly calling the joints' SetParam() method in source code to set the fmax parameter, as suggested here (also suggesting that joint_limits_interface might handle this at some point)
• Adding damping and friction (and ensuring all nodes use simulated time), as suggested here.

In the end, both of these suggestions appear to achieve the same thing for position- and velocity- controlled joints:

• In Open Dynamics Engine in ROS 1, the joints' fmax parameter (dParamFMax in Gazebo source code, see here) was set explicitly by a function call in gazebo_ros_control here if the joints used position or velocity command interfaces.
• In ODE, it appears that the friction parameter is also used to set dParamFMax, see here. Empirically, that does seem to be what happens when I pass friction through a joint <dynamics> tag like I do in this PR.

In this PR I set the "friction" to 100x the maximum joint effort for reasons suggested here. In short, it seems that dParamFMax is a mathematical parameter more than something which should be usually set to a physical joint parameter.

The ODE manual says this about joint motors:

Motors have two parameters: a desired speed, and the maximum force that is available to reach that speed. This is a very simple model of real life motors, engines or servos. However, is it quite useful when modeling a motor (or engine or servo) that is geared down with a gearbox before being connected to the joint. Such devices are often controlled by setting a desired speed, and can only generate a maximum amount of power to achieve that speed (which corresponds to a certain amount of force available at the joint).

This describes a geared motor with no supervision operating at its stall torque, but I wouldn't characterize the UR's joints that way. I believe the robot will fault well before it slows down to limit itself at some max torque.

Limitations

If the UR ROS 2 Gazebo simulation ever evolves toward supporting EffortJointInterface like universal_robot/issues/521, then injecting friction through the robot description would appear to violate the guidance in this comment. That said, if gazebo_ros2_control eventually handles this, I'd think it'd be better to appropriately reset the joints if possible.

Also, an effort command interface is only useful in simulation, since the real robot can't accept joint-level effort commands.

I also haven't tried to test this against the other physics engines yet. I'd currently expect:

• It's a problem in DART, see here.
• It'd be neutral in Bullet, see here if JointPtr->SetParam() is called, and here if it describes <axis> friction.
• It'd be neutral in Simbody, see here if JointPtr->SetParam() is called and here if an <axis> element is parsed.

However I don't understand enough about Gazebo and the URDF->SDF->In-Memory Joint pipeline to know if that's where the <dynamics> tag ends up.

Questions

This PR seems to provide an expedient fix for ur_simulation_gazebo/issues/19 but I'm not sure it's the right approach. I'm wondering:

• Is ur_description intended to support other physics engines besides ODE when used in the context of ur_simulation_gazebo?
• Are there eventual plans, either here in ur_description or in the simulation packages to support effort joint interfaces like universal_robot/issues/521?

[danzimmerman]

I've only tested this on Humble so far as well, with a complicated installation from source.

There's a workspace snapshot here with deps as submodules: https://github.com/danzimmerman/dz_ws/tree/fb16699a1113cbcd17f6c61724dfb1fe34f96404/src and I'm building against a Robostack Humble binary installation.

I should be able to test against Rolling native apt binaries shortly.

[danzimmerman]

Worked with Rolling

[danzimmerman]

I see that #54 is going to interact with this and that ros-controls/gz_ros2_control#122 represents movement toward control by joint effort (though probably only for latest versions of Gazebo, not classic, it's a big PR)

[gavanderhoorn]

Just noticed this:

Is ur_description intended to support other physics engines besides ODE when used in the context of ur_simulation_gazebo?

even though I'm just a user of these packages, based on my experience, "hard-coding" something for a specific simulator in what is -- I believe -- supposed to be a base package with just some robot models, is definitely something to try to avoid.

Ideally, any Gazebo-isms are added in ur_simulation_gazebo using composition and we avoid violating separation of concerns.

I don't write this as a reviewer though. This is just a comment on your comment/though.

[danzimmerman]

Ideally, any Gazebo-isms are added in ur_simulation_gazebo using composition and we avoid violating separation of concerns.

I don't write this as a reviewer though. This is just a comment on your comment/though.

Thanks for the comments!

This makes sense to me, and seems to be what #52 is aiming at, without going so far as to split the robot description among several packages.

#55 is proposing changes to joint <dynamics> tags for Ignition/new Gazebo, and this would be alleviated if we could set simulation-specific properties in simulation-specific code.

Unfortunately, I have not yet been successful overriding or adding joint <dynamics> or any other joint properties with <gazebo reference= /> tags.

Apparently, what's happening is that the SDF conversion adds additional <axis>, <physics> and other tags to the SDF joint instead of combining or overriding joint parameters with the info from the <gazebo> tags.

If I remove shoulder_pan_joint's URDF <dynamics> tag and modify the conditional Gazebo additions in ur.urdf.xacro to this:

   <xacro:if value="$(arg sim_gazebo)">
    <!-- Gazebo plugins -->
    <gazebo reference="world">
    </gazebo>
    <gazebo reference="$(arg prefix)shoulder_pan_joint">
      <axis>
        <xyz>0 0 1</xyz>
        <dynamics>
          <damping>0.0</damping>
          <friction>867530.9</friction>
        </dynamics>
        <limit>
          <lower>-5.4321</lower>
          <upper>5.4321</upper>
        </limit>
      </axis>
      <physics>
        <ode>
          <max_force>123456.0</max_force>
        </ode>
      </physics>
    </gazebo>
    <gazebo>
      <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
        <parameters>$(arg simulation_controllers)</parameters>
      </plugin>
    </gazebo>
  </xacro:if>

I get the following in the SDF, with duplicated <axis> and <physics> tags in shoulder pan joint:

<joint name='shoulder_pan_joint' type='revolute'>
      <pose relative_to='base_link'>0 0 0.1625 0 -0 3.14159</pose>
      <parent>base_link</parent>
      <child>shoulder_link</child>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <lower>-6.28319</lower>
          <upper>6.28319</upper>
          <effort>150</effort>
          <velocity>3.14159</velocity>
        </limit>
        <dynamics>                                       <--- I removed the URDF dynamics tag, so no friction or damping here
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
      </axis>
      <physics>
        <ode>
          <limit>
            <cfm>0</cfm>
            <erp>0.2</erp>
          </limit>
        </ode>
      </physics>
      <axis>                                                 <--- Second axis tag with my desired settings
        <xyz>0 0 1</xyz>
        <dynamics>
          <damping>0</damping>
          <friction>867531</friction>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
        <limit>
          <lower>-5.4321</lower>
          <upper>5.4321</upper>
        </limit>
      </axis>
      <physics>                                         <--- Second physics tag with my desired settings, no erp or cfm
        <ode>
          <max_force>123456</max_force>
        </ode>
      </physics>
    </joint>

This seems to be a known issue for the <physics> tag at least:

gazebosim/sdformat#232

and affected other tags in the past

gazebosim/sdformat#71

I'm not seeing anything about duplicated <axis> tags in the open URDF issues at https://github.com/gazebosim/sdformat/labels/URDF, but that does seem to be what's blocking my ability to use <gazebo> tags to change joint friction or inject physics tags like <max_force>.

[danzimmerman]

https://classic.gazebosim.org/tutorials?tut=ros_urdf&cat=connect_ros#%3Cgazebo%3EElementsForJoints says:

---

<gazebo> Elements For Joints

Name	Type	Description
stopCfm	double	Joint stop constraint force mixing (cfm) and error reduction parameter (erp) used by ODE
stopErp
provideFeedback	bool	Allows joints to publish their wrench data (force-torque) via a Gazebo plugin
implicitSpringDamper	bool	If this flag is set to true, ODE will use ERP and CFM to simulate damping. This is a more stable numerical method for damping than the default damping tag. The cfmDamping element is deprecated and should be changed to implicitSpringDamper.
springStiffness	double	Spring stiffness in N/m.
springReference	double	Equilibrium position for the spring.
cfmDamping
fudgeFactor	double	Scale the excess for in a joint motor at joint limits. Should be between zero and one.

Again, similar to <gazebo> elements for <robot> and <link>, any arbitrary blobs that are not parsed according to the table above are inserted into the the corresponding <joint> element in the SDF. This is particularly useful for plugins, as discussed in the ROS Motor and Sensor Plugins tutorial.

---

So inserting other tags like <axis> and <physics> and getting them as pasted-in blobs after the tags converted from URDF would seem to be expected behavior.

Tags from the table above do get parsed into the first <axis> tag, but it seems that friction can't be specified except via the URDF's dynamics tag, and ODE's max_force can't be specified at all.

[danzimmerman]

even though I'm just a user of these packages, based on my experience, "hard-coding" something for a specific simulator in what is -- I believe -- supposed to be a base package with just some robot models, is definitely something to try to avoid.

I definitely agree with and appreciate your point here.

Instead of what I'm doing here, I think might propose something like adding generic <joint_name>_damping and <joint_name>_friction parameters to ur_macro.xacro to allow users to pass in appropriate values from a higher-level .xacro. Those could all default to zero as they do now, for the real robot, FakeSystem and anything else that does not need to specify them.

Joint dynamics parameters could then be easily specified in any .urdf.xacro files in ur_simulation_gazebo, ur_simulation_ignition, whatever packages are driving #54, and other consumers.

I think any such proposed change should wait for #52 and build on top of it, because that's seems to be a helpful simplifying step toward consuming ur_macro.xacro in other contexts.

It's not the best or only fix for ur_simulation_gazebo issue 19, since setting urdf_joint_friction to a non-zero value is still kind of a hack, but there could be other reasons to modify the URDF joints' <dynamics> tags arbitrarily from outside of this package.

[danzimmerman]

I've started drafting an alternate approach based on my comment above, so I'm going to close this. Thanks for the comments @gavanderhoorn.

My alternate approach builds on top of the work in progress in #52 and that makes it easier to set package-specific joint damping/friction parameters, so I'll open something for discussion later if that moves forward.