Hi all,
I’m working on simulating a parallel manipulator (a Stewart platform) using Gazebo Harmonic and running into issues related to joint definitions and model structure. I hope someone can help clarify what’s going wrong or suggest a workaround.
Problem Summary
When I launch my simulation, I get errors related to joint creation, specifically about a child link already having a parent joint. This seems to be a problem with the closed-loop kinematics in my SDF model.
model.sdf
<?xml version="1.0"?>
<sdf version="1.9">
<model name="closed_loop_arm">
<link name="base_link">
<pose>0 0 0 0 0 0</pose>
<inertial>
<mass>1.0</mass>
<inertia>
<ixx>0.01</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.01</iyy>
<iyz>0.0</iyz>
<izz>0.01</izz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<box>
<size>0.2 0.2 0.1</size>
</box>
</geometry>
<material>
<ambient>0.2 0.2 0.2 1</ambient>
<diffuse>0.2 0.2 0.2 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
</material>
</visual>
<collision name="collision">
<geometry>
<box>
<size>0.2 0.2 0.1</size>
</box>
</geometry>
</collision>
<enable_wind>false</enable_wind>
</link>
<link name="arm1_link">
<pose relative_to="base_link">0.1 0 0.05 0 0 0</pose>
<inertial>
<mass>0.5</mass>
<inertia>
<ixx>0.005</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.005</iyy>
<iyz>0.0</iyz>
<izz>0.005</izz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.03</radius>
<length>0.8</length>
</cylinder>
</geometry>
<pose>0 0 0.4 0 0 0</pose>
<material>
<ambient>0.0 0.0 0.8 1</ambient>
<diffuse>0.0 0.0 0.8 1</diffuse>
<specular>0.0 0.0 0.8 1</specular>
</material>
</visual>
<collision name="collision">
<geometry>
<cylinder>
<radius>0.03</radius>
<length>0.8</length>
</cylinder>
</geometry>
<pose>0 0 0.4 0 0 0</pose>
</collision>
</link>
<joint name="joint1" type="revolute">
<parent>base_link</parent>
<child>arm1_link</child>
<pose relative_to="base_link">0.1 0 0.05 0 0 0</pose>
<axis>
<xyz>0 0 1</xyz>
</axis>
<limit>
<lower>-1.57</lower>
<upper>1.57</upper>
</limit>
</joint>
<link name="arm2_link">
<pose relative_to="base_link">-0.1 0 0.05 0 0 0</pose>
<inertial>
<mass>0.5</mass>
<inertia>
<ixx>0.005</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.005</iyy>
<iyz>0.0</iyz>
<izz>0.005</izz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.03</radius>
<length>0.8</length>
</cylinder>
</geometry>
<pose>0 0 0.4 0 0 0</pose>
<material>
<ambient>0.8 0.0 0.0 1</ambient>
<diffuse>0.8 0.0 0.0 1</diffuse>
<specular>0.8 0.0 0.0 1</specular>
</material>
</visual>
<collision name="collision">
<geometry>
<cylinder>
<radius>0.03</radius>
<length>0.8</length>
</cylinder>
</geometry>
<pose>0 0 0.4 0 0 0</pose>
</collision>
</link>
<joint name="joint2" type="revolute">
<parent>base_link</parent>
<child>arm2_link</child>
<pose relative_to="base_link">-0.1 0 0.05 0 0 0</pose>
<axis>
<xyz>0 0 1</xyz>
</axis>
<limit>
<lower>-1.57</lower>
<upper>1.57</upper>
</limit>
</joint>
<link name="platform_link">
<pose relative_to="arm1_link">0 0 0.8 0 0 0</pose>
<inertial>
<mass>0.8</mass>
<inertia>
<ixx>0.05</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.05</iyy>
<iyz>0.0</iyz>
<izz>0.05</izz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<box>
<size>0.3 0.1 0.05</size>
</box>
</geometry>
<pose>0.15 0 -0.025 0 0 0</pose>
<material>
<ambient>0.8 0.8 0.0 1</ambient>
<diffuse>0.8 0.8 0.0 1</diffuse>
<specular>0.8 0.8 0.0 1</specular>
</material>
</visual>
<collision name="collision">
<geometry>
<box>
<size>0.3 0.1 0.05</size>
</box>
</geometry>
<pose>0.15 0 -0.025 0 0 0</pose>
</collision>
<enable_wind>false</enable_wind>
</link>
<joint name="joint3" type="revolute">
<parent>arm1_link</parent>
<child>platform_link</child>
<pose relative_to="arm1_link">0 0 0.8 0 0 0</pose>
<axis>
<xyz>0 1 0</xyz>
</axis>
</joint>
<joint name="joint4" type="revolute">
<parent>arm2_link</parent>
<child>platform_link</child>
<pose relative_to="arm2_link">0 0 0.8 0 0 0</pose>
<axis>
<xyz>0 1 0</xyz>
</axis>
</joint>
</model>
</sdf>
platform_test.sdf
<?xml version="1.0"?>
<sdf version="1.9">
<world name="default">
<physics type="dart" default="true">
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
</physics>
<gravity>0 0 -9.81</gravity>
<plugin
filename="libgz-sim-physics-system.so"
name="gz::sim::systems::Physics">
</plugin>
<plugin
filename="libgz-sim-user-commands-system.so"
name="gz::sim::systems::UserCommands">
</plugin>
<plugin
filename="libgz-sim-scene-broadcaster-system.so"
name="gz::sim::systems::SceneBroadcaster">
</plugin>
<include>
<uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/Ground Plane</uri>
</include>
<include>
<uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/Sun</uri>
</include>
<include>
<uri>model://stewart</uri>
<name>stewart</name>
<pose>0 0 0.2 0 0 0</pose>
</include>
</world>
</sdf>
Error Message
Here is the relevant error from the log:
[Err] [SDFFeatures.cc:1111] Asked to create a joint between links [arm2_link] as parent and [platform_link] as child, but the child link already has a parent joint of type [RevoluteJoint].
[Wrn] [EntityManagementFeatures.cc:469] No joint named [joint4] for modelID [5]
[Err] [SDFFeatures.cc:1111] Asked to create a joint between links [arm2_link] as parent and [platform_link] as child, but the child link already has a parent joint of type [RevoluteJoint].
Question
- Is there a way to model closed-loop kinematics (parallel manipulators) in SDF for Gazebo Harmonic?
- Is there a workaround or plugin that allows multiple parent joints for a single link, or a way to “mimic” this behavior for simulation purposes?
- Any advice on how to restructure the model to avoid this error, or pointers to example Stewart platform models that work with Gazebo Harmonic?
Thanks in advance for your help!