Introduction

This tutorial shows how to model a 4-bar linkage in Gazebo, with examples using SDFormat and URDF. This is a challenge because URDF uses a tree structure to represent robot kinematics, which does not permit closed loops like a 4-bar linkage. SDFormat is able to model closed kinematic loops because it uses a graph structure. Since gazebo converts URDF to SDF before loading, we can declare an SDFormat joint inside <gazebo> extension tags to close the loop. That joint will get added to the model after being converted to SDFormat.

Simple 4-bar linkage in SDFormat

This is an SDFormat example of a 4-bar linkage connected to the ground at each end. It has 4 revolute joints labeled joint_A, joint_B, joint_C, and joint_D and 3 links named link_AB, link_BC, and link_CD.

The model files are in the four_bar_sdf folder. To use this model, create a folder ~/.gazebo/models/four_bar_sdf and copy the model.config and model.sdf to that folder. You can then insert the model into a simulation using the Insert panel in the left side of the gazebo client.

For brevity, the model parameters are encoded in an embedded ruby template file named model.sdf.erb. The model.sdf can be generated from the template using the erb command: erb -T 1 model.sdf.erb > model.sdf.

This allows geometric parameters to be defined in one place along with a helper function for computing the moment of inertia of a uniform box.

  # Box dimensions
  t = 0.02
  X = 0.20
  Y = 1.5 * X
  z0 = 2*t

  def box_inertia(dx, dy, dz)
    density = 600
    box = {}
    box[:size]   = Vector[dx, dy, dz]
    box[:volume] = dx * dy * dz
    box[:mass] = density * box[:volume]
    box[:ixx]  = box[:mass]/12.0 * (dy**2 + dz**2)
    box[:iyy]  = box[:mass]/12.0 * (dz**2 + dx**2)
    box[:izz]  = box[:mass]/12.0 * (dx**2 + dy**2)
    box[:ixy]  = 0.0
    box[:ixz]  = 0.0
    box[:iyz]  = 0.0
    box
  end

  # Points

The link parameters are stored in a dictionary named boxes:

  # Points A, D are attached to ground
  # Links AB, BC, CD connected by revolute joints
  #
  #            A          D
  # ^Y         |          |
  # |          |          |
  # |   X      |          |
  # O--->      B----------C
  #
  boxes = {}

  boxes["AB"] = box_inertia(t, Y, t);
  boxes["AB"][:offset] = Vector[-t/2, -Y/2, 0]

  boxes["BC"] = box_inertia(X, t, t);
  boxes["BC"][:offset] = Vector[X/2, -(Y+t/2), 0]

  boxes["CD"] = box_inertia(t, Y, t);
  boxes["CD"][:offset] = Vector[X+t/2, -Y/2, 0]

  # Revolute

and the joint parameters are stored in a dictionary named joints:

  # Revolute joints placed at inside corners of boxes
  joints = {}

  joints["A"] = {}
  joints["A"][:type] = "revolute"
  joints["A"][:parent] = "world"
  joints["A"][:child] = "link_AB"
  joints["A"][:pose] = Vector[t/2, Y/2, 0, 0, 0, 0]
  joints["A"][:axis] = Vector[0, 0, 1]

  joints["B"] = {}
  joints["B"][:type] = "revolute"
  joints["B"][:parent] = "link_AB"
  joints["B"][:child] = "link_BC"
  joints["B"][:pose] = Vector[-X/2, t/2, 0, 0, 0, 0]
  joints["B"][:axis] = Vector[0, 0, 1]

  joints["D"] = {}
  joints["D"][:type] = "revolute"
  joints["D"][:parent] = "world"
  joints["D"][:child] = "link_CD"
  joints["D"][:pose] = Vector[-t/2, Y/2, 0, 0, 0, 0]
  joints["D"][:axis] = Vector[0, 0, 1]

  joints["C"] = {}
  joints["C"][:type] = "revolute"
  joints["C"][:parent] = "link_CD"
  joints["C"][:child] = "link_BC"
  joints["C"][:pose] = Vector[X/2, t/2, 0, 0, 0, 0]
  joints["C"][:axis] = Vector[0, 0, 1]

  # end first ruby code block

