Add head. Fix torso.

tor_description/robots/tor_head.urdf.xacro

+<?xml version="1.0"?>
+<!--
+
+  Copyright (c) 2016, PAL Robotics, S.L.
+  All rights reserved.
+
+  This work is licensed under the Creative Commons Attribution-NonCommercial-NoDerivs 3.0 Unported License.
+  To view a copy of this license, visit http://creativecommons.org/licenses/by-nc-nd/3.0/ or send a letter to
+  Creative Commons, 444 Castro Street, Suite 900, Mountain View, California, 94041, USA.
+-->
+<robot name="tor" xmlns:xacro="http://www.ros.org/wiki/xacro"
+        xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+        xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+        xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface">
+
+  <xacro:include filename="$(find tor_description)/urdf/head/head.urdf.xacro" />
+
+ <link name="torso_2_link">
+    <inertial>
+      <origin xyz="-0.04551 -0.00053 0.16386" rpy="0.00000 0.00000 0.00000"/>
+      <mass value="17.55011"/>
+      <inertia ixx="0.37376900000" ixy="0.00063900000" ixz="0.01219600000"
+               iyy="0.24790200000" iyz="0.00000700000"
+               izz="0.28140400000"/>
+    </inertial>
+
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://tor_description/meshes/torso/torso_2.STL" scale="1 1 1"/>
+      </geometry>
+      <material name="LightGrey" />
+    </visual>
+
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://tor_description/meshes/torso/torso_2_collision.STL" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <xacro:tor_head name="head" parent="torso_2_link"/>
+
+  <!-- Generic simulator_gazebo plugins -->
+  <xacro:include filename="$(find tor_description)/gazebo/gazebo.urdf.xacro" />
+  <!-- Materials for visualization -->
+  <xacro:include filename="$(find tor_description)/urdf/materials.urdf.xacro" />
+
+</robot>

tor_description/robots/tor_lower_body_v2.urdf.xacro

@@ -16,17 +16,17 @@
   <xacro:include filename="$(find tor_description)/urdf/leg/leg_v2.urdf.xacro" />
     <link name="base_link">
       <inertial>
-        <origin xyz="-0.06071 0.00861 0.06368" rpy="0.00000 0.00000 0.00000"/>
-        <mass value="13.34865"/>
-        <inertia ixx="0.06652900000" ixy="-0.00035700000" ixz="0.00027600000"
-                 iyy="0.03746500000" iyz="-0.00125400000"
-                 izz="0.07895100000"/>
+        <origin xyz="-0.08222 0.00838 -0.07261" rpy="0.00000 0.00000 0.00000"/>
+        <mass value="13.53810"/>
+        <inertia ixx="0.06989000000" ixy="-0.00011700000" ixz="0.00023000000"
+                 iyy="0.03998200000" iyz="-0.00132500000"
+                 izz="0.08234500000"/>
       </inertial>
 
       <visual>
         <origin rpy="0 0 0" xyz="0 0 0"/>
         <geometry>
-          <mesh filename="package://tor_description/meshes/v2/base_link_lo_res.stl" scale="1 1 1"/>
+          <mesh filename="package://tor_description/meshes/torso/base_link.STL" scale="1 1 1"/>
         </geometry>
         <material name="DarkGrey" />
       </visual>
@@ -34,7 +34,7 @@
       <collision>
         <origin rpy="0 0 0" xyz="0 0 0"/>
         <geometry>
-          <mesh filename="package://tor_description/meshes/v2/base_link_collision.stl" scale="1 1 1"/>
+          <mesh filename="package://tor_description/meshes/torso/base_link_collision.STL" scale="1 1 1"/>
         </geometry>
       </collision>  
     </link>

tor_description/robots/upload_head.launch

+<launch>
+
+  <!-- Robot description -->
+  <param name="robot_description" command="$(find xacro)/xacro.py '$(find tor_description)/robots/tor_head.urdf.xacro'" />
+
+</launch>

