diff --git a/tor_description/meshes/arm/arm_7.STL b/tor_description/meshes/arm/arm_7.STL
index a53b22fd80c4cd3d98e33f2fcea9899b7e9af4e6..7007580faa8f17184b8f1374e1528dc27462e4e7 100644
Binary files a/tor_description/meshes/arm/arm_7.STL and b/tor_description/meshes/arm/arm_7.STL differ
diff --git a/tor_description/meshes/arm/arm_7_collision.STL b/tor_description/meshes/arm/arm_7_collision.STL
index 945e0d1c4c6b533e982b91d6b4b413fb653b4eb3..5ecbddec4284ddb8c82a9573ffe918eac9400fa8 100644
Binary files a/tor_description/meshes/arm/arm_7_collision.STL and b/tor_description/meshes/arm/arm_7_collision.STL differ
diff --git a/tor_description/meshes/gripper/base_link.STL b/tor_description/meshes/gripper/base_link.STL
new file mode 100644
index 0000000000000000000000000000000000000000..1a966f0ef8a513e3546a6efcb51ac52cba166a26
Binary files /dev/null and b/tor_description/meshes/gripper/base_link.STL differ
diff --git a/tor_description/meshes/gripper/base_link_collision.STL b/tor_description/meshes/gripper/base_link_collision.STL
new file mode 100644
index 0000000000000000000000000000000000000000..84b58eeab8151e9cb308b0a9810ea7c57a6f5dea
Binary files /dev/null and b/tor_description/meshes/gripper/base_link_collision.STL differ
diff --git a/tor_description/meshes/gripper/fingertip.STL b/tor_description/meshes/gripper/fingertip.STL
new file mode 100644
index 0000000000000000000000000000000000000000..a2117817fbb1d66b0c889f10c929da014e3cedc5
Binary files /dev/null and b/tor_description/meshes/gripper/fingertip.STL differ
diff --git a/tor_description/meshes/gripper/fingertip_collision.STL b/tor_description/meshes/gripper/fingertip_collision.STL
new file mode 100644
index 0000000000000000000000000000000000000000..dc3ae5545e9ee749f0c906f2b34ced55b909e170
Binary files /dev/null and b/tor_description/meshes/gripper/fingertip_collision.STL differ
diff --git a/tor_description/meshes/gripper/gripper_motor_double.STL b/tor_description/meshes/gripper/gripper_motor_double.STL
new file mode 100644
index 0000000000000000000000000000000000000000..a3725950575e7e9c7e8254bfd46f0cc4d063fef8
Binary files /dev/null and b/tor_description/meshes/gripper/gripper_motor_double.STL differ
diff --git a/tor_description/meshes/gripper/gripper_motor_double_collision.STL b/tor_description/meshes/gripper/gripper_motor_double_collision.STL
new file mode 100644
index 0000000000000000000000000000000000000000..ae635b1a3554354124d71edc852c4dac21519304
Binary files /dev/null and b/tor_description/meshes/gripper/gripper_motor_double_collision.STL differ
diff --git a/tor_description/meshes/gripper/gripper.STL b/tor_description/meshes/gripper/gripper_simple.STL
similarity index 100%
rename from tor_description/meshes/gripper/gripper.STL
rename to tor_description/meshes/gripper/gripper_simple.STL
diff --git a/tor_description/meshes/gripper/gripper_collision.STL b/tor_description/meshes/gripper/gripper_simple_collision.STL
similarity index 100%
rename from tor_description/meshes/gripper/gripper_collision.STL
rename to tor_description/meshes/gripper/gripper_simple_collision.STL
diff --git a/tor_description/meshes/gripper/gripper_tweezers.STL b/tor_description/meshes/gripper/gripper_tweezers.STL
new file mode 100644
index 0000000000000000000000000000000000000000..5f6780a05702855efebdec7e06dae8129304d578
Binary files /dev/null and b/tor_description/meshes/gripper/gripper_tweezers.STL differ
diff --git a/tor_description/meshes/gripper/gripper_tweezers_collision.STL b/tor_description/meshes/gripper/gripper_tweezers_collision.STL
new file mode 100644
index 0000000000000000000000000000000000000000..a51b53796f5e0028998d92e8f142f4d1dd2a9cda
Binary files /dev/null and b/tor_description/meshes/gripper/gripper_tweezers_collision.STL differ
diff --git a/tor_description/meshes/gripper/inner_double.STL b/tor_description/meshes/gripper/inner_double.STL
new file mode 100644
index 0000000000000000000000000000000000000000..6193f7dfdb0f4c1823b25f0508c220a0e8f524b5
Binary files /dev/null and b/tor_description/meshes/gripper/inner_double.STL differ
diff --git a/tor_description/meshes/gripper/inner_double_collision.STL b/tor_description/meshes/gripper/inner_double_collision.STL
new file mode 100644
index 0000000000000000000000000000000000000000..dd8635848a7f058c92271cc7828af27aef680e78
Binary files /dev/null and b/tor_description/meshes/gripper/inner_double_collision.STL differ
diff --git a/tor_description/meshes/sensors/orbbec/orbbec.STL b/tor_description/meshes/sensors/orbbec/orbbec.STL
new file mode 100644
index 0000000000000000000000000000000000000000..3cc1c8a6596ea87bad3a2356d414747ec9d6d189
Binary files /dev/null and b/tor_description/meshes/sensors/orbbec/orbbec.STL differ
diff --git a/tor_description/robots/tor_full.urdf.xacro b/tor_description/robots/tor_full.urdf.xacro
index 031461f2cb381eace1f3ba2e25883dfc24b6f09c..b0d97ea39eea26c15d673802bd18ba4c0b919050 100644
--- a/tor_description/robots/tor_full.urdf.xacro
+++ b/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" />
diff --git a/tor_description/urdf/arm/arm.urdf.xacro b/tor_description/urdf/arm/arm.urdf.xacro
index d42d279b16abf4c1590b706227ef91ddbe27d532..967b3dc749573549483114b8a25077f46c5dfd1f 100644
--- a/tor_description/urdf/arm/arm.urdf.xacro
+++ b/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" />
diff --git a/tor_description/urdf/gripper/gripper.urdf.xacro b/tor_description/urdf/gripper/gripper.urdf.xacro
index 098abe05f4df725b3737ac2f9c676b8b26fad74d..965ec54913df693fe5a341a8e4437880ecd8dfda 100644
--- a/tor_description/urdf/gripper/gripper.urdf.xacro
+++ b/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>
diff --git a/tor_description/urdf/gripper/gripper_v2.urdf.xacro b/tor_description/urdf/gripper/gripper_v2.urdf.xacro
new file mode 100644
index 0000000000000000000000000000000000000000..adf75754af358b81f47415f6641c653e056313c5
--- /dev/null
+++ b/tor_description/urdf/gripper/gripper_v2.urdf.xacro
@@ -0,0 +1,123 @@
+<?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>
diff --git a/tor_description/urdf/sensors/ftsensor.urdf.xacro b/tor_description/urdf/sensors/ftsensor.urdf.xacro
index cba9ab5cab4fad9ad8a6dec86123145d71fc427d..fa9625bdc7858c66f3722ceb40a8325b588d8cfb 100644
--- a/tor_description/urdf/sensors/ftsensor.urdf.xacro
+++ b/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>
diff --git a/tor_description/urdf/sensors/orbbec_astra_pro.urdf.xacro b/tor_description/urdf/sensors/orbbec_astra_pro.urdf.xacro
index efa4ecbefa079bf47a3fb19228cf08ead15fdf10..a85bb9a0ece46429cdcedec171a898bf5b5b8f4d 100644
--- a/tor_description/urdf/sensors/orbbec_astra_pro.urdf.xacro
+++ b/tor_description/urdf/sensors/orbbec_astra_pro.urdf.xacro
@@ -50,9 +50,9 @@
<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" />
+ <mesh filename="package://tor_description/meshes/sensors/orbbec/orbbec.STL" />
</geometry>
- <material name="Grey">
+ <material name="DarkGrey">
<color rgba="0.5 0.5 0.5 1"/>
</material>
</visual>