Properly making soft joints to world in Gazebo 9 + ROS Melodic
I'm trying to interact a position-controlled robot with "fixed" obstacles in its environment. In this case, they're a couple of floating spheres, "magically" fixed in space. I'm using default ros-melodic-desktop-full
installations to do this, in this case Gazebo 9.13.1 on Windows.
With <static>
obstacles and default constraint parameters, the robot "explodes" if it touches obstacles. This is reasonable behavior given the incompatible constraints of infinitely rigid object meets immovable object.
If I add some global constraint force mixing with <cfm>
tags at world->physics scope, it stabilizes the simulation and makes the robot's contacts with the world understandable, but it also allows the joints to slide apart. This is also correct behavior from the standpoint of what CFM does, but it's not the desired behavior.
What I'd like to do is attach the floating spheres to the world frame using spring-dampers that are much softer than the robot joints, so that they easily slide out of the way if the robot collides without pulling the robot joints apart visibly.
I'm having a very hard time finding a correct and complete description on how to do this, partially because any suggestions are scattered across many years of SDF file format changes, unanswered ROS Answers and Gazebo Answers questions, and so on.
I have successfully held the spheres in place with fixed joints so they move a little with relaxed constraints. After a long time experimenting (because of lack of a complete example) I figured out I could put those joints in a nested <model>
so I could write this "boundary condition" in my world file instead of the complex model file that defines the visual/inertial/collision geometry.
Now I'd like to soften the joints that hold the spheres in place. So I'm trying to set different <cfm>
and <erp>
values for the spheres' fixed joints, or maybe <implicit_spring_damper>
or ... what? Everything I've tried has resulted in the spheres' displacements being the same order of magnitude as the robot's displacement.
I still can't find a documentation source that's any more useful than http://sdformat.org/spec?elem=joint&v..., but that just kind of tells me which tags I might try, not how they fit together.
The image was captured using these settings for the red sphere:
<model name="soft_fixed_big_red">
<include>
<uri>model://big_red</uri>
<name>red_sphere</name>
</include>
<joint name="bc" type="fixed">
<parent>world</parent>
<child>red_sphere::link</child>
<physics>
<ode>
<implicit_spring_damper>1</implicit_spring_damper>
<cfm>0.9</cfm>
<erp>0.2</erp>
</ode>
<provide_feedback>true</provide_feedback>
</physics>
</joint>
<pose>0.15 0.50 0.45 0 0 0</pose>
</model>
The spheres had mass properties like solid plastic when I started. I tried making them thin, extremely light spherical shells, which made it shakier but roughly similar in relative sphere and robot displacements.
At the world level (which contains the red and blue sphere models, ground plane, and sun), I have this:
<physics name ...
<dynamics>
tagged joints may be a partial solution, but very verbose to go to 6DOF springinessI'm chasing the wrong modeling paradigm here and I think I'm probably going to go with virtual cardboard boxes for anything that can sit on the floor until I can get around to building some effort-controlled URx robots that more gracefully handle accidental contact with a rigid world.
However, the original question about using CFM and ERP to do 6DOF soft constraints comes up from time to time, hopefully someone can help there?
I tried to add two prismatic joints in series with a light link in between and now the red sphere behaves correctly for a little while when in contact with the robot but then implodes to the origin.
Serial joints now working correctly. Had the damping set too low on the second joint of the red sphere.
Working okay now with x, y, z, slides with critically-damped spring dampers using
<implicit_spring_damper>
set to 1 as well. Dummy link between joints is just 10 grams with kg-scale spheres, works okay.