Issues Simulating Parallel Manipulator in Gazebo Harmonic: Joint Parent Conflict

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!

Gazebo Harmonic introduced support for mimic joints. Would that fit your use case?

If you have a question: Please ask on Robotics Stack Exchange according to our support guidelines. This is not the right place.