Merge branch 'full_tor' of gitlab:robots/tor_robot into full_tor

tor_description/robots/tor_full.urdf.xacro

@@ -16,6 +16,7 @@
   <xacro:include filename="$(find tor_description)/urdf/torso/torso.urdf.xacro" />
   <xacro:include filename="$(find tor_description)/urdf/head/head.urdf.xacro" />
   <xacro:include filename="$(find tor_description)/urdf/arm/arm.urdf.xacro" />
+  <xacro:include filename="$(find tor_description)/urdf/sensors/ftsensor.urdf.xacro" />
   <xacro:include filename="$(find tor_description)/urdf/gripper/gripper.urdf.xacro" />
   <xacro:include filename="$(find tor_description)/urdf/leg/leg_v2.urdf.xacro" />
 
@@ -26,8 +27,11 @@
   <xacro:tor_arm name="arm" parent="torso_2_link" side="left" reflect="1"/>
   <xacro:tor_arm name="arm" parent="torso_2_link" side="right" reflect="-1"/>
 
-  <xacro:tor_gripper name="left_gripper" parent="arm_left_7_link"/>
-  <xacro:tor_gripper name="right_gripper" parent="arm_right_7_link"/>
+  <xacro:ft_sensor name="wrist" parent="arm_right_7_link" side="right" reflect="1"  />
+  <xacro:ft_sensor name="wrist" parent="arm_left_7_link"  side="left"  reflect="-1" />
+
+  <xacro:tor_gripper name="left_gripper" parent="wrist_left_ft_tool_link"/>
+  <xacro:tor_gripper name="right_gripper" parent="wrist_right_ft_tool_link"/>
 
   <xacro:tor_leg prefix="left"  reflect="1" />
   <xacro:tor_leg prefix="right" reflect="-1" />

tor_description/urdf/arm/arm.urdf.xacro

@@ -241,7 +241,7 @@
     <!--***********************-->
     <!--        TOOL           -->
     <!--***********************-->
-    <link name="${name}_${side}_tool_link">
+<!--     <link name="${name}_${side}_tool_link">
       <inertial>
         <mass value="0.1" />
         <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0" />
@@ -264,10 +264,14 @@
     <joint name="${name}_${side}_tool_joint" type="fixed">
       <parent link="${name}_${side}_7_link" />
       <child link="${name}_${side}_tool_link" />
-      <origin xyz="0 0 ${reflect * 0.046}" rpy="${90.0 * deg_to_rad} ${-90.0 * reflect * deg_to_rad} 0" />
+      <xacro:if value="${reflect == 1}">
+        <origin xyz="0 0 -0.051" rpy="0 ${90.0 * deg_to_rad} 0" />
+      </xacro:if>
+      <xacro:if value="${reflect == -1}">
+        <origin xyz="0 0 -0.051" rpy="${180.0 * deg_to_rad} ${90.0 * deg_to_rad} ${0.0 * deg_to_rad}" />
+      </xacro:if>
     </joint>
-
-
+ -->
     <!-- extensions -->
     <xacro:tor_arm_simple_transmission name="${name}" side="${side}" number="1" reduction="1.0" />
     <xacro:tor_arm_simple_transmission name="${name}" side="${side}" number="2" reduction="1.0" />

tor_description/urdf/gripper/gripper.urdf.xacro

@@ -10,7 +10,7 @@
 
   <xacro:macro name="tor_gripper" params="name parent">
 
-  <link name="${name}_link">
+  <link name="${name}_base_link">
     <inertial>
       <origin xyz="0.01507 -0.01177 -0.02788" rpy="0.00000 0.00000 0.00000"/>
       <mass value="0.66262"/>
@@ -21,27 +21,56 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0" />
       <geometry>
