migrate useful updates from local fork upstream (i.e. here)

[hsu]

We were testing simple object grasping and controls in gazebo, and forked a local copy here. There were various updates (mostly inertia updates that matches the geometry) that we would like check if it makes sense to push upstream. The final model we used to do dynamic grasp testing in gazebo is this urdf.

If model is updated upstream, we'll simply keep our custom controller config downstream.

[guihomework]

I answer here because I helped shadow rework most of the hand inertia in june last year to follow a rule in gazebo saying one should never have inertia varying by more than a order of magnitude between two links. The finger elements being light and small compared to the shadow arm, there were usually explosions or vibration in gazebo 1.5 and 1.9.5 (groovy and hydro). The inertia display feature is also new to me, and indeed it helps a lot visualize the gigantic inertias.
I will try out your suggested inertia, but if the problem of difference of inertia between linked element is now solved, we could go back to the correct inertia estimated a long time a go but removed.
for example ixx distal before june was 0.00031 now is 0.001 to follow the rule. I am quite sure the value before june was also multiplied by a factor of 100 because electric gazebo would never handle small inertia values.

[hsu]

@guihomework, thanks for the info! You might want to test with these physics parameters in ODE.

Note that sufficient joint damping (e.g. here it's set pretty high, but I think it does not need to be so high depending on controller gains)) and the flag ImplicitSpringDamper == true is very important to simulation stability in ODE.

In addition to minor improvements ODE inertia ratio issue in Gazebo 1.9.6, I highly recommend trying things out with alternate engines, see Physics Engine Support under Project Status, any feedback is highly welcome.

[guihomework]

@hsu , I analyzed your urdf, and indeed, new settings appear that I (and probably shadow) was not aware of. ImpliciteSpringDamper or older cfmDamping options were missed or never got into the shadow model when the support was added.
So I did some tests.
Your urdf loads nicely in indigo gazebo.
I dediced I would then update the inertia of the shadow hand model to the realistic ones I found in older commits (indeed the Ixx of distal was 0.0000031) and this led to exploding hand.
I then put exactly your inertia values => exploding hand.
I then started with your urdf and added back one at a time the differences with the generated urdf from the latest xacro from shadow. I found at least one culprit for hand exploding. Shadow and I always thought hand exploding came from too small inertia but it is the sr_gazebo_controller_manager plugin that makes the hand collapse or explode right now with your urdf.
You can look at the sr_description.urdf, here and starting sr_hand gazebo_hand.launch leads to hand collapse
https://github.com/ubi-agni/sr-ros-interface/blob/T_realistic_inertia/sr_description/sr_description.urdf
which is yours + a fix for transmission + the plugin loaded. I guess the plugin is doing something wrong when interfacing with the gazebo api, related to the inertia (this is the only change), but I don't know what to look for to fix that.
Until this is fixed, I would suggest not to try merging new inertias. After the plugin fix I will probably tweak the inertia again to get to the exact value I computed previously which should match everybody's need. Adding cfm parameters might also improve things in our vibration we noticed.

In june 2014 I tested the bullet engine which did not help on our vibration and collapsing issues. Now it makes sense, the problem was somewhere else.

Finally, I think I would like to set the effort limits (now 1000) in the joints to realistic values again as well as the velocities. Your urdf has old values.

hope this helps and wish you could guide us to fix the https://github.com/shadow-robot/sr-ros-interface/tree/indigo-devel/sr_gazebo_plugins

[guihomework]

After analyzing the sr_gazebo_plugin, it appears the damping was creating huge forces due to a lot of noise on the velocity reading
fingers appear static (no position change) but velocity of 0.5 radians/s are visible and of course changing sign.

There was no clamping of force. So adding
´´´
if (fst->joint_->limits)
{
double effort_limit= fst->joint_->limits->effort;
effort_command=std::min(std::max(effort_command, -effort_limit), effort_limit);
}
´´´
helped but mostly reducing the damping to 0.001 or so made the hand not explode but still vibrate a lot. 1 does not help and the suggested simulation parameters for ODE did not change or reduce the vibrations.

Retuning the hand finally made the hand a little more stable in some positions and unstable in others, so not as nice as with previous larger inertia.
Indeed very small inertia make the controller tuning (basic position_controller) really hard due to velocity noise and hence no adequate value of the derivative term can be set in the PID otherwise the fingers oscillates due to noise. There is not static friction, so a very little force on these very little inertia make everything vibrate.

Before retuning I redid the computation for all the inertia and setting these good values make the display of the inertia in gazebo look nice.

@hsu did you use your urdf of the shadow hand with real effort controllers in gazebo ? I could see there are gazebo motors, but I never used such things as we want to mimic the real hand behaviour which runs ros controllers.

So currently these smaller inertia create more problems then they solve. What is the advantage to set the inertia exactly to the value they should if this makes the simulation unstable ?

I did not have time to test a different physic simulator, but would be nice to get some new input from you regarding our tests.

[guihomework]

Follow up on velocity problems :

• I tried to remove velocity limits (setting them to unrealistic 30rad/s) to see if https://bitbucket.org/osrf/gazebo/issue/634/joint-velocity-limit-causes-significant was the issue.
  It did not improve, probably because of next point.
• I also read in one of your presentations (about ODE extensions atlas and sandia hand ) http://www.birs.ca/workshops/2014/14w5147/files/Steve%20Peters%20John%20Hsu%20OSRF%20Banff%20Presentation.pdf
  that explicit viscous damping was not working at 1ms for the hand and therefore implicit damping spring damper was required. Is this implemented as to gazebo 2.2.3 (indigo/trusty)

So I suppose we should remove the explicit damping from sr_gazebo_plugins (taken from previous pr2_gazebo_plugins)
Trying that results in stable simulation with dynamic damping set to 0.1. I will now try to retune the controllers and see If I can get a controllable hand this way.

[guihomework]

#199 should fullfil the request.
thanks a lot for the good suggestions.

However, I would need suggestions on how to fix a last problem

THJ2 (thumb middle link motion) currently reacts a bit too much to THJ5 movements. THJ1 which is after THJ2 is not affected at all. It looks like an inertia problem but it reacts in the same direction as the movement of THJ5 which is counter-intuitive when thinking in terms of inertia. Usually a link has a tendency to not move at t=0 due to inertia, hence creating a joint movement in the opposite direction to keep the chain assembled.

Inertia of the elements in-between were set correctly and also tested exagerated big or small with no effect to this problem.

I have no clue where this comes from. Any ideas ?