Merge remote-tracking branch 'origin/fix_right_wrist_and_gripper' into dubnium-devel

talos_description/urdf/arm/wrist.urdf.xacro

@@ -44,14 +44,14 @@
       <visual>
         <origin xyz="0 0 0" rpy="0 0 0" />
         <geometry>
-          <mesh filename="package://talos_description/meshes/arm/arm_5.STL" scale="1 1 1"/>
+          <mesh filename="package://talos_description/meshes/arm/arm_5.STL" scale="1 ${reflect} 1"/>
         </geometry>
         <material name="LightGrey" />
       </visual>
       <collision>
         <origin xyz="0 0 0" rpy="0 0 0" />
         <geometry>
-          <mesh filename="package://talos_description/meshes/arm/arm_5_collision.STL" scale="1 1 1"/>
+          <mesh filename="package://talos_description/meshes/arm/arm_5_collision.STL" scale="1 ${reflect} 1"/>
         </geometry>
       </collision>
     </link>
@@ -59,14 +59,8 @@
     <joint name="${name}_${side}_5_joint" type="revolute">
       <parent link="${parent}" />
       <child link="${name}_${side}_5_link" />
-      <xacro:if value="${reflect == 1}">
-        <origin xyz="-0.02000 0.00000 -0.26430" 
-              rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
-      </xacro:if>
-      <xacro:if value="${reflect == -1}">
-        <origin xyz="-0.02000 0.00000 -0.26430" 
-              rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${180.00000 * deg_to_rad}"/>
-      </xacro:if>
+      <origin xyz="-0.02000 0.00000 -0.26430" 
+                rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
       <axis xyz="0 0 1" />
       <limit lower="${-145.0 * deg_to_rad}" upper="${145.0 * deg_to_rad}" effort="${wrist_1_max_effort}" velocity="${wrist_1_max_vel}" />
       <dynamics friction="${wrist_friction}" damping="${wrist_damping}"/>
@@ -140,7 +134,7 @@
       <visual>
         <origin xyz="0 0 0" rpy="0 0 0" />
         <geometry>
-          <mesh filename="package://talos_description/meshes/arm/arm_7.STL" scale="1 1 1"/>
+          <mesh filename="package://talos_description/meshes/arm/arm_7.STL" scale="1 ${reflect} 1"/>
         </geometry>
         <material name="DarkGrey" />
       </visual>
@@ -148,7 +142,7 @@
       <collision>
         <origin xyz="0 0 0" rpy="0 0 0" />
         <geometry>
-          <mesh filename="package://talos_description/meshes/arm/arm_7_collision.STL" scale="1 1 1"/>
+          <mesh filename="package://talos_description/meshes/arm/arm_7_collision.STL" scale="1 ${reflect} 1"/>
         </geometry>
       </collision>
     </link>

talos_description/urdf/gripper/gripper_v2.urdf.xacro

@@ -39,7 +39,7 @@
               <parent link="${parent}"/>
               <child link="${name}_base_link"/>
               <origin xyz="0.00000 0.00000 -0.02875" 
-                      rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${(reflect -1)* 90.000000 * deg_to_rad}"/>
+                      rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
               <axis xyz="0 0 0" />
             </joint>
             

talos_description/urdf/torso/torso.urdf.xacro

@@ -77,13 +77,13 @@
     </link>
 
     <joint name="${name}_1_joint" type="revolute">
-      <parent link="${name}_2_link"/>
+      <parent link="base_link"/>
       <child link="${name}_1_link"/>
-      <origin xyz="0.0 0.0 0.0" 
+      <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 1 0" />
-      <limit lower="${-45.0 * deg_to_rad}" upper="${15.0 * deg_to_rad}" 
-             effort="${torso_max_effort}" velocity="${torso_max_vel}"/>
+      <axis xyz="0 0 1" />
+      <limit lower="${-75.00000 * deg_to_rad}" upper="${75.00000 * deg_to_rad}" 
+             effort="${torso_max_effort}" velocity="${torso_max_vel}" />
       <dynamics damping="1.0" friction="1.0"/>
 
       <!-- <safety_controller k_position="20"
@@ -122,12 +122,12 @@
 
     <joint name="${name}_2_joint" type="revolute">
       <parent link="${name}_1_link"/>
-      <child link="base_link"/>
-      <origin xyz="0.0 0.0 -0.0722" 
+      <child link="${name}_2_link"/>
+      <origin xyz="0.0 0.0 0.0" 
               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="${torso_max_effort}" velocity="${torso_max_vel}" />
+      <axis xyz="0 1 0" />
+      <limit lower="${-15.0 * deg_to_rad}" upper="${45.0 * deg_to_rad}" 
+             effort="${torso_max_effort}" velocity="${torso_max_vel}"/>
       <dynamics friction="1.0" damping="1.0"/>
 
       <!-- <safety_controller k_position="20"