diff --git a/launch/description.launch b/launch/description.launch
index ecd3642dfcd5aca47fbb59e4978a639d911ceba2..f3efcecd3487484798d530cfaedf36c98ac03eb2 100644
--- a/launch/description.launch
+++ b/launch/description.launch
@@ -4,6 +4,7 @@
<arg name="ns" default="dabo"/>
<arg name="model" default="dabo"/>
<arg name="static_wheels" default="false" />
+ <arg name="joint_state_pub" default="false"/>
<group ns="$(arg ns)">
@@ -15,15 +16,14 @@
<param name="publish_frequency" type="double" value="20.0"/>
</node>
- <!--
<node pkg="joint_state_publisher"
type="joint_state_publisher"
- name="joint_state_publisher">
+ name="joint_state_publisher"
+ if="$(arg joint_state_pub)">
<param name="use_gui" value="true"/>
<remap from="$(arg ns)/joint_states" to="$(arg ns)/joint_states/"/>
<remap from="$(arg ns)/robot_description" to="$(arg ns)/robot_description/"/>
</node>
- -->
</group>
diff --git a/urdf/dabo.xacro b/urdf/dabo.xacro
index a89bd8983d0e50fc07216511cbfdef9cf50b889a..d8fea04c049c245b16b1594e8be6b74d9eaa3d0f 100644
--- a/urdf/dabo.xacro
+++ b/urdf/dabo.xacro
@@ -20,17 +20,20 @@
<xacro:include filename="$(find iri_dabo_description)/urdf/include/head.xacro" />
<xacro:head name="$(arg name)" parent="top_plate">
- <origin xyz="0 0 0.7" rpy="0 0 0" />
+ <origin xyz="-0.03 0 0.57" rpy="0 0 0" />
</xacro:head>
<xacro:include filename="$(find iri_dabo_description)/urdf/include/arm.xacro" />
<xacro:arm name="$(arg name)" prefix="left" parent="top_plate">
- <origin xyz="0 0.25 0.5" rpy="0.15 0 0" />
+ <origin xyz="-0.05 0.17 0.5" rpy="0.0 0 0" />
</xacro:arm>
<xacro:include filename="$(find iri_dabo_description)/urdf/include/arm.xacro" />
<xacro:arm name="$(arg name)" prefix="right" parent="top_plate">
- <origin xyz="0 -0.25 0.5" rpy="0.15 0 3.14159" />
+ <origin xyz="-0.05 -0.17 0.5" rpy="0.0 0 3.14159" />
</xacro:arm>
+ <xacro:include filename="$(find iri_dabo_description)/urdf/include/control.gazebo" />
+ <xacro:control name="$(arg name)"/>
+
</robot>
\ No newline at end of file
diff --git a/urdf/dabo_fixed.xacro b/urdf/dabo_fixed.xacro
new file mode 100644
index 0000000000000000000000000000000000000000..53a2afa8ee83957a62b169ee45919217f319f1e5
--- /dev/null
+++ b/urdf/dabo_fixed.xacro
@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+
+<robot name="dabo" xmlns:xacro="http://ros.org/wiki/xacro">
+
+ <xacro:arg name="name" default="dabo"/>
+
+ <xacro:include filename="$(find iri_segway_rmp200_description)/urdf/include/segway_rmp200.xacro" />
+ <xacro:segway_rmp200 name="$(arg name)" sim_config="$(find iri_dabo_gazebo)/config/dabo_sim_config.yaml"/>
+
+ <xacro:include filename="$(find iri_segway_rmp200_description)/urdf/include/caster_support_wheels.xacro" />
+ <xacro:caster_support_wheels/>
+
+ <xacro:include filename="$(find iri_dabo_description)/urdf/include/body.xacro" />
+ <xacro:body name="$(arg name)" parent="top_plate">
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ </xacro:body>
+
+ <xacro:include filename="$(find iri_dabo_description)/urdf/include/sensors.xacro" />
+ <xacro:sensors name="$(arg name)" parent="base_link"/>
+
+ <xacro:include filename="$(find iri_dabo_description)/urdf/include/head_fixed.xacro" />
+ <xacro:head name="$(arg name)" parent="top_plate">
+ <origin xyz="-0.03 0 0.57" rpy="0 0 0" />
+ </xacro:head>
+
+ <xacro:include filename="$(find iri_dabo_description)/urdf/include/arm_fixed.xacro" />
+ <xacro:arm name="$(arg name)" prefix="left" parent="top_plate">
+ <origin xyz="-0.05 0.17 0.5" rpy="0.0 0 0" />
+ </xacro:arm>
+
+ <xacro:include filename="$(find iri_dabo_description)/urdf/include/arm_fixed.xacro" />
+ <xacro:arm name="$(arg name)" prefix="right" parent="top_plate">
+ <origin xyz="-0.05 -0.17 0.5" rpy="0.0 0 3.14159" />
+ </xacro:arm>
+
+</robot>
\ No newline at end of file
diff --git a/urdf/include/arm.xacro b/urdf/include/arm.xacro
index 097e6c6fef87715615cad830b5c310841cd7472c..bc8008e05020819f7d36756ef0970953f27ff79d 100644
--- a/urdf/include/arm.xacro
+++ b/urdf/include/arm.xacro
@@ -2,17 +2,64 @@
<root xmlns:xacro="http://ros.org/wiki/xacro">
+ <xacro:include filename="$(find iri_joint_trajectory_gazebo)/urdf/servo.xacro"/>
+
+ <xacro:property name="PI" value="3.1415926535897931" />
+ <xacro:property name="servo1_name" value="arm_servo1" />
+ <xacro:property name="servo2_name" value="arm_servo2" />
+ <xacro:property name="load_name" value="arm" />
+ <xacro:property name="joint1_name" value="arm_pan" />
+ <xacro:property name="joint2_name" value="arm_tilt" />
+
<xacro:macro name="arm" params="prefix name parent *origin">
+
+ <joint name="${parent}2${prefix}_${servo1_name}_joint" type="fixed" >
+ <parent link="${parent}"/>
+ <child link="${prefix}_${servo1_name}"/>
+ <xacro:insert_block name="origin"/>
+ </joint>
+
+ <xacro:servo name="${prefix}_${servo1_name}">
+ <container>
+ <geometry>
+ <box size="0.08 0.05 0.1"/>
+ </geometry>
+ </container>
+ <container>
+ <material name="black">
+ <color rgba="0 0 0 1"/>
+ </material>
+ </container>
+ <container>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ </container>
+ </xacro:servo>
- <link name="${prefix}_arm">
- <visual>
+ <xacro:servo name="${prefix}_${servo2_name}">
+ <container>
+ <geometry>
+ <box size="0.1 0.05 0.05"/>
+ </geometry>
+ </container>
+ <container>
+ <material name="black">
+ <color rgba="0 0 0 1"/>
+ </material>
+ </container>
+ <container>
<origin rpy="0 0 0" xyz="0 0 0"/>
+ </container>
+ </xacro:servo>
+
+ <link name="${prefix}_${load_name}">
+ <visual>
+ <origin xyz="0 0 0" rpy="0 -${PI/2} -${PI}"/>
<geometry>
<mesh filename="package://iri_dabo_description/meshes/arm_low.dae"/>
</geometry>
</visual>
<collision>
- <origin xyz="0 0.025 -0.15" rpy="0 0 0"/>
+ <origin xyz="-0.15 -0.025 0" rpy="0 -${PI/2} -${PI}"/>
<geometry>
<cylinder length="0.4" radius="0.07"/>
</geometry>
@@ -26,12 +73,51 @@
izz="0.0001"/>
</inertial>
</link>
+
+
+ <xacro:servo_joint name ="${prefix}_${joint1_name}"
+ parent="${prefix}_${servo1_name}"
+ child ="${prefix}_${servo2_name}">
+ <container>
+ <origin rpy="-${PI/2} -${PI/2} 0" xyz="0 0.05 0"/>
+ </container>
+ <container>
+ <limit effort="25.0" lower="-${PI}" upper="${PI}" velocity="0.5"/>
+ </container>
+ </xacro:servo_joint>
+
+ <xacro:servo_transmission name="${prefix}_${joint1_name}"
+ joint_interface="EffortJointInterface"
+ reduction="1">
+ </xacro:servo_transmission>
- <joint name="${parent}2${prefix}_arm_joint" type="fixed" >
- <parent link="${parent}"/>
- <child link="${prefix}_arm"/>
- <xacro:insert_block name="origin"/>
- </joint>
+ <xacro:servo_joint name ="${prefix}_${joint2_name}"
+ parent="${prefix}_${servo2_name}"
+ child ="${prefix}_${load_name}">
+ <container>
+ <origin rpy="-${PI/2} 0 0" xyz="0 0 0.025"/>
+ </container>
+ <container>
+ <limit effort="25.0" lower="0" upper="${PI/2}" velocity="0.5"/>
+ </container>
+ </xacro:servo_joint>
+
+ <xacro:servo_transmission name="${prefix}_${joint2_name}"
+ joint_interface="EffortJointInterface"
+ reduction="1">
+ </xacro:servo_transmission>
+
+ <gazebo reference="${prefix}_${servo1_name}">
+ <mu1>0.2</mu1>
+ <mu2>0.2</mu2>
+ <material>Gazebo/Black</material>
+ </gazebo>
+
+ <gazebo reference="${prefix}_${servo2_name}">
+ <mu1>0.2</mu1>
+ <mu2>0.2</mu2>
+ <material>Gazebo/Black</material>
+ </gazebo>
</xacro:macro>
diff --git a/urdf/include/arm_fixed.xacro b/urdf/include/arm_fixed.xacro
new file mode 100644
index 0000000000000000000000000000000000000000..e1196c163f286cf03036e9f849af03e305581aff
--- /dev/null
+++ b/urdf/include/arm_fixed.xacro
@@ -0,0 +1,107 @@
+<?xml version="1.0"?>
+
+<root xmlns:xacro="http://ros.org/wiki/xacro">
+
+ <xacro:include filename="$(find iri_joint_trajectory_gazebo)/urdf/servo.xacro"/>
+
+ <xacro:property name="PI" value="3.1415926535897931" />
+ <xacro:property name="servo1_name" value="arm_servo1" />
+ <xacro:property name="servo2_name" value="arm_servo2" />
+ <xacro:property name="load_name" value="arm" />
+ <xacro:property name="joint1_name" value="arm_pan" />
+ <xacro:property name="joint2_name" value="arm_tilt" />
+
+ <xacro:macro name="arm" params="prefix name parent *origin">
+
+ <joint name="${parent}2${prefix}_${servo1_name}_joint" type="fixed" >
+ <parent link="${parent}"/>
+ <child link="${prefix}_${servo1_name}"/>
+ <xacro:insert_block name="origin"/>
+ </joint>
+
+ <xacro:servo name="${prefix}_${servo1_name}">
+ <container>
+ <geometry>
+ <box size="0.08 0.05 0.1"/>
+ </geometry>
+ </container>
+ <container>
+ <material name="black">
+ <color rgba="0 0 0 1"/>
+ </material>
+ </container>
+ <container>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ </container>
+ </xacro:servo>
+
+ <xacro:servo name="${prefix}_${servo2_name}">
+ <container>
+ <geometry>
+ <box size="0.1 0.05 0.05"/>
+ </geometry>
+ </container>
+ <container>
+ <material name="black">
+ <color rgba="0 0 0 1"/>
+ </material>
+ </container>
+ <container>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ </container>
+ </xacro:servo>
+
+ <link name="${prefix}_${load_name}">
+ <visual>
+ <origin xyz="0 0 0" rpy="0 -${PI/2} -${PI}"/>
+ <geometry>
+ <mesh filename="package://iri_dabo_description/meshes/arm_low.dae"/>
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="-0.15 -0.025 0" rpy="0 -${PI/2} -${PI}"/>
+ <geometry>
+ <cylinder length="0.4" radius="0.07"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <mass value="1"/>
+ <inertia
+ ixx="0.0001" ixy="0.0" ixz="0.0"
+ iyy="0.0001" iyz="0.0"
+ izz="0.0001"/>
+ </inertial>
+ </link>
+
+ <joint name="${prefix}_${joint1_name}_joint"
+ type="fixed">
+ <parent link="${prefix}_${servo1_name}"/>
+ <child link="${prefix}_${servo2_name}"/>
+ <origin rpy="-${PI/2} -${PI/2} 0" xyz="0 0.05 0"/>
+ <axis xyz="0 0 1"/>
+ </joint>
+
+ <joint name="${prefix}_${joint2_name}_joint"
+ type="fixed">
+ <parent link="${prefix}_${servo2_name}"/>
+ <child link="${prefix}_${load_name}"/>
+ <origin rpy="-${PI/2} 0 0" xyz="0 0 0.025"/>
+ <axis xyz="0 0 1"/>
+ </joint>
+
+ <gazebo reference="${prefix}_${servo1_name}">
+ <mu1>0.2</mu1>
+ <mu2>0.2</mu2>
+ <material>Gazebo/Black</material>
+ </gazebo>
+
+ <gazebo reference="${prefix}_${servo2_name}">
+ <mu1>0.2</mu1>
+ <mu2>0.2</mu2>
+ <material>Gazebo/Black</material>
+ </gazebo>
+
+ </xacro:macro>
+
+</root>
diff --git a/urdf/include/body.xacro b/urdf/include/body.xacro
index 0dbe0a7f6bcf53d6bcc7fd694ee455996b64d038..c2bacf14507432fbbdbae08ff77dd07137fd3243 100644
--- a/urdf/include/body.xacro
+++ b/urdf/include/body.xacro
@@ -11,6 +11,12 @@
<mesh filename="package://iri_dabo_description/meshes/body_low_dabo.dae"/>
</geometry>
</visual>
+ <collision>
+ <origin xyz="0 0.0 0.3" rpy="0 0 0"/>
+ <geometry>
+ <box size="0.3 0.4 0.6"/>
+ </geometry>
+ </collision>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="1"/>
diff --git a/urdf/include/control.gazebo b/urdf/include/control.gazebo
new file mode 100644
index 0000000000000000000000000000000000000000..47400d2daa11cb4029e819bd541fe780e93afd58
--- /dev/null
+++ b/urdf/include/control.gazebo
@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+
+<root xmlns:xacro="http://ros.org/wiki/xacro">
+
+ <xacro:macro name="control" params="name">
+
+ <xacro:property name="ctrl_period" value="0.001"/>
+
+ <gazebo>
+ <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
+ <robotNamespace>${name}</robotNamespace>
+ <controlPeriod>${ctrl_period}</controlPeriod>
+ <legacyModeNS>true</legacyModeNS>
+ </plugin>
+ </gazebo>
+
+ </xacro:macro>
+
+</root>
\ No newline at end of file
diff --git a/urdf/include/head.xacro b/urdf/include/head.xacro
index 3e5cb4ebe343807d6d22a64135cd064b8cde947d..9cbbe8b433385aacc2d37cb6ecf5bc5bf45fa34d 100644
--- a/urdf/include/head.xacro
+++ b/urdf/include/head.xacro
@@ -2,17 +2,82 @@
<root xmlns:xacro="http://ros.org/wiki/xacro">
+ <xacro:include filename="$(find iri_joint_trajectory_gazebo)/urdf/servo.xacro"/>
+
+ <xacro:property name="PI" value="3.1415926535897931" />
+ <xacro:property name="servo1_name" value="head_servo1" />
+ <xacro:property name="servo2_name" value="head_servo2" />
+ <xacro:property name="servo3_name" value="head_servo3" />
+ <xacro:property name="load_name" value="head" />
+ <xacro:property name="joint1_name" value="head_pan" />
+ <xacro:property name="joint2_name" value="head_roll" />
+ <xacro:property name="joint3_name" value="head_tilt" />
+
<xacro:macro name="head" params="name parent *origin">
+
+ <joint name="${parent}2${servo1_name}_joint" type="fixed">
+ <parent link="${parent}"/>
+ <child link="${servo1_name}"/>
+ <xacro:insert_block name="origin"/>
+ </joint>
- <link name="head">
+ <xacro:servo name="${servo1_name}">
+ <container>
+ <geometry>
+ <box size="0.054 0.032 0.04"/>
+ </geometry>
+ </container>
+ <container>
+ <material name="black">
+ <color rgba="0 0 0 1"/>
+ </material>
+ </container>
+ <container>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ </container>
+ </xacro:servo>
+
+ <xacro:servo name="${servo2_name}">
+ <container>
+ <geometry>
+ <box size="0.04 0.032 0.054"/>
+ </geometry>
+ </container>
+ <container>
+ <material name="black">
+ <color rgba="0 0 0 1"/>
+ </material>
+ </container>
+ <container>
+ <origin rpy="0 0 0" xyz="-0.02 0 0.05"/>
+ </container>
+ </xacro:servo>
+
+ <xacro:servo name="${servo3_name}">
+ <container>
+ <geometry>
+ <box size="0.032 0.04 0.054"/>
+ </geometry>
+ </container>
+ <container>
+ <material name="black">
+ <color rgba="0 0 0 1"/>
+ </material>
+ </container>
+ <container>
+ <origin rpy="0 0 0" xyz="0 -0.02 -0.027"/>
+ </container>
+ </xacro:servo>
+
+ <link name="${load_name}">
<visual>
- <origin rpy="1.5707 0 0" xyz="0 0 0"/>
+ <origin xyz="-0.05 0.01 0" rpy="0 0 -${PI/2}"/>
<geometry>
<mesh filename="package://iri_dabo_description/meshes/head_low_dabo.dae"/>
</geometry>
</visual>
<collision>
- <origin xyz="0 0 0.05" rpy="0 0 0"/>
+ <origin xyz="0 0.01 0" rpy="0 0 0"/>
<geometry>
<sphere radius="0.15"/>
</geometry>
@@ -26,13 +91,72 @@
izz="0.00750394"/>
</inertial>
</link>
+
+ <xacro:servo_joint name="${joint1_name}"
+ parent="${servo1_name}"
+ child="${servo2_name}">
+ <container>
+ <origin rpy="0 0 0" xyz="0.01722 0 0.0"/>
+ </container>
+ <container>
+ <limit effort="25.0" lower="-${PI/2}" upper="${PI/2}" velocity="5"/>
+ </container>
+ </xacro:servo_joint>
- <joint name="${parent}2head_joint" type="fixed">
- <parent link="${parent}"/>
- <child link="head"/>
- <xacro:insert_block name="origin"/>
- <axis xyz="0 0 1"/>
- </joint>
+ <xacro:servo_transmission name="${joint1_name}"
+ joint_interface="EffortJointInterface"
+ reduction="1">
+ </xacro:servo_transmission>
+
+ <xacro:servo_joint name="${joint2_name}"
+ parent="${servo2_name}"
+ child="${servo3_name}">
+ <container>
+ <origin rpy="-${PI/2} 0 -${PI/2}" xyz="0 0 0.08293"/>
+ </container>
+ <container>
+ <limit effort="25.0" lower="-0.7" upper="0.7" velocity="5"/>
+ </container>
+ </xacro:servo_joint>
+
+ <xacro:servo_transmission name="${joint2_name}"
+ joint_interface="EffortJointInterface"
+ reduction="1">
+ </xacro:servo_transmission>
+
+ <xacro:servo_joint name="${joint3_name}"
+ parent="${servo3_name}"
+ child="${load_name}">
+ <container>
+ <origin rpy="-${PI/2} 0 -${PI/2}" xyz="0 -0.060 0.007"/>
+ </container>
+ <container>
+ <limit effort="25.0" lower="-0.49" upper="0.11" velocity="5"/>
+ </container>
+ </xacro:servo_joint>
+
+ <xacro:servo_transmission name="${joint3_name}"
+ joint_interface="EffortJointInterface"
+ reduction="1">
+ </xacro:servo_transmission>
+
+ <gazebo reference="${servo1_name}">
+ <mu1>0.2</mu1>
+ <mu2>0.2</mu2>
+ <material>Gazebo/Black</material>
+ </gazebo>
+
+ <gazebo reference="${servo2_name}">
+ <mu1>0.2</mu1>
+ <mu2>0.2</mu2>
+ <material>Gazebo/Black</material>
+ </gazebo>
+
+ <gazebo reference="${servo3_name}">
+ <mu1>0.2</mu1>
+ <mu2>0.2</mu2>
+ <material>Gazebo/Black</material>
+ </gazebo>
</xacro:macro>
diff --git a/urdf/include/head_fixed.xacro b/urdf/include/head_fixed.xacro
new file mode 100644
index 0000000000000000000000000000000000000000..9cc1228b3aef864a5ab7603b4d5894b6d5562496
--- /dev/null
+++ b/urdf/include/head_fixed.xacro
@@ -0,0 +1,136 @@
+<?xml version="1.0"?>
+
+<root xmlns:xacro="http://ros.org/wiki/xacro">
+
+ <xacro:include filename="$(find iri_joint_trajectory_gazebo)/urdf/servo.xacro"/>
+
+ <xacro:property name="PI" value="3.1415926535897931" />
+ <xacro:property name="servo1_name" value="head_servo1" />
+ <xacro:property name="servo2_name" value="head_servo2" />
+ <xacro:property name="servo3_name" value="head_servo3" />
+ <xacro:property name="load_name" value="head" />
+ <xacro:property name="joint1_name" value="head_pan" />
+ <xacro:property name="joint2_name" value="head_roll" />
+ <xacro:property name="joint3_name" value="head_tilt" />
+
+ <xacro:macro name="head" params="name parent *origin">
+
+ <joint name="${parent}2${servo1_name}_joint" type="fixed">
+ <parent link="${parent}"/>
+ <child link="${servo1_name}"/>
+ <xacro:insert_block name="origin"/>
+ </joint>
+
+ <xacro:servo name="${servo1_name}">
+ <container>
+ <geometry>
+ <box size="0.054 0.032 0.04"/>
+ </geometry>
+ </container>
+ <container>
+ <material name="black">
+ <color rgba="0 0 0 1"/>
+ </material>
+ </container>
+ <container>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ </container>
+ </xacro:servo>
+
+ <xacro:servo name="${servo2_name}">
+ <container>
+ <geometry>
+ <box size="0.04 0.032 0.054"/>
+ </geometry>
+ </container>
+ <container>
+ <material name="black">
+ <color rgba="0 0 0 1"/>
+ </material>
+ </container>
+ <container>
+ <origin rpy="0 0 0" xyz="-0.02 0 0.05"/>
+ </container>
+ </xacro:servo>
+
+ <xacro:servo name="${servo3_name}">
+ <container>
+ <geometry>
+ <box size="0.032 0.04 0.054"/>
+ </geometry>
+ </container>
+ <container>
+ <material name="black">
+ <color rgba="0 0 0 1"/>
+ </material>
+ </container>
+ <container>
+ <origin rpy="0 0 0" xyz="0 -0.02 -0.027"/>
+ </container>
+ </xacro:servo>
+
+ <link name="${load_name}">
+ <visual>
+ <origin xyz="-0.05 0.01 0" rpy="0 0 -${PI/2}"/>
+ <geometry>
+ <mesh filename="package://iri_dabo_description/meshes/head_low_dabo.dae"/>
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0.05 0.05" rpy="0 0 0"/>
+ <geometry>
+ <sphere radius="0.15"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin xyz="0.07366358 0.00094968 -0.00868937" rpy="0 0 0"/>
+ <mass value="1.0"/>
+ <inertia
+ ixx="0.00966943" ixy="-0.00004500" ixz="-0.00064634"
+ iyy="0.00845836" iyz="0.00001799"
+ izz="0.00750394"/>
+ </inertial>
+ </link>
+
+ <joint name="${joint1_name}" type="fixed">
+ <parent link="${servo1_name}"/>
+ <child link="${servo2_name}"/>
+ <origin rpy="0 0 0" xyz="0.01722 0 0.0"/>
+ <axis xyz="0 0 1"/>
+ </joint>
+
+ <joint name="${joint2_name}" type="fixed">
+ <parent link="${servo2_name}"/>
+ <child link="${servo3_name}"/>
+ <origin rpy="-${PI/2} 0 -${PI/2}" xyz="0 0 0.08293"/>
+ <axis xyz="0 0 1"/>
+ </joint>
+
+ <joint name="${joint3_name}" type="fixed">
+ <parent link="${servo3_name}"/>
+ <child link="${load_name}"/>
+ <origin rpy="-${PI/2} 0 -${PI/2}" xyz="0 -0.060 0.007"/>
+ <axis xyz="0 0 1"/>
+ </joint>
+
+ <gazebo reference="${servo1_name}">
+ <mu1>0.2</mu1>
+ <mu2>0.2</mu2>
+ <material>Gazebo/Black</material>
+ </gazebo>
+
+ <gazebo reference="${servo2_name}">
+ <mu1>0.2</mu1>
+ <mu2>0.2</mu2>
+ <material>Gazebo/Black</material>
+ </gazebo>
+
+ <gazebo reference="${servo3_name}">
+ <mu1>0.2</mu1>
+ <mu2>0.2</mu2>
+ <material>Gazebo/Black</material>
+ </gazebo>
+
+ </xacro:macro>
+
+</root>
\ No newline at end of file