tor_description/urdf/head/head.transmission.xacro

+<?xml version="1.0"?>
+<!--
+
+  Copyright (c) 2016, PAL Robotics, S.L.
+  All rights reserved.
+
+  This work is licensed under the Creative Commons Attribution-NonCommercial-NoDerivs 3.0 Unported License.
+  To view a copy of this license, visit http://creativecommons.org/licenses/by-nc-nd/3.0/ or send a letter to
+  Creative Commons, 444 Castro Street, Suite 900, Mountain View, California, 94041, USA.
+-->
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:macro name="tor_head_differential_transmission"
+               params="name number1 number2 act_reduction1 act_reduction2 jnt_reduction1 jnt_reduction2" >
+    <transmission name="torso_trans">
+      <type>transmission_interface/DifferentialTransmission</type>
+      <actuator name="${name}_${number1}_motor">
+        <role>actuator1</role>
+        <mechanicalReduction>${act_reduction1}</mechanicalReduction>
+      </actuator>
+      <actuator name="${name}_${number2}_motor">
+        <role>actuator2</role>
+        <mechanicalReduction>${act_reduction2}</mechanicalReduction>
+      </actuator>
+      <joint name="${name}_${number1}_joint">
+        <role>joint1</role>
+        <offset>0.0</offset>
+        <mechanicalReduction>${jnt_reduction1}</mechanicalReduction>
+        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      </joint>
+      <joint name="${name}_${number2}_joint">
+        <role>joint2</role>
+        <offset>0.0</offset>
+        <mechanicalReduction>${jnt_reduction2}</mechanicalReduction>
+        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      </joint>
+    </transmission>
+  </xacro:macro>
+
+
+</robot>

tor_description/urdf/head/head.urdf.xacro

