End Effector Link 'Floating' Around

edit

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=GSsB2...

See Gazebo demonstration (issue apparent): https://www.youtube.com/watch?v=fiZu8...

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