A model template is then included that references the computed parameters for each link and joint:

<sdf version="1.5">
  <model name="four_bar_sdf">
    <pose>0 0 <%= z0 %>  0 0 0</pose>
    <self_collide>true</self_collide>
<%
  boxes.keys.each do |k|
    box = boxes[k]
    link_name = "link_#{k}"
%>
    <link name="<%= link_name %>">
      <pose><%= a_to_s(box[:offset]) %>  0 0 0</pose>
      <inertial>
        <mass><%= box[:mass] %></mass>
        <inertia>
          <ixx><%= box[:ixx] %></ixx>
          <iyy><%= box[:iyy] %></iyy>
          <izz><%= box[:izz] %></izz>
          <ixy><%= box[:ixy] %></ixy>
          <ixz><%= box[:ixz] %></ixz>
          <iyz><%= box[:iyz] %></iyz>
        </inertia>
      </inertial>
      <collision name="box<%= k %>Collision">
        <geometry>
          <box>
            <size><%= a_to_s(box[:size]) %></size>
          </box>
        </geometry>
      </collision>
      <visual name="box<%= k %>Visual">
        <geometry>
          <box>
            <size><%= a_to_s(box[:size]) %></size>
          </box>
        </geometry>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/Wood</name>
          </script>
        </material>
      </visual>
    </link>
<%
  end
  joints.keys.each do |k|
    joint = joints[k]
    joint_name = "joint_#{k}"
%>
    <joint name="<%= joint_name %>" type="<%= joint[:type] %>">
      <pose><%= a_to_s(joint[:pose]) %></pose>
      <parent><%= joint[:parent] %></parent>
      <child><%= joint[:child] %></child>
      <axis>
        <xyz><%= a_to_s(joint[:axis]) %></xyz>
      </axis>
    </joint>
<%
  end
%>
  </model>
</sdf>

The full model.sdf is instantiated from this template, as you can see in the snippet below:

    <link name="link_CD">
      <pose>0.21000000000000002 -0.15000000000000002 0  0 0 0</pose>
      <inertial>
        <mass>0.07200000000000001</mass>
        <inertia>
          <ixx>0.0005424000000000002</ixx>
          <iyy>4.800000000000001e-06</iyy>
          <izz>0.0005424000000000002</izz>
          <ixy>0.0</ixy>
          <ixz>0.0</ixz>
          <iyz>0.0</iyz>
        </inertia>
      </inertial>
      <collision name="boxCDCollision">
        <geometry>
          <box>
            <size>0.02 0.30000000000000004 0.02</size>
          </box>
        </geometry>
      </collision>
      <visual name="boxCDVisual">
        <geometry>
          <box>
            <size>0.02 0.30000000000000004 0.02</size>
          </box>
        </geometry>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/Wood</name>
          </script>
        </material>
      </visual>
    </link>
    <joint name="joint_A" type="revolute">
      <pose>0.01 0.15000000000000002 0 0 0 0</pose>
      <parent>world</parent>
      <child>link_AB</child>
      <axis>
        <xyz>0 0 1</xyz>
      </axis>
    </joint>
    <joint name="joint_B" type="revolute">

Split 4-bar linkage with an extra fixed joint

A model with kinematic loops can be partially modeled as a tree by cutting some of the loops. This can be done by removing a joint or by splitting a link in half. The loop can be closed again by adding an extra joint. In this example, the middle link is split in half and a fixed joint is added to hold them together. It has the same 4 revolute joints joint_A, joint_B, joint_C, and joint_D but with 4 links link_AB, link_BE, link_EC, link_CD and a fixed joint joint_E.

The SDFormat model files are in the four_bar_split_fixed_sdf folder. To use this model, create a folder ~/.gazebo/models/four_bar_split_fixed_sdf and copy the model.config and model.sdf to that folder. You can also examine the model template created with embedded ruby to see how the model is constructed.

The difference between the four_bar_sdf and four_bar_split_fixed_sdf model templates is shown below:

--- a/kinematic_loop/four_bar_sdf/model.sdf.erb
+++ b/kinematic_loop/four_bar_split_fixed_sdf/model.sdf.erb
@@ -35,15 +35,21 @@
   # ^Y         |          |
   # |          |          |
   # |   X      |          |
-  # O--->      B----------C
+  # O--->      B-----E----C
   #
   boxes = {}

   boxes["AB"] = box_inertia(t, Y, t);
   boxes["AB"][:offset] = Vector[-t/2, -Y/2, 0]

-  boxes["BC"] = box_inertia(X, t, t);
-  boxes["BC"][:offset] = Vector[X/2, -(Y+t/2), 0]
+  # boxes["BC"] = box_inertia(X, t, t);
+  # boxes["BC"][:offset] = Vector[X/2, -(Y+t/2), 0]
+
+  boxes["BE"] = box_inertia(X/2, t, t);
+  boxes["BE"][:offset] = Vector[X/4, -(Y+t/2), 0]
+
+  boxes["EC"] = box_inertia(X/2, t, t);
+  boxes["EC"][:offset] = Vector[X*3/4, -(Y+t/2), 0]

   boxes["CD"] = box_inertia(t, Y, t);
   boxes["CD"][:offset] = Vector[X+t/2, -Y/2, 0]
@@ -61,8 +67,8 @@
   joints["B"] = {}
   joints["B"][:type] = "revolute"
   joints["B"][:parent] = "link_AB"
-  joints["B"][:child] = "link_BC"
-  joints["B"][:pose] = Vector[-X/2, t/2, 0, 0, 0, 0]
+  joints["B"][:child] = "link_BE"
+  joints["B"][:pose] = Vector[-X/4, t/2, 0, 0, 0, 0]
   joints["B"][:axis] = Vector[0, 0, 1]

   joints["D"] = {}
@@ -75,11 +81,15 @@
   joints["C"] = {}
   joints["C"][:type] = "revolute"
   joints["C"][:parent] = "link_CD"
-  joints["C"][:child] = "link_BC"
-  joints["C"][:pose] = Vector[X/2, t/2, 0, 0, 0, 0]
+  joints["C"][:child] = "link_EC"
+  joints["C"][:pose] = Vector[X/4, t/2, 0, 0, 0, 0]
   joints["C"][:axis] = Vector[0, 0, 1]

-  # end first ruby code block
+  joints["E"] = {}
+  joints["E"][:type] = "fixed"
+  joints["E"][:parent] = "link_BE"
+  joints["E"][:child] = "link_EC"
+  joints["E"][:pose] = Vector[-X/4, 0, 0, 0, 0, 0]
 %>
 <sdf version="1.5">
   <model name="four_bar_sdf">
@@ -134,9 +144,15 @@
       <pose><%= a_to_s(joint[:pose]) %></pose>
       <parent><%= joint[:parent] %></parent>
       <child><%= joint[:child] %></child>
+<%
+    if joint.has_key?(:axis)
+%>
       <axis>
         <xyz><%= a_to_s(joint[:axis]) %></xyz>
       </axis>
+<%
+    end
+%>
     </joint>
 <%
   end

Split 4-bar linkage in URDF with an SDFormat fixed joint

The revolute joints in the split 4-bar linkage can be modeled in URDF with the fixed joint added using a <gazebo> extension tag. This URDF model is in the four_bar_split_fixed_urdf folder with model.config and model.urdf files and the embedded ruby model template. The model.urdf can be generated from the template using the erb command: erb -T 1 model.urdf.erb > model.urdf. Comparing this model with the four_bar_split_fixed_sdf model provides a useful comparison of how coordinate frames are defined in SDFormat and URDF.

The SDFormat fixed joint is specified in gazebo tags:

  <!-- SDFormat joints -->
  <gazebo>
    <joint name="joint_E" type="fixed">
      <pose>-0.1 -0.01 0 0 0 0</pose>
      <parent>link_BE</parent>
      <child>link_EC</child>
    </joint>
  </gazebo>
</robot>