+<?xml version="1.0"?>
+<!--
+
+  Copyright (c) 2016, PAL Robotics, S.L.
+  All rights reserved.
+
+  This work is licensed under the Creative Commons Attribution-NonCommercial-NoDerivs 3.0 Unported License.
+  To view a copy of this license, visit http://creativecommons.org/licenses/by-nc-nd/3.0/ or send a letter to
+  Creative Commons, 444 Castro Street, Suite 900, Mountain View, California, 94041, USA.
+-->
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <!--File includes-->
+  <xacro:include filename="$(find tor_description)/urdf/deg_to_rad.xacro" />
+  <xacro:include filename="$(find tor_description)/urdf/head/head.transmission.xacro" />
+  <xacro:include filename="$(find tor_description)/urdf/sensors/orbbec_astra_pro.urdf.xacro" />
+
+  <!--Constant parameters-->
+  <xacro:property name="head_friction" value="1.0" />
+  <xacro:property name="head_damping"  value="0.5" />
+  <xacro:property name="head_max_vel"  value="3.0" />
+  <xacro:property name="head_eps"      value="0.02" />
+
+  <!--************************-->
+  <!--     HEAD_1 (TILT)      -->
+  <!--************************-->
+  <xacro:macro name="tor_head_1" params="name parent">
+    <link name="${name}_link">
+      <inertial>
+        <origin xyz="0.00120 0.00145 0.02165" rpy="0.00000 0.00000 0.00000"/>
+        <mass value="0.65988"/>
+        <inertia ixx="0.00122100000" ixy="-0.00000400000" ixz="0.00007000000"
+                 iyy="0.00092400000" iyz="-0.00004100000"
+                 izz="0.00103300000"/>
+      </inertial>
+
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://tor_description/meshes/head/head_1.stl" scale="1 1 1"/>
+        </geometry>
+        <material name="LightGrey" />
+      </visual>
+
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://tor_description/meshes/head/head_1_collision.stl" scale="1 1 1"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="${name}_joint" type="revolute">
+      <parent link="${parent}"/>
+      <child link="${name}_link"/>
+      <origin xyz="0.00000 0.00000 0.31600" 
+              rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
+      <axis xyz="0 1 0" />
+      <limit lower="${-15.00000 * deg_to_rad}" upper="${45.00000 * deg_to_rad}" effort="1.0" velocity="1.0" />
+      <dynamics damping="${head_damping}" friction="${head_friction}"/>
+       <!-- <safety_controller k_position="20"
+                          k_velocity="20"
+                          soft_lower_limit="${-15.00000 * deg_to_rad + eps_radians}"
+                          soft_upper_limit="${45.00000 * deg_to_rad - eps_radians}" /> -->
+    </joint>
+
+    <gazebo reference="${name}_link">
+      <mu1>0.9</mu1>
+      <mu2>0.9</mu2>
+    </gazebo>
+
+    <gazebo reference="${name}_joint">
+       <implicitSpringDamper>1</implicitSpringDamper>
+    </gazebo>
+  </xacro:macro>
+
+  <!--************************-->
+  <!--      HEAD_2 (PAN)      -->
+  <!--************************-->
+  <xacro:macro name="tor_head_2" params="name parent">
+    <link name="${name}_link">
+      <inertial>
+        <origin xyz="-0.01036 -0.00037 0.13778" rpy="0.00000 0.00000 0.00000"/>
+        <mass value="1.40353"/>
+        <inertia ixx="0.00722500000" ixy="-0.00002500000" ixz="0.00032900000"
+                 iyy="0.00687300000" iyz="0.00004400000"
+                 izz="0.00437300000"/>
+      </inertial>
+
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://tor_description/meshes/head/head_2.stl" scale="1 1 1"/>
+        </geometry>
+        <material name="LightGrey" />
+      </visual>
+
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://tor_description/meshes/head/head_2_collision.stl" scale="1 1 1"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="${name}_joint" type="revolute">
+      <parent link="${parent}"/>
+      <child link="${name}_link"/>
+      <origin xyz="0.00000 0.00000 0.00000" 
+              rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
+      <axis xyz="0 0 1" />
+      <limit lower="${-75.00000 * deg_to_rad}" upper="${75.00000 * deg_to_rad}" effort="1.0" velocity="1.0" />
+      <dynamics damping="${head_damping}" friction="${head_friction}"/>
+       <!-- <safety_controller k_position="20"
+                          k_velocity="20"
+                          soft_lower_limit="${-75.00000 * deg_to_rad + eps_radians}"
+                          soft_upper_limit="${75.00000 * deg_to_rad - eps_radians}" /> -->
+    </joint>
+
+    <gazebo reference="${name}_link">
+      <mu1>0.9</mu1>
+      <mu2>0.9</mu2>
+    </gazebo>
+
+    <gazebo reference="${name}_joint">
+       <implicitSpringDamper>1</implicitSpringDamper>
+    </gazebo>
+
+
+  </xacro:macro>
+
+  <xacro:macro name="tor_head" params="name parent">
+    <xacro:tor_head_1 name="${name}_1" parent="${parent}" />
+    <xacro:tor_head_2 name="${name}_2" parent="${name}_1_link"/>
+    <xacro:xtion_pro_live name="rgbd" parent="${name}_2">
+      <!-- Pose of sensor frame wrt to base -->
+      <origin xyz="0.066 0.0 0.1982" rpy="0 0 0"/>
+      <!-- Pose of optical frame wrt to sensor -->
+      <origin xyz="0 0 0" rpy="${-90 * deg_to_rad} 0 ${-90 * deg_to_rad}"/>
+    </xacro:xtion_pro_live>
+    <xacro:tor_head_differential_transmission name="${name}" number1="1" number2="2"
+                                                act_reduction1="1.0" act_reduction2="1.0"
+                                                jnt_reduction1="1.0" jnt_reduction2="1.0" />
+  </xacro:macro>
+
+</robot>

tor_description/urdf/leg/leg_v2.urdf.xacro

