Robot's joints break on Gazebo

[skhbk]

My Environment

• Ubuntu 22.04.1
• ROS 2 Humble desktop install

Description

When I start simulation by using ros2 launch ur_simulation_gazebo ur_sim_control.launch.py, the robot's joints break as in this image:

To clarify this phenomenon, I changed the initial joint positions as follows:

shoulder_pan_joint: 0.0
shoulder_lift_joint: -1.4
elbow_joint: 2.2
wrist_1_joint: -2.37
wrist_2_joint: -1.57
wrist_3_joint: 0.0

If the controller is not activated, this does not happen (the robot just lies down).

My Attempts

Turning off gravity

I added turnGravityOff tag for each link in the URDF description:

<gazebo reference="shoulder_link">
    <turnGravityOff>true</turnGravityOff>
</gazebo>

Then the robot looks fine, but it may behave the same for external forces other than gravity.

Increasing damping and friction

Increasing the values of damping and friction for each joint in URDF made the robot stable, but the joints still shift a little.

<dynamics damping="1" friction="1"/>

Adding CFM and ERP

I didn't see the effect of this.

<gazebo reference="shoulder_pan_joint">
    <implicitSpringDamper>true</implicitSpringDamper>
    <stopCfm>0</stopCfm>
    <stopErp>0.2</stopErp>
</gazebo>

[fmauch]

Thanks for reporting this. This package is still in a rather early stage (hence it is not released as binary form) and we currently don't have many resources available to work on this, I'm afraid.

[skhbk]

Thanks for your response.
Is this problem specific to my environment?

[danzimmerman]

Thanks for reporting this. This package is still in a rather early stage (hence it is not released as binary form) and we currently don't have many resources available to work on this, I'm afraid.

Are this package and the Ignition sim package in a similar state?

Thanks for your response.
Is this problem specific to my environment?

No, I'm seeing something similar on Rolling. The robot is actually undergoing slow relaxation oscillations in the relative joint poses in my case, but unfortunately my Gazebo video capture also isn't working 😅

(Edit: the oscillations and breaking joints happen for me in the all-zeros starting posture if I turn on a little bit of x-direction gravity)

[skhbk]

Thanks, @danzimmerman.
I captured a video with all-zeros posture and some x-direction gravity.

breaking_joints.mp4

[danzimmerman]

I captured a video with all-zeros posture and some x-direction gravity.

Yes, very similar to what I'm seeing. If I figure anything out about workarounds/fixes, I'll post here and/or make a PR.

[relffok]

Yes, very similar to what I'm seeing. If I figure anything out about workarounds/fixes, I'll post here and/or make a PR.

I'm experiencing the same issue on humble (galactic was fine).
Feel free to document your process, maybe we can figure it out together!

[danzimmerman]

galactic was fine

That's interesting.

Will definitely document what I find if I find anything. I just got back to working on this.

[danzimmerman]

Can confirm it works fine on Galactic. (Ubuntu 20.04 native install)

https://www.youtube.com/watch?v=wRaylxmKC4w

[danzimmerman]

These issues seem similar, several different suggestions there.

ros-controls/gazebo_ros2_control#73
ros-controls/gazebo_ros2_control#54

[danzimmerman]

@relffok Among other discussion of the Gazebo fmax parameter, I found this comment from a while back (I added the bold emphasis):

@JaehyunShim Here is my comments ros-controls/gazebo_ros2_control#44 (comment). There are several ways to workaround it. Currently, I created an additional gazebo plugin to help calling SetParam("fmax", 0, ) inside the Load() function, so that when joint_limit_interface is ready, I can simply remove my own plugin. You can also modify the gazebo_ros2_control to add in your own fix.

It seems like there's something similar in gazebo_ros_control for Noetic here where the joint's fmax is set to the joint's effort limit:

{
  // joint->SetParam("fmax") must be called if joint->SetAngle() or joint->SetParam("vel") are
  // going to be called. joint->SetParam("fmax") must *not* be called if joint->SetForce() is
  // going to be called.
#if GAZEBO_MAJOR_VERSION > 2
  joint->SetParam("fmax", 0, joint_effort_limits_[j]);
#else
  joint->SetMaxForce(0, joint_effort_limits_[j]);
#endif
}

Workaround Comparison

I forked and modified gazebo_ros2_control implementation of GazeboSystem::registerJoints(...) here and set a random large limit:

  // At first we do a test with an arbitrary force limit of 1e6
  this->dataPtr->sim_joints_[j]->SetParam("fmax", 0, 1000000.0);

To get the robot to move, I'm using

