robot slips after upgrade to kinetic
Dear all, After upgrading from Ubuntu 14.04 to 16.04 and from Indigo to Kinetic I'm experiencing a weird behavior from Gazebo and the HyQ robot model.
The complete robot model can be found here.
I made the minimal set of modifications to make it compatible with the new xacro standard (see corresponding branch)
Even after setting unrealistically high friction coefficients in the corresponding tag (see here), the robot can't walk and slips badly.
With the same description on Gazebo 2/ROS Indigo the robot always was fine.
I've narrowed down the issue to this portion of the URDF:
<!-- Foot link -->
<link name="${name}_foot">
<inertial>
<origin xyz="0 0 0"/>
<mass value="0.0"/>
<inertia ixx="0.0" iyy="0.0" izz="0.0" ixy="0.0" ixz="0.0" iyz="0.0"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="0.02175"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="0.02175"/>
</geometry>
</collision>
</link>
Apparently, a collision set as a perfect sphere is handled in a different way from Gazebo 2 to 7.
If I remove the collision field, the tip of the leg (which is not spherical) is taken for the collision, and the robot works fine.
My suspect goes to the torsional friction feature, which has been introduced in new versions of Gazebo (see this related question).
UPDATE
I've reduced (yet not solved) the problem by setting the fdir1
tag to a reasonable value (it was 0 0 0
). Here's how the link looks like now:
<link name="${name}_foot">
<inertial>
<origin xyz="0 0 0"/>
<mass value="0.07"/>
<inertia ixx="0.0000112" iyy="0.0000112" izz="0.0000112" ixy="0.0" ixz="0.0" iyz="0.0"/>
</inertial>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="0.02175"/>
</geometry>
</collision>
</link>
<gazebo reference="${name}_foot">
<mu1>1</mu1>
<mu2>1</mu2>
<fdir1>1 0 0</fdir1>
<maxContacts>1</maxContacts>
<collision name="${name}_lowerleg_fixed_joint_lump__${name}_foot_collision_1">
<geometry>
<sphere>
<radius>0.02175</radius>
</sphere>
</geometry>
<surface>
<friction>
<torsional>
<coefficient>10</coefficient>
<use_patch_radius>true</use_patch_radius>
<surface_radius>0.05</surface_radius>
<patch_radius>0.01</patch_radius>
</torsional>
</friction>
<contact>
<poissons_ratio>0.5</poissons_ratio>
<elastic_modulus>1e8</elastic_modulus>
</contact>
</surface>
</collision>
<material>Gazebo/Black</material>
</gazebo>
However, I still see slipping behavior (and switching contacts) when I try to move: https://youtu.be/NMUSZn1pm5U
the torsional friction is not enabled by default so I think it's probably not the problem. Given this link has 0 mass and inertia, I am assuming this link is connected with another one using fixed joint so they get lumped together? I would turn on contacts visualization and see if the blue contact points associated with this collision is unstable. If so, you can try tuning the contact params for the foot collision.
You are right, the foot is connected with the lower leg link through a fixed joint. The lowerleg has reasonable mass and inertia parameters. By looking at the contacts, you were right, they are unstable: https://youtu.be/mLbGiFolpfE What contact parameters should I tune exactly and where? And why wasn't this happening in Gazebo 2? Thanks.