@@ -43,7 +43,7 @@
     <joint name="leg_${prefix}_1_joint" type="revolute">
       <parent link="base_link"/>
       <child link="leg_${prefix}_1_link"/>
-      <origin xyz="0.00000 ${reflect*0.08500} -0.14200" 
+      <origin xyz="-0.02 ${reflect*0.08500} -0.27105" 
               rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
       <axis xyz="0 0 1" />
      <xacro:if value="${reflect == 1}">

tor_description/urdf/sensors/orbbec_astra_pro.urdf.xacro

+<?xml version="1.0"?>
+<!--
+
+  Copyright (c) 2013, PAL Robotics, S.L.
+  All rights reserved.
+
+  This work is licensed under the Creative Commons Attribution-NonCommercial-NoDerivs 3.0 Unported License.
+  To view a copy of this license, visit http://creativecommons.org/licenses/by-nc-nd/3.0/ or send a letter to
+  Creative Commons, 444 Castro Street, Suite 900, Mountain View, California, 94041, USA.
+
+  ASUS Xtion Pro Live
+    - visual model, without base
+    - collision geometry
+    - TF frames
+    - loads Gazebo plugin
+
+    Note: There is a bug that Gazebo in ROS Fuerte ignores the roll/pitch/yaw in the visual tag,
+          therefore it needs to be in a separate link, with the orientation specified at the joint.
+
+    ToDo: Fix visual mesh, re-scale it, & the origin is not centered along the frontal axis.
+          Can simplify collision mesh.
+          Rotate origin in mesh, to match the orientation of the base link and bypass the bug.
+
+-->
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:include filename="$(find tor_description)/urdf/sensors/xtion_pro_live.gazebo.xacro" />
+
+  <!-- Macro -->
+  <xacro:macro name="xtion_pro_live" params="name parent *origin *optical_origin">
+
+    <!-- frames in the center of the camera -->
+    <joint name="${name}_joint" type="fixed">
+      <insert_block name="origin"/>
+      <axis xyz="0 0 1"/>
+      <!-- <limit lower="0" upper="0.001" effort="100" velocity="0.01"/> -->
+      <parent link="${parent}_link"/>
+      <child link="${name}_link"/>
+    </joint>
+
+    <link name="${name}_link">
+      <inertial>
+        <mass value="0.01" />
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <!-- inertia tensor computed analytically for a solid cuboid -->
+        <inertia ixx="0.000030"  ixy="0.0"  ixz="0.0"
+                 iyy="0.000030"  iyz="0.0"
+                 izz="0.000002" />
+      </inertial>
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="package://tor_description/meshes/sensors/xtion_pro_live/xtion_pro_live.dae" />
+        </geometry>
+        <material name="Grey">
+          <color rgba="0.5 0.5 0.5 1"/>
+        </material>
+      </visual>
+      <collision>
+        <origin xyz="-0.01 0.0025 0" rpy="0 0 0"/>
+        <geometry>
+          <box size="0.04 0.185 0.03"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="${name}_optical_joint" type="fixed">
+      <insert_block name="optical_origin" />
+      <parent link="${name}_link"/>
+      <child link="${name}_optical_frame"/>
+    </joint>
+
+    <link name="${name}_optical_frame">
+      <inertial>
+        <mass value="0.01" />
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <inertia ixx="0.0"  ixy="0.0"  ixz="0.0"
+                 iyy="0.0"  iyz="0.0"
+                 izz="0.0" />
+      </inertial>
+    </link>
+
+    <!-- frames of the depth sensor -->
+    <joint name="${name}_depth_joint" type="fixed">
+      <origin xyz="0.0 0.03751 0.0" rpy="0 0 0" />
+      <parent link="${name}_link" />
+      <child link="${name}_depth_frame"/>
+    </joint>
+
+    <link name="${name}_depth_frame"/>
+
+    <joint name="${name}_depth_optical_joint" type="fixed">
+      <origin xyz="0 0 0" rpy="${-90.0 * deg_to_rad} 0.0 ${-90.0 * deg_to_rad}" />
+      <parent link="${name}_depth_frame" />
+      <child link="${name}_depth_optical_frame"/>
+    </joint>
+
+    <link name="${name}_depth_optical_frame"/>
+
+    <!-- frames of the rgb sensor -->
+    <joint name="${name}_rgb_joint" type="fixed">
+      <origin xyz="0.0 0.01251 0.0" rpy="0 0 0" />
+      <parent link="${name}_link" />
+      <child link="${name}_rgb_frame"/>
+    </joint>
+
+    <link name="${name}_rgb_frame"/>
+
+    <joint name="${name}_rgb_optical_joint" type="fixed">
+      <origin xyz="0 0 0" rpy="${-90.0 * deg_to_rad} 0.0 ${-90.0 * deg_to_rad}" />
+      <parent link="${name}_rgb_frame" />
+      <child link="${name}_rgb_optical_frame"/>
+    </joint>
+
+    <link name="${name}_rgb_optical_frame"/>
+
+    <!-- extensions -->
+    <xacro:xtion_pro_live_rgbd_camera_gazebo name="${name}" />
+
+  </xacro:macro>
+
+</robot>

