ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Links disconnect in (custom) spherical joint in urdf.xacro

asked 2020-12-08 03:51:06 -0500

praskot gravatar image

updated 2020-12-08 03:59:45 -0500

Created a custom spherical joint using dummy links to simulate a spherical pendulum in Gazebo. Code below,

<xacro:macro name="rotational_joint" params="parent child jx jy jz r p y">
    <xacro:dummy_link name="${parent}_${child}_dummylink1"/>
    <xacro:dummy_link name="${parent}_${child}_dummylink2"/>
    <joint name="${parent}_${child}_roll" type="continuous">
        <parent link="${parent}"/>
        <child link="${parent}_${child}_dummylink1"/>
        <axis xyz="1 0 0"/>
        <!-- <joint_properties damping="0.0" friction="0.0" /> -->
        <origin xyz="${jx} ${jy} ${jz}" rpy="${r} 0 0"/>
    </joint>
    <joint name="${parent}_${child}_pitch" type="continuous">
        <parent link="${parent}_${child}_dummylink1"/>
        <child link="${parent}_${chilC:\fakepath\sp_smallmass.gifd}_dummylink2"/>
        <axis xyz="0 1 0"/>
        <!-- <joint_properties damping="0.0" friction="0.0" /> -->
        <origin xyz="0 0 0" rpy="0 ${p} 0"/>
    </joint>
    <joint name="${parent}_${child}_yaw" type="continuous">
        <parent link="${parent}_${child}_dummylink2"/>
        <child link="${child}"/>
        <axis xyz="0 0 1"/>
        <!-- <joint_properties damping="0.0" friction="0.0" /> -->
        <origin xyz="0 0 0" rpy="0 0 ${y}"/>
    </joint>
</xacro:macro>

It works (looks) good for smaller pendulum mass but for larger mass values, the joint disconnects and (moves along the cable length in addition to the rotations). See image below (left larger mass value, right smaller mass). image description

Link to the videos vid1, vid2. Full urdf file here.

What can I try to fix this issue?


I understand there is a universal joint in Gazebo link1, link2. However, I haven't been able to get it to work. From link 2, could I potentially create a model_plugin to incorporate a spherical pendulum? Any suggestions?


edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-12-15 02:43:43 -0500

praskot gravatar image

updated 2020-12-15 02:44:58 -0500

Based on the accepted answer here, these two options helped,

  1. Reducing the time_step from 0.001 to 0.0001 (obviously this slows the simulation time)
  2. Increasing sor_pgs_iters to 1000 (even up to 10000) (and this too slows the simulations, though not as much as option 1)

(PS: Still not comfortable with the resulting physics, the joint shouldn't disconnect and will try to work on a custom-model-plugin)

edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2020-12-08 03:51:06 -0500

Seen: 270 times

Last updated: Dec 15 '20