Including a Spherical (Ball) Joint in Robot Model
I'm having trouble including a spherical/ball joint in my robot model. I have tried using the 'ball' joint type defined by sdf (http://sdformat.org/spec?ver=1.6&elem=joint#joint_type) but the urdf will no longer compile, throwing the error: 'Joint [joint_name] has no known type [ball]'.
I have also tried building a spherical joint using three orthogonal universal joints. The model does not throw any errors, and in rviz everything is displayed as expected and without issue. In gazebo however, neither the three universal joints composing the spherical joint nor the child link show up, or are listed as part of the model.
.xacro:
...
<link name="${leg_name}_psi_link">
<visual><geometry><sphere radius=".00001"/></geometry></visual>
<collision><geometry><sphere radius=".00001"/></geometry></collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value=".00001"/>
<inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" />
</link>
<link name="${leg_name}_theta_link">
<visual><geometry><sphere radius=".00001"/></geometry></visual>
<collision><geometry><sphere radius=".00001"/></geometry></collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value=".00001"/>
<inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" />
</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>
<xacro:default_inertial mass="${foot_mass}"/>
</link>
<joint name="${leg_name}_psi_joint" type="continuous" >
<parent link="${leg_name}_tibia_link" />
<child link="${leg_name}_psi_link" />
<axis xyz="0 0 1" />
<dynamics damping="0.005" />
<origin xyz="0.075 0 0" rpy="0 0 0" />
<limit lower="${-pi/2}" upper="${-pi/2}" velocity="${joint_velocity_limit}" effort="${joint_effort_limit}" />
</joint>
<joint name="${leg_name}_theta_joint" type="continuous" >
<parent link="${leg_name}_psi_link"/>
<child link="${leg_name}_theta_link"/>
<axis xyz="0 1 0" />
<dynamics damping="0.005" />
<origin xyz="0 0 0" rpy="0 0 0" />
<limit lower="${-pi/2}" upper="${-pi/2}" velocity="${joint_velocity_limit}" effort="${joint_effort_limit}" />
</joint>
<joint name="${leg_name}_phi_joint" type="continuous" >
<parent link="${leg_name}_theta_link"/>
<child link="${leg_name}_foot_link"/>
<axis xyz="1 0 0" />
<dynamics damping="0.005" />
<origin xyz="0 0 0" rpy="0 0 0" />
<limit lower="${-pi/2}" upper="${-pi/2}" velocity="${joint_velocity_limit}" effort="${joint_effort_limit}" />
</joint>
...
launch file (relevent parts)
<!-- Load robot description -->
<param name="robot_description" command="xacro --inorder 'path/to/robot.xacro'" />
<!-- load the controllers -->
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" ns="/syropod" args="joint_state_controller
AL_coxa_joint
AL_femur_joint
AL_tibia_joint
AR_coxa_joint
AR_femur_joint
AR_tibia_joint
BL_coxa_joint
BL_femur_joint
BL_tibia_joint
BR_coxa_joint
BR_femur_joint
BR_tibia_joint"/>
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model"
args="-urdf
-model syropod
-param robot_description
-x 0.0
-y 0.0
-z 0.2
-R 0.0
-P 0.0
-Y 0.0
-J AL_coxa_joint 0.0
-J AL_femur_joint 0.0
-J AL_tibia_joint 0.0
-J AL_psi_joint 0.0
-J AL_theta_joint 0.0
-J AL_phi_joint 0.0
-J AR_coxa_joint 0.0
-J AR_femur_joint 0.0
-J AR_tibia_joint 0.0
-J AR_psi_joint 0.0
-J AR_theta_joint 0.0
-J AR_phi_joint 0.0
-J BL_coxa_joint 0.0
-J BL_femur_joint 0.0
-J BL_tibia_joint 0.0
-J BL_psi_joint 0.0
-J BL_theta_joint 0.0
-J BL_phi_joint 0.0
-J BR_coxa_joint 0.0
-J BR_femur_joint 0.0
-J BR_tibia_joint 0.0
-J BR_psi_joint 0.0
-J BR_theta_joint 0.0
-J BR_phi_joint 0.0"
respawn="false" output="screen"/>
Why do none of the joints or links appear in the robot model?What am I missing/doing wrong? Is there a better way to do this?
Notes
- the three orthogonal joints represent the roll pitch yaw of the spherical joint they compose. their mass and inertia matrices are close to zero intentionally. they are not zero because I understand that this can cause issues in gazebo.
- ${legname}foot_link is the end effector link
Thanks
Asked by JeremySMorgan on 2018-11-12 03:41:27 UTC
Answers
I can see a few things.
SDF and URDF are different formats that support different sets of joint types. Unfortunately ball
joints are only supported by SDF and not URDF!
You have defined three continuous
joints but also given them limits, this will not work. By definition a continuous joint doesn't have any limits, you want to use the revolute
joint type instead. Hopefully this will get this showing up in Gazebo. You may still have some issues with your joint at singularities but it should be a good approximation of a ball joint in URDF.
NOTE: A universal
joint is a specific type of joint which includes two revolute joints with axes at 90 degrees to each other. This composite joint type is only supported by SDF.
Asked by PeteBlackerThe3rd on 2018-11-13 05:13:05 UTC
Comments
thanks! I didn't realize urdf and sdf had a different set of possible joint types. I would give you an upvote by I don't have the 3points required XD. I also realized that the issue was actually that I didn't have a
Asked by JeremySMorgan on 2018-11-13 20:24:24 UTC
No problem, yes URDF and SDF are so similar and so different it can be confusing. I've just given you enough karma to up vote BTW ;-)
Asked by PeteBlackerThe3rd on 2018-11-16 07:04:58 UTC
The issue is that <origin>, <mass>, and <inertia> tags must be nested inside a <inertial> element for each of the links, instead of being nested in the <link> tag.
correct:
<link name="${leg_name}_psi_link"> <visual><geometry><sphere radius=".01"/></geometry></visual> <collision><geometry><sphere radius=".005"/></geometry></collision> <inertial> <origin xyz="0 0 0" rpy="0 0 0"/> <mass value="${gimbal_link_mass}"/> <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/> </inertial> </link>
incorrect:
<link name="${leg_name}_psi_link"> <visual><geometry><sphere radius=".01"/></geometry></visual> <collision><geometry><sphere radius=".005"/></geometry></collision> <origin xyz="0 0 0" rpy="0 0 0"/> <mass value="${gimbal_link_mass}"/> <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/> </link>
additional notes:
- un-actuated joints should not have a <transmission> tag
- gazebo will not load links with zero mass
Asked by JeremySMorgan on 2018-11-13 20:56:10 UTC
Comments