tor_description/urdf/sensors/xtion_pro_live.gazebo.xacro

+<?xml version="1.0"?>
+<!--
+
+  Copyright (c) 2013, PAL Robotics, S.L.
+  All rights reserved.
+
+  This work is licensed under the Creative Commons Attribution-NonCommercial-NoDerivs 3.0 Unported License.
+  To view a copy of this license, visit http://creativecommons.org/licenses/by-nc-nd/3.0/ or send a letter to
+  Creative Commons, 444 Castro Street, Suite 900, Mountain View, California, 94041, USA.
+
+  ROS Gazebo plugin for RGBD sensor (OpenNI/Kinect/ASUS)
+   * Publishes MONO8 format "depth" image & PointCloud
+   * Publishes RGB8 format color image & 2nd copy of PointCloud (for registered)
+
+  Configuration according with:
+  http://www.asus.com/Multimedia/Xtion_PRO_LIVE/specifications/
+  and stockbot_laser_sensors/launch/xtion_pro_live.launch, with long range [0.45, 10.0]m
+  instead of [0.8, 3.5]m.
+-->
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:macro name="xtion_pro_live_rgbd_camera_gazebo" params="name">
+    <gazebo reference="${name}_link">
+      <!-- IR + depth -->
+      <sensor type="depth" name="${name}_frame_sensor">
+        <always_on>true</always_on>
+        <update_rate>6.0</update_rate>
+        <camera>
+          <horizontal_fov>${58.0 * deg_to_rad}</horizontal_fov>
+          <image>
+            <format>R8G8B8</format>
+            <width>320</width>
+            <height>240</height>
+          </image>
+          <clip>
+            <near>0.45</near>
+            <far>10.0</far>
+          </clip>
+        </camera>
+        <plugin name="${name}_frame_controller" filename="libgazebo_ros_openni_kinect.so">
+          <alwaysOn>true</alwaysOn>
+          <updateRate>6.0</updateRate>
+          <cameraName>${name}</cameraName>
+          <imageTopicName>ir/image_raw</imageTopicName>
+          <cameraInfoTopicName>ir/camera_info</cameraInfoTopicName>
+          <depthImageTopicName>depth/image_raw</depthImageTopicName>
+          <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
+          <pointCloudTopicName>depth/points</pointCloudTopicName>
+          <frameName>${name}_optical_frame</frameName>
+          <pointCloudCutoff>0.45</pointCloudCutoff>
+          <rangeMax>10.0</rangeMax>
+          <distortionK1>0.00000001</distortionK1>
+          <distortionK2>0.00000001</distortionK2>
+          <distortionK3>0.00000001</distortionK3>
+          <distortionT1>0.00000001</distortionT1>
+          <distortionT2>0.00000001</distortionT2>
+        </plugin>
+      </sensor>
+
+      <!-- RGB -->
+      <sensor type="depth" name="${name}_frame_sensor">
+        <always_on>true</always_on>
+        <update_rate>6.0</update_rate>
+        <camera>
+          <horizontal_fov>${58.0 * deg_to_rad}</horizontal_fov>
+          <image>
+            <format>R8G8B8</format>
+            <width>320</width>
+            <height>240</height>
+          </image>
+          <clip>
+            <near>0.45</near>
+            <far>10.0</far>
+          </clip>
+        </camera>
+        <plugin name="${name}_frame_controller" filename="libgazebo_ros_openni_kinect.so">
+          <alwaysOn>true</alwaysOn>
+          <updateRate>6.0</updateRate>
+          <cameraName>${name}</cameraName>
+          <imageTopicName>rgb/image_raw</imageTopicName>
+          <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
+          <pointCloudTopicName>rgb/points</pointCloudTopicName>
+          <frameName>${name}_optical_frame</frameName>
+          <pointCloudCutoff>0.45</pointCloudCutoff>
+          <rangeMax>10.0</rangeMax>
+          <distortionK1>0.00000001</distortionK1>
+          <distortionK2>0.00000001</distortionK2>
+          <distortionK3>0.00000001</distortionK3>
+          <distortionT1>0.00000001</distortionT1>
+          <distortionT2>0.00000001</distortionT2>
+        </plugin>
+      </sensor>
+    </gazebo>
+  </xacro:macro>
+</robot>

