# Gazebo freaks when joints hit limits

I'm finishing up a gazebo plugin for servo control of joints. I've noticed that whenever a joint goes past the lower/upper limits defined in the urdf, that all joints in my model fly off to Neverland and Gazebo starts reporting NaNs for velcoities.

This is non-idel abviously, but it's not even clear how to stop this other than putting rather large padding around the limits. In the case where the limit is close to, the natural variance of the ODE system can cause the joints to move around in the noise and even move the joint as much as 0.001 radians from it's last position, so any padding has to be at least 0.01 radians that I'm noticing now.

Any ideas on how to get Gazebo to not freak out when a joint goes (barely) over the limits? I generally drive my real robot to physical limits on certain kinds of joints, and I'd like to simulate this in Gazebo.

edit retag close merge delete

I figured out that the joint fly away because the Force was set too high. But even if you lower the force, the joints ALL shake whenever a single joint is not in the limits (een if this is something like -1.e-7 for position with a limit of 0.
( 2012-01-20 01:35:57 -0500 )edit

Sort by » oldest newest most voted

What you are seeing is numerical instability of your simulation. Assuming it's a problem of numerical stiffness of your simulated system, one way to fix this problem is by making the time steps smaller - i.e. update your world file's <physics:ode> block by decreasing <stepTime>, for example:

<physics:ode>
<stepTime>0.00001</stepTime>
<gravity>0 0 -9.8</gravity>
<cfm>0.0000000001</cfm>
<erp>0.2</erp>
<quickStep>true</quickStep>
<quickStepIters>10</quickStepIters>
<quickStepW>1.3</quickStepW>
<contactMaxCorrectingVel>100.0</contactMaxCorrectingVel>
<contactSurfaceLayer>0.001</contactSurfaceLayer>
</physics:ode>


Another fix is to force the constraint solver to provide a better solution of the system by increasing the number of inner iterations by increasing <quickstepIters>:

<physics:ode>
<stepTime>0.001</stepTime>
<gravity>0 0 -9.8</gravity>
<cfm>0.0000000001</cfm>
<erp>0.2</erp>
<quickStep>true</quickStep>
<quickStepIters>1000</quickStepIters>
<quickStepW>1.3</quickStepW>
<contactMaxCorrectingVel>100.0</contactMaxCorrectingVel>
<contactSurfaceLayer>0.001</contactSurfaceLayer>
</physics:ode>


Note that both modifications above help stabilize simulation but slow things down as well.

Alternatively, you can modify the joint constraint properties by tuning stopKp and stopKd for your joint (try lowering ERP at the joint limit). For example, add this XML extension to your model urdf:

<gazebo reference="your_problematic_joint">
<stopKd value="1000000.0" />
<stopKp value="1000000.0" />
</gazebo>


Note that the relationship between time step size h, kp, kd, cfm and erp are (as extracted from ODEPhysics.cc in gazebo):

    // Compute the CFM and ERP by assuming the two bodies form a
// spring-damper system.
kp = 1.0 / (1.0 / geom1->surface->kp + 1.0 / geom2->surface->kp);
kd = geom1->surface->kd + geom2->surface->kd;
contact.surface.soft_erp = h * kp / (h * kp + kd);
contact.surface.soft_cfm = 1.0 / (h * kp + kd);


More details on the CFM and ERP parameters can be found in the ODE manual.

If you can't stabilize the system via one or all of the above suggestions, please provide

• Your complete model specification.
• Specify what kind of controller are you using (e.g. pid force control, velocity control, ... etc.).
• Plots of position/velocity/force vs. time for the joint right before the instability happens will be very helpful as well.
• And preferably a way for others to reproduce the same issue you are seeing.
more

I am trying to impelment force control using the ft sensor gazebo_ros_plugin mounted inside a joint which connects the base link with the contact link of my robot. My problem is how to specifiy the elasticity coeff. and the damping coeff. in the joint. Do you have any suggestions?

( 2017-06-08 10:49:00 -0500 )edit

I'm currently facing a similar issue: a gimbal i'm modeling rapidly oscillates when it reaches a joint limit. No reasonable amount of force could return it to acceptable joint range.

Setting <contact_max_correcting_vel> to a small (1 digit) number/ or 0 fixes this issue. The default value of 100 seems to have been causing the behavior. Unfortunately this also changes the entire system behavior so this may not be a viable solution.

I'm not widely knowledgeable in this area, so I can't explain why this solution works, or confidently predict if it will work for different models.

cheers

more