End Effector Link 'Floating' Around
The end effectors of the quadruped robot I am working with have gimbals allowing for non normal contact to the ground. I have modeled this by attaching the end effectors to their respective legs with ball/spherical joint's (composed of three orthogonal revolute joints). Unfortunately the end effectors are floating all over the place.
See Rviz Demonstration: https://www.youtube.com/watch?v=GSsB2q1r4qo
See Gazebo demonstration (issue apparent): https://www.youtube.com/watch?v=fiZu8H2Bkr0
urdf:
<!-- joints -->
<joint name="${leg_name}_psi_joint" type="revolute" >
<parent link="${leg_name}_tibia_link" />
<child link="${leg_name}_psi_link" />
<origin xyz="0.0775 0 0" rpy="0 0 0" />
<axis xyz="0 0 1" />
<limit lower="-1.57" upper="1.57" velocity="5" effort="10" />
<dynamics damping="${gimbal_damping}" />
</joint>
<joint name="${leg_name}_theta_joint" type="revolute" >
<parent link="${leg_name}_psi_link"/>
<child link="${leg_name}_theta_link"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<axis xyz="0 1 0" />
<limit lower="-1.57" upper="1.57" velocity="5" effort="10" />
</joint>
<joint name="${leg_name}_phi_joint" type="revolute" >
<parent link="${leg_name}_theta_link"/>
<child link="${leg_name}_foot_link"/>
<axis xyz="1 0 0" />
<origin xyz="0 0 0" rpy="0 0 0" />
<limit lower="-1.57" upper="1.57" velocity="5" effort="10" />
</joint>
<!-- links -->
<link name="${leg_name}_psi_link">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="$.0001"/>
<inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
</inertial>
</link>
<link name="${leg_name}_theta_link">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value=".0001"/>
<inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
</inertial>
</link>
<link name="${leg_name}_foot_link">
<visual><geometry><mesh filename="${meshes_path}/foot_link.stl" /></geometry></visual>
<collision><geometry><mesh filename="${meshes_path}/foot_link_collision.stl" /></geometry></collision>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="${foot_mass}"/>
<inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
</inertial>
</link>
Even with joint limits of 0 the end effector floats everywhere.
Why is this happening? How can I fix it?
Thanks
Asked by JeremySMorgan on 2018-11-16 21:49:52 UTC
Answers
The "inertia" matrix number can't be too small, especially zero. Try a bigger value.
Asked by littleghost on 2018-11-16 23:22:05 UTC
Comments