ros2 launch ur_robot_driver test_joint_trajectory_controller.launch.py

As-Is ❌

On the as-is humble branch of gazebo_ros2_control, the robot is badly broken:

https://youtu.be/4MPdITYEl54

Workaround ✅

On my dz/humble-fmax-test branch, the robot seems to work fine:

https://youtu.be/Gdoyjp_Gego

Next Steps

According to this comment, this comment and the Noetic code in ROS 1 gazebo_ros_control above, this is something that should be handled with the joint limits interface.

So I need to look into ros2_control/joint_limits_interface to see what's going on there.

I don't think I have access to joint limit data where I hacked it in to gazebo_ros2_control.

Also worth noting is this joint friction/damping plus use_sim_time workaround suggested by @shonigmann here, however my galactic installation has a mix of use_sim_time settings on the nodes, as far as I can tell has zero damping or friction specified in the joints (retrieved with gz model --model-name ur --info) and that works fine.

[danzimmerman]

Another observation:

When I set fmax from the code, the joints' friction parameters change to the value I set.

In gazebo_ros2_control/src/gazebo_system.cpp:

this->dataPtr->sim_joints_[j]->SetParam("fmax", 0, 867530.9);

In the description retrieved with gz model --model-name ur --info:

joint {
  name: "ur::shoulder_pan_joint"
  id: 47
  angle: 0.0059931542239271
  type: REVOLUTE
  parent: "ur::base_link"
  parent_id: 11
  child: "ur::shoulder_link"
  child_id: 16
  pose {
    position {
      x: 0
      y: 0
      z: 0
    }
    orientation {
      x: 0
      y: 0
      z: 0
      w: 1
    }
  }
  axis1 {
    xyz {
      x: 0
      y: 0
      z: 1
    }
    limit_lower: -6.28319
    limit_upper: 6.28319
    limit_effort: 150
    limit_velocity: 3.14159
    damping: 0
    friction: 867530.9                       <---------- Set from code by setting fmax
    use_parent_model_frame: false
    xyz_expressed_in: ""
  }
}

There's a Gazebo Answers post suggesting they need to be set together:

https://answers.gazebosim.org/question/19635/modfy-a-joints-friction-value-deuring-exacution/?answer=19758#post-id-19758

There's an archived Bitbucket PR for Gazebo discussing this a bit:

https://osrf-migration.github.io/gazebo-gh-pages/#!/osrf/gazebo/pull-requests/1221/page/1

[danzimmerman]

Another Gazebo Answers post about fmax and friction:

https://answers.gazebosim.org/question/24487/change-joint-characteristics-at-runtime/

To lock a joint, I would set the fmax field to a large value and vel to 0 (this is also how we set joint friction, using both fmax and vel)

The dParamFMax parameter is set here in ODEJoint::SetParam(...) (and at Line 444 for friction, which as the post says uses vel and fmax).

It also appears that a max_force tag in the SDF would pass through to the fmax parameter here at Line 131.

max_force/fmax seem to be the maximum force for an ODE joint motor as described here:

http://ode.org/wiki/index.php/Manual#Stops_and_motor_parameters

Joint motors solve all these problems: they bring the body up to speed in one time step, provided that does not take more force than is allowed. Joint motors need no extra parameters because they are actually implemented as constraints. They can effectively see one time step into the future to work out the correct force.

In prior implementations it seems like people have been setting fmax to the joint effort limit but given the description above I'm not sure that's right. The effort limit is a separate parameter anyway:

limit_lower: -6.28319
limit_upper: 6.28319
limit_effort: 150            <--- effort limit from URDF xacro
limit_velocity: 3.14159
damping: 0
friction: 867530.9         <--- fmax set in code

Some more docs on joint motors:

https://classic.gazebosim.org/tutorials?tut=set_velocity&cat=#SetJointVelocityUsingJointMotors

The first call sets the key vel to the velocity the joint should travel at. It is meters per second for prismatic joints and radians per second for all others. The other call sets the key fmax. It is the maximum force or torque a joint motor may apply during a time step. Set it larger than the force required to be at the target velocity at the next time step. Set it smaller to apply a force over many time steps until the velocity is reached.

In velocity/position control, I'd think we'd always want to stay well over the minimal amount of joint torque to achieve the next joint velocity in simulation.

I'd think the effort limits from this page should probably be handled some other supervisory way (joint_limits_interface?) if they're included at all in Gazebo simulation. The real UR robot will not start slowly accumulating velocity errors if you exceed its joint limits, it'll just fault.

I'm going to see if I can pass max_force through from the .urdf.xacro file instead of injecting it in the source code.