Controller makes robot unstable in Gazebo

edit

Fuerte, Gazebo 1.6.16

Hi all, I've setup a robot model in a world. I start the simulation paused and everything is ok, but when I press 'play' the controller won't hold the robots position.

From it's resting position, it actually jiggles a bit then flops the links to one side. I've tried adjusting the parameters... I wonder if my robot's mass is too light. The measurements are accurate, but being in meter units the inertia matrix is very small for each link.

e.g. for one link:

<inertial>
    <mass value="0.5692" />
    <origin xyz="-0.000000157 0.014876512 0.044359344"/>
    <inertia ixx="0.001427609768" ixy="-0.000000003731" ixz="0.000000282945" 
                                  iyy= "0.000797193532" iyz="0.000106505679" 
                                                        izz= "0.001191235228" />
</inertial>

I'm using the robot_mechanism_controllers/JointPositionController controller for each joint, with:

  pid:
    p: 2.0
    i: 0.0
    d: 0.5
    i_clamp: 0.0

Gazebo settings in the URDF are

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

<selfCollide>true</selfCollide>
<static>false</static>
<turnGravityOff>false</turnGravityOff>

Do I need to set these other parameters?:

<initial_joint_position>    ?
<stopKd value="1.0" />   ?
<stopKp value="1000000.0" />   ?

World settings are:

<physics type="ode">
  <gravity xyz="0 0 -9.8"/>
  <ode>
    <solver type="quick" dt="0.001" iters="10" sor="1.3"/>
    <constraints cfm="0.0" erp="0.1" contact_max_correcting_vel="10.0" contact_surface_layer="0.001"/>
  </ode>
</physics>

This is so frustrating...

Thanks for your help.