-        <mesh filename="package://tor_description/meshes/gripper/gripper.STL"/>
+        <mesh filename="package://tor_description/meshes/gripper/base_link.STL"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0" />
+      <geometry>
+        <mesh filename="package://tor_description/meshes/gripper/base_link_collision.STL"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="${name}_base_joint" type="fixed">
+    <parent link="${parent}" />
+    <child link="${name}_base_link" />
+    <axis xyz="0 0 0" />
+    <origin xyz="0 0 0" rpy="0 0 0" />
+  </joint>
+
+  <link name="${name}_link">
+    <inertial>
+      <origin xyz="0.01507 -0.01177 -0.02788" rpy="0.00000 0.00000 0.00000"/>
+      <mass value="0.3"/>
+      <inertia ixx="0.00195600000" ixy="-0.00007300000" ixz="0.00026500000"
+               iyy="0.00139500000" iyz="-0.00015700000"
+               izz="0.00170400000"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0" />
+      <geometry>
+        <mesh filename="package://tor_description/meshes/gripper/gripper_simple.STL"/>
       </geometry>
       <material name="Orange"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0" />
       <geometry>
-        <mesh filename="package://tor_description/meshes/gripper/gripper_collision.STL"/>
+        <mesh filename="package://tor_description/meshes/gripper/gripper_simple_collision.STL"/>
       </geometry>
     </collision>
   </link>
 
   <joint name="${name}_joint" type="fixed">
-    <parent link="${parent}" />
+    <parent link="${name}_base_link" />
     <child link="${name}_link" />
     <axis xyz="0 0 0" />
-    <origin xyz="0 0.02 -0.10975" rpy="0 0 0" />
+    <origin xyz="0 0.02025 -0.02875" rpy="0 0 0" />
   </joint>
 
 
-
  </xacro:macro>
 
 </robot>

tor_description/urdf/gripper/gripper_v2.urdf.xacro

+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <!--File includes-->
+  <!-- <xacro:include filename="$(find tor_description)/urdf/deg_to_rad.urdf.xacro" />
+  <xacro:include filename="$(find tor_description)/urdf/gripper.transmission.xacro" />
+  <xacro:include filename="$(find tor_description)/urdf/gripper.gazebo.xacro" /> -->
+
+
+  <xacro:macro name="tor_gripper" params="name parent">
+
+           <link name="${name}_base_link">
+              <inertial>
+                <origin xyz="-0.00534 0.00362 -0.02357" rpy="0.00000 0.00000 0.00000"/>
+                <mass value="0.61585"/>
+                <inertia ixx="0.00047200000" ixy="-0.00000800000" ixz="0.00003500000"
+                         iyy="0.00052100000" iyz="0.00000000000"
+                         izz="0.00069000000"/>
+              </inertial>
+
+              <visual>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                  <mesh filename="package://tor_description/meshes/gripper/base_link.STL" scale="1 1 1"/>
+                </geometry>
+              </visual>
+
+              <collision>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                  <mesh filename="package://tor_description/meshes/gripper/base_link_collision.STL" scale="1 1 1"/>
+                </geometry>
+              </collision>
+            </link>
+
+            <joint name="${name}_base_link_joint" type="fixed">
+              <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} ${90.00000 * deg_to_rad}"/>
+              <axis xyz="0 0 0" />
+            </joint>
+
+            <!-- Joint gripper_motor_double (None) : child link -- -> parent link gripper_base_link -->
+            
+
+            <link name="${name}_motor_double_link">
+              <inertial>
+                <origin xyz="0.02270 0.01781 -0.00977" rpy="0.00000 0.00000 0.00000"/>
+                <mass value="0.16889"/>
+                <inertia ixx="0.00015900000" ixy="-0.00007000000" ixz="0.00003800000"
+                         iyy="0.00022100000" iyz="-0.00005300000"
+                         izz="0.00026800000"/>
+              </inertial>
+
+              <visual>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                  <mesh filename="package://tor_description/meshes/gripper/gripper_motor_double.STL" scale="1 1 1"/>
+                </geometry>
+              </visual>
+
+              <collision>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                  <mesh filename="package://tor_description/meshes/gripper/gripper_motor_double_collision.STL" scale="1 1 1"/>
+                </geometry>
+              </collision>
+            </link>
+
+            <joint name="${name}_motor_double_joint" type="revolute">
+              <parent link="${name}_base_link"/>
+              <child link="${name}_motor_double_link"/>
+              <origin xyz="0.00000 0.02025 -0.03000" 
+                      rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
+              <!-- You may want to change axis -->
+              <axis xyz="0 0 1" />
+              <limit lower="${0.00000 * deg_to_rad}" upper="${60.00000 * deg_to_rad}" effort="1.0" velocity="1.0" />
+              <dynamics friction="1.0" damping="1.0"/>
+            </joint>
+
+
+            <link name="${name}_inner_double_link">
+              <inertial>
+                <origin xyz="-0.00056 0.03358 -0.01741" rpy="0.00000 0.00000 0.00000"/>
+                <mass value="0.11922"/>
+                <inertia ixx="0.00009100000" ixy="0.00000100000" ixz="-0.00000100000"
+                         iyy="0.00015600000" iyz="-0.00003200000"
+                         izz="0.00012600000"/>
+              </inertial>
+
+              <visual>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                  <mesh filename="package://tor_description/meshes/gripper/inner_double.STL" scale="1 1 1"/>
+                </geometry>
+              </visual>
+
+              <collision>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                  <mesh filename="package://tor_description/meshes/gripper/inner_double_collision.STL" scale="1 1 1"/>
+                </geometry>
+              </collision>
+            </link>
+
+            <joint name="${name}_inner_double_joint" type="revolute">
+              <parent link="${name}_base_link"/>
+              <child link="${name}_inner_double_link"/>
+              <origin xyz="0.00000 0.00525 -0.05598" 
+                      rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
+              <!-- You may want to change axis -->
+              <axis xyz="0 0 1" />
+              <limit lower="${0.00000 * deg_to_rad}" upper="${0.00000 * deg_to_rad}" effort="1.0" velocity="1.0" />
+              <mimic joint="${name}_motor_double_joint" multiplier="${1.0}" offset="0.0" />
+
+            </joint>
+
+
+ </xacro:macro>
+
+</robot>

