Change ur_gazebo to EffortJointInterface [discussion]

[gavanderhoorn]

As part of the melodic-devel-staging breaking changes (#448) we should consider whether to change the models in ur_gazebo to a hardware_interface/EffortJointInterface by default, instead of the hardware_interface/PositionJointInterface they have now.

Gazebo works better when everything in a simulation is (ultimately) effort controlled, especially when models in the simulation need to interact.

While the updated ur_gazebo (#518) allows for the hardware_interface to be configured using a .launch file, making use of any hardware_interface other than PositionJointInterface will also require updates to the controllers (which are currently configured to expect a PositionJointInterface).

From a UX perspective, only a whole-sale change from position to effort makes sense to me.

A disadvantage would be that the effort interface requires properly tuned PIDs to work correctly and have the inertia and mass parameters properly set.

And this means PIDs for the no-load case as well as the case where the robot is outfitted with a gripper and is lifting some object.

The latter could be left to the user/integrator, but the former would certainly have to be done by the maintainers here.

[gavanderhoorn]

Related previous discussion: #87 and #161 (which actually removed the EffortJointInterface) and #227 (which makes it configurable again).

[gavanderhoorn]

We could also configure a set of both position and effort controllers in the controller configuration .yamls.

The position ones will be started by default, with the option for the user to switch to the effort ones.

It would be the user's responsibility to make sure she's also started the simulation with the effort hardware_interface.

Trying to load/start an effort-based controller when that interface is unavailable will simply return a nr of fatal errors.

[matteolucchi]

First: thanks for the great work @gavanderhoorn, your contributions have been very helpful !

I am also struggling with some issues related to the use of the PositionJointInterface. I will try to tune the PID params, at least for the UR10 and if I have any success I'll report back here

[gavanderhoorn]

@matteolucchi wrote:

I am also struggling with some issues related to the use of the PositionJointInterface. I will try to tune the PID params, at least for the UR10 and if I have any success I'll report back here

Please make sure to check the melodic-devel-staging branch, as things may have changed wrt kinetic-devel.

And #518 changes quite a few things in ur_gazebo, so that would be something to take into account as well.

And ros-industrial/abb_experimental#110 shows some nice work for an ABB IRB 1200 on a similar topic. Of course this is not directly usable for the URs here, but the linked tutorial and explanation of the work done might help.

[matteolucchi]

Please make sure to check the melodic-devel-staging branch, as things may have changed wrt kinetic-devel.

And #518 changes quite a few things in ur_gazebo, so that would be something to take into account as well.

Yes, I was thinking to start from #518 to ease things for a possible future pull request.

And ros-industrial/abb_experimental#110 shows some nice work for an ABB IRB 1200 on a similar topic. Of course this is not directly usable for the URs here, but the linked tutorial and explanation of the work done might help.

Thank you ! I will look into that!

[gavanderhoorn]

Oh and I almost forgot: #504.

As we actually know the tensors, masses and cogs for all robots, we should use those.

Perhaps @fmauch could use some help with that @matteolucchi.

[matteolucchi]

Ok so If I got it right, until now @fmauch updated the cog of the UR16e in #504, and this should be replicated for all the other models. Once this is done, all the physical parameters of the different models should be correct (or good enough), and I could proceed with switching to the EffortJointInterface and try to tune the PID params. Do I get it right?

[gavanderhoorn]

until now @fmauch updated the cog of the UR16e in #504, and this should be replicated for all the other models.

Whatever we have for the other variants should be compared to the information @fmauch dug up.

Once this is done, all the physical parameters of the different models should be correct (or good enough)

Well, they should at least correspond to a real robot, so ideally we'd use those, yes.

and I could proceed with switching to the EffortJointInterface and try to tune the PID params.

That would seem to be the procedure yes.

Luckily the UR10 has actual inertia values (see #504 (comment)), so you should be able to work on this even without the other variants having been fixed.

[matteolucchi]

Ok so I will get back to this once the steps mentioned #504 (comment) are completed

[gavanderhoorn]

@ipa-nhg: you moved this to in progress.

As a discussion item, or are you working on the actual changes needed?

[fmauch]

I've done a quick&dirty implementation for a UR16e to test #504: https://github.com/fmauch/universal_robot/tree/effort_controller

I can move the robot, but the PID values are far from good, I think.

[ipa-nhg]

@gavanderhoorn
@ipa-kut is working on this now. He will send later a PR with the changes, we can then test the new controllers and decide how to continue

[matteolucchi]

I am now done with the tuning of Gazebo PID parameters for all the UR models, see #526 . I based my pull request on #525.

[danzimmerman]

I've pulled in @matteolucchi's #526 to melodic-devel-staging on my own fork to investigate the and wanted to report some issues I'm having with gravity-induced sag of the robot.

The switch to EffortJointInterface is effective in preventing the "exploding" robot problem when the robot contacts rigid obstacles, but it is preventing the robot from reaching a desired end-effector pose to "expected" precision (trajectory goal tolerances, etc.) when external forces (here, gravity) are applied. The exact error will be posture-dependent if gravity is on.

I bet it still makes sense for new users to have the EffortJointInterface as the default, but I feel like this kind of behavior might read as a "bug" to users who are expecting the simulated robot to have positioning errors on the order of those of the real machine's specs.

Initial Observation

My setup in Gazebo uses the UR5 (non-e) with a Robotiq FT-300 sensor and 2F85 gripper. I put in a physical pose target marker which made the issue clearly visible.

Here's an image with Gazebo gravity off (top) and on (bottom):

Below is a plot of the joint angle error with respect to the trajectory goal (b) and corresponding end-effector position error (d). Up until the dashed line, the robot was moving (The zoomed-out joint-space and end-effector trajectory can be seen in (a) and (c) respectively. I started my .rosbag before I sent the trajectory goal so I could record it.)

After the dashed line, the robot is nominally stationary, and I'm turning gravity on and off.

Although the angular errors are small, we're looking at a vertical end-effector position shift of nearly 10mm in this outstretched posture.

ur5_bringup.launch Default Setup

To rule out the gripper as an issue, I ran the same test with the default configuration started in Gazebo with roslaunch ur_gazebo ur5_bringup.launch and got similar results:

Without a visual reference it's a bit hard to see, but amounts to (d) about 7mm of end-effector position error when the robot is at its all-zeros home position:

The error is not as severe as with the extra mass of the gripper, but it's fairly large compared to the real robot.

Stiffnesses and Gains

I averaged the angular deflection and the joint torque over the gray shaded areas in the figure above to look at the joint stiffness for the joints with substantial torques. Those are the shoulder_lift_joint and the elbow_joint in this configuration, which are holding the full weight of the robot beyond them:

shoulder_lift_joint: -52.34168 Nm
elbow_joint: -14.45096 Nm

Their corresponding deflections in degrees:

shoulder_lift_joint ∆θ:0.2942 deg
elbow_joint ∆θ:0.3853 deg

and the implied stiffnesses dividing the joint torque in Nm by the angular deflection in radians:

shoulder_lift_joint τ/∆θ: -10194.91308 Nm/rad
elbow_joint τ/∆θ: -2148.83381 Nm/rad

The stiffnesses implied there are very close to the proportional gains in
https://github.com/matteolucchi/universal_robot/blob/gazebo_effort_interface/ur_gazebo/config/ur5_controllers.yaml#L17

shoulder_pan_joint: {p: 4000,  d: 200, i: 1, i_clamp: 1}    
shoulder_lift_joint: {p: 10000,  d: 200, i: 1, i_clamp: 1}    
elbow_joint: {p: 2000,  d: 20, i: 1, i_clamp: 1}   
wrist_1_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}
wrist_2_joint: {p: 500,  d: 1, i: 1, i_clamp: 1} 
wrist_3_joint: {p: 10,  d: 0.1, i: 0, i_clamp: 1}

If I'm not mistaken, this is to be expected, since the EffortJointInterface is controlling joint position error using an applied torque and with the integral term clamped to a low value (1 Nm, I guess?) the proportional term does the heavy lifting:

It applies a torque proportional to the angular error, and the gain is low enough for the total control torque to equilibrate with the actual torque due to gravity at a fairly large angular position error.

I've tried some naive tweaking of the PID values toward a more realistic stiffness, and tried increasing integral gain and clamp levels, but don't have a stable solution yet.

I believe that the physical joint stiffnesses on a UR5 are O(1e6) Nm/rad, and I'd be surprised if we could increase them that high. I will contribute anything further I find on that front, but I've stopped working on this in favor of returning to the position-controlled interface for my experiments.

Alternate Solutions besides EffortJointInterface?

My current solution to avoiding "explosion" on collision of the position-controlled robot with its environment is to soft-mount the obstacles using Gazebo's implicit spring-damper functionality:

These are .sdf.xacro files with three orthogonal axes of critically-damped prismatic spring-dampers (apparently there's some numerical rotation in there as well, but they're well behaved for pretty awful collisions)

My next attempt, which will work with regard to collisions with the floor, is to put spring-dampers in the joints of the robot itself and possibly its base mounting with very large physically realistic spring stiffnesses, to give the robot a little bit of compliance.

I think this may fix the "exploding" problem and result in a robot with a dynamics + statics behavior closer to the real robot than a direct-drive effort control interface might. Would be very interested to discuss if anyone's already tried an "elastic robot" approach instead.

[fmauch]

I'd close this since #619 is now merged. The mentioned issues in #521 (comment) still exist and should probably continued in a separate issue.

[danzimmerman]

The mentioned issues in #521 (comment) still exist and should probably continued in a separate issue.

Do you feel like this is an enhancement issue or maybe more of a documentation issue on different ways to simulate the UR arms?

Technical Enhancements

One mitigation with effort controllers if PID tweaking can't achieve acceptable sag might be to add feedforward terms, at least gravity compensation, to default effort controllers, but that seems like a big project for all of the arms, and it adds extra setup and complications (what if the base link orientation is not fixed in space, but subject to a change in orientation? Does the gravity compensation track it based on the base pose in the world? Is that a stable controller?)

Full dynamic compensation would be interesting too, but it sounds like it's dominated by the motor reflected inertias, per this comment:

#504 (comment)

I could see it being really valuable to some users to have high-fidelity models of these arms' dynamic response, but in my experience since I wrote the comment above, servoJ gain and lookahead time settings are significant contributors to the "dynamics" of the real arm and therefore important to capture, easily more than the physical arm dynamics.

URSim handles this in a useful way:

https://forum.universal-robots.com/t/what-behaviors-does-ur-sim-simulate/20709

but of course there's no contact simulation, no visualization of grippers, etc.

Contact simulation for manipulation is a fraught concept in Gazebo anyway, lots of people wondering why they can't achieve stable grasps.

Documentation - A UR Simulation Guide?

Maybe the best approach forward on this is to document the various issues in a central location, something like:

• A beginner tutorial on selecting position control instead of the default effort controller
• A discussion of the tradeoffs of position vs. effort control in Gazebo (explode-on-contact vs. gravity sag), including
  • Pictures/video examples of the undesired behaviors that arise with each controller
  • Some links into GitHub issues and summary of the discussion around this change
  • Some discussion of the PID controllers and where to set them to get into the concepts and code if they want to change something more advanced
• A section on URSim and why you might use that instead of Gazebo
• (Maybe, eventually) a section on mitigations for various use cases
  • More sophisticated controller schemes, especially if there are some plugins out there
  • Gazebo obstacle soft mounting as my comment above
  • Small joint elasticity for the robot as speculated above
  • Gazebo physics engine settings (I only tried cfm and erp tweaking in ODE, which I did not find satisfactory but others might)
  • ???

I have some blockers on contributing parts of the outline above, especially for ROS 1 beginner tutorial and proven/concrete mitigation examples.

I set this issue aside more than a year ago and currently don't have a ROS 1 installation. I've moved on to ROS 2.

However, I'd be happy to try to put together a draft docs PR with the portions I could contribute quickly from memory with existing media and examples. I think I probably have an "exploding Gazebo UR video" sitting around somewhere to go with the gravity sag images and figures above.

Unfortunately I set the soft-mounting and elastic-robot projects aside, and if I pick them back up I'll probably be using the newest (Ignition) Gazebo.

If a start to this kind of guide sounds good, where's a good place to open a PR?

[fmauch]

@danzimmerman First of all thank you very much for your continuous efforts with this. I think a good place for something like that would be the tutorials repo where we could add something to the documentation, as well as an example package to make it easy for users to try out different configurations. Any input there is very well appreciated :-)