tor_description/urdf/torso/torso.urdf.xacro

@@ -22,7 +22,7 @@
 
   <xacro:macro name="tor_torso" params="name">
     <!--************************-->
-    <!--        TORSO_2         -->
+    <!--        TORSO_2  (TILT) -->
     <!--************************-->
     <link name="${name}_2_link">
       <inertial>
@@ -51,7 +51,7 @@
 
 
     <!--************************-->
-    <!--        TORSO_1         -->
+    <!--        TORSO_1  (PAN)  -->
     <!--************************-->
     <link name="${name}_1_link">
       <inertial>
@@ -97,17 +97,17 @@
     <!--************************-->
     <link name="base_link">
       <inertial>
-        <origin xyz="-0.06071 0.00861 0.06368" rpy="0.00000 0.00000 0.00000"/>
-        <mass value="13.34865"/>
-        <inertia ixx="0.06652900000" ixy="-0.00035700000" ixz="0.00027600000"
-                 iyy="0.03746500000" iyz="-0.00125400000"
-                 izz="0.07895100000"/>
+        <origin xyz="-0.08222 0.00838 -0.07261" rpy="0.00000 0.00000 0.00000"/>
+        <mass value="13.53810"/>
+        <inertia ixx="0.06989000000" ixy="-0.00011700000" ixz="0.00023000000"
+                 iyy="0.03998200000" iyz="-0.00132500000"
+                 izz="0.08234500000"/>
       </inertial>
 
       <visual>
         <origin xyz="0 0 0" rpy="0 0 0" />
         <geometry>
-          <mesh filename="package://tor_description/meshes/torso/base_link_2.STL" scale="1 1 1"/>
+          <mesh filename="package://tor_description/meshes/torso/base_link.STL" scale="1 1 1"/>
         </geometry>
         <material name="LightGrey" />
       </visual>
@@ -123,7 +123,7 @@
     <joint name="${name}_2_joint" type="revolute">
       <parent link="${name}_1_link"/>
       <child link="base_link"/>
-      <origin xyz="0.0 0.0 -0.2012" 
+      <origin xyz="0.0 0.0 -0.0722" 
               rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
       <axis xyz="0 0 1" />
       <limit lower="${-75.00000 * deg_to_rad}" upper="${75.00000 * deg_to_rad}"