tor_description/urdf/sensors/ftsensor.urdf.xacro

@@ -38,7 +38,7 @@
     <joint name="${name}_${side}_ft_joint" type="fixed">
       <parent link="${parent}" />
       <child link="${name}_${side}_ft_link" />
-      <origin xyz="${0.0157*0.5} 0 0" rpy="${90.0 * deg_to_rad} 0 ${90.0 * deg_to_rad}" />
+      <origin xyz="0 0 -0.051" rpy="${0.0 * deg_to_rad} 0 ${-90.0 * deg_to_rad}" />
     </joint>
 
     <!--***********************-->
@@ -50,14 +50,14 @@
         <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0" />
       </inertial>
       <visual>
-        <origin xyz="0.0 0 0" rpy="0 ${90.0 * deg_to_rad} 0" />
+        <origin xyz="0.0 0 0" rpy="0 ${0.0 * deg_to_rad} 0" />
         <geometry>
           <cylinder radius="${0.05*0.5}" length="0.00975"/>
         </geometry>
         <material name="FlatBlack" />
       </visual>
       <collision>
-        <origin xyz="0.0 0 0" rpy="0 ${90.0 * deg_to_rad} 0" />
+        <origin xyz="0.0 0 0" rpy="0 ${0.0 * deg_to_rad} 0" />
         <geometry>
           <cylinder radius="${0.05*0.5}" length="0.00975"/>
         </geometry>
@@ -67,7 +67,8 @@
     <joint name="${name}_${side}_tool_joint" type="fixed">
       <parent link="${name}_${side}_ft_link" />
       <child link="${name}_${side}_ft_tool_link" />
-      <origin xyz="0 0 ${0.0157*0.5 + 0.00975*0.5}" rpy="${-90.0 * deg_to_rad} ${-90.0 * deg_to_rad} 0" />
+      <origin xyz="0 0 ${-1.0 * (0.0157*0.5 + 0.00975*0.5)}" rpy="0 0 ${-90.0 * reflect * deg_to_rad}" />
+
     </joint>
 
 </xacro:macro>