diff --git a/tor_description/meshes/arm/arm_1.STL b/tor_description/meshes/arm/arm_1.STL
new file mode 100644
index 0000000000000000000000000000000000000000..cefd0c8069c2dff48ad5f8213ba88c468602c937
Binary files /dev/null and b/tor_description/meshes/arm/arm_1.STL differ
diff --git a/tor_description/meshes/arm/arm_1_collision.STL b/tor_description/meshes/arm/arm_1_collision.STL
new file mode 100644
index 0000000000000000000000000000000000000000..dfc4f0caca4445f062a4fb30e70208825592c499
Binary files /dev/null and b/tor_description/meshes/arm/arm_1_collision.STL differ
diff --git a/tor_description/meshes/arm/arm_2.STL b/tor_description/meshes/arm/arm_2.STL
new file mode 100644
index 0000000000000000000000000000000000000000..5fae3cd5f3644d1411a2651e05bc5dbef0014413
Binary files /dev/null and b/tor_description/meshes/arm/arm_2.STL differ
diff --git a/tor_description/meshes/arm/arm_2_collision.STL b/tor_description/meshes/arm/arm_2_collision.STL
new file mode 100644
index 0000000000000000000000000000000000000000..5e25baabe3429017fcb0dec6e7a23170425a3623
Binary files /dev/null and b/tor_description/meshes/arm/arm_2_collision.STL differ
diff --git a/tor_description/meshes/arm/arm_3.STL b/tor_description/meshes/arm/arm_3.STL
new file mode 100644
index 0000000000000000000000000000000000000000..a820970cc69181066f384e143c197659b610f76c
Binary files /dev/null and b/tor_description/meshes/arm/arm_3.STL differ
diff --git a/tor_description/meshes/arm/arm_3_collision.STL b/tor_description/meshes/arm/arm_3_collision.STL
new file mode 100644
index 0000000000000000000000000000000000000000..e979eca31bf22c75f6570fe8e196b1c9ee69267b
Binary files /dev/null and b/tor_description/meshes/arm/arm_3_collision.STL differ
diff --git a/tor_description/meshes/arm/arm_4.STL b/tor_description/meshes/arm/arm_4.STL
new file mode 100644
index 0000000000000000000000000000000000000000..3cda3f75c158007ea1526679f9f3a1d036eb77ad
Binary files /dev/null and b/tor_description/meshes/arm/arm_4.STL differ
diff --git a/tor_description/meshes/arm/arm_4_collision.STL b/tor_description/meshes/arm/arm_4_collision.STL
new file mode 100644
index 0000000000000000000000000000000000000000..ed30028a8751ea59cd32a5a3e6118bd6f605d3ce
Binary files /dev/null and b/tor_description/meshes/arm/arm_4_collision.STL differ
diff --git a/tor_description/meshes/arm/arm_5.STL b/tor_description/meshes/arm/arm_5.STL
new file mode 100644
index 0000000000000000000000000000000000000000..a592f22c03560db57da2c506a5e664b4a670c06c
Binary files /dev/null and b/tor_description/meshes/arm/arm_5.STL differ
diff --git a/tor_description/meshes/arm/arm_5_collision.STL b/tor_description/meshes/arm/arm_5_collision.STL
new file mode 100644
index 0000000000000000000000000000000000000000..0b64df3b66f3ba5ec3b3c6aa1cc2e4efffe4ae0e
Binary files /dev/null and b/tor_description/meshes/arm/arm_5_collision.STL differ
diff --git a/tor_description/meshes/arm/arm_6.STL b/tor_description/meshes/arm/arm_6.STL
new file mode 100644
index 0000000000000000000000000000000000000000..43ea7b8c75c430f5e1bcd0992d2046435b9948b1
Binary files /dev/null and b/tor_description/meshes/arm/arm_6.STL differ
diff --git a/tor_description/meshes/arm/arm_6_collision.STL b/tor_description/meshes/arm/arm_6_collision.STL
new file mode 100644
index 0000000000000000000000000000000000000000..6a992dde6b109c914af07543bd4110fefa191fb9
Binary files /dev/null and b/tor_description/meshes/arm/arm_6_collision.STL differ
diff --git a/tor_description/meshes/arm/arm_7.STL b/tor_description/meshes/arm/arm_7.STL
new file mode 100644
index 0000000000000000000000000000000000000000..a53b22fd80c4cd3d98e33f2fcea9899b7e9af4e6
Binary files /dev/null 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
new file mode 100644
index 0000000000000000000000000000000000000000..945e0d1c4c6b533e982b91d6b4b413fb653b4eb3
Binary files /dev/null and b/tor_description/meshes/arm/arm_7_collision.STL differ
diff --git a/tor_description/meshes/gripper/gripper.STL b/tor_description/meshes/gripper/gripper.STL
new file mode 100644
index 0000000000000000000000000000000000000000..5f6780a05702855efebdec7e06dae8129304d578
Binary files /dev/null and b/tor_description/meshes/gripper/gripper.STL differ
diff --git a/tor_description/meshes/gripper/gripper_collision.STL b/tor_description/meshes/gripper/gripper_collision.STL
new file mode 100644
index 0000000000000000000000000000000000000000..a51b53796f5e0028998d92e8f142f4d1dd2a9cda
Binary files /dev/null and b/tor_description/meshes/gripper/gripper_collision.STL differ
diff --git a/tor_description/robots/tor_arm.urdf.xacro b/tor_description/robots/tor_arm.urdf.xacro
new file mode 100644
index 0000000000000000000000000000000000000000..a47029af9523416e28d4562810e3b5152b889ab1
--- /dev/null
+++ b/tor_description/robots/tor_arm.urdf.xacro
@@ -0,0 +1,55 @@
+<?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/arm/arm.urdf.xacro" />
+ <xacro:include filename="$(find tor_description)/urdf/gripper/gripper.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_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"/>
+
+ <!-- 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>
diff --git a/tor_description/robots/tor_full.urdf.xacro b/tor_description/robots/tor_full.urdf.xacro
new file mode 100644
index 0000000000000000000000000000000000000000..031461f2cb381eace1f3ba2e25883dfc24b6f09c
--- /dev/null
+++ b/tor_description/robots/tor_full.urdf.xacro
@@ -0,0 +1,40 @@
+<?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/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/gripper/gripper.urdf.xacro" />
+ <xacro:include filename="$(find tor_description)/urdf/leg/leg_v2.urdf.xacro" />
+
+ <xacro:tor_torso name="torso" />
+
+ <xacro:tor_head name="head" parent="torso_2_link"/>
+
+ <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:tor_leg prefix="left" reflect="1" />
+ <xacro:tor_leg prefix="right" reflect="-1" />
+
+ <!-- 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>
diff --git a/tor_description/robots/upload_arm.launch b/tor_description/robots/upload_arm.launch
new file mode 100644
index 0000000000000000000000000000000000000000..5045dd96d08540868347948ec4cd3d56003df4e7
--- /dev/null
+++ b/tor_description/robots/upload_arm.launch
@@ -0,0 +1,6 @@
+<launch>
+
+ <!-- Robot description -->
+ <param name="robot_description" command="$(find xacro)/xacro.py '$(find tor_description)/robots/tor_arm.urdf.xacro'" />
+
+</launch>
diff --git a/tor_description/robots/upload_full.launch b/tor_description/robots/upload_full.launch
new file mode 100644
index 0000000000000000000000000000000000000000..7253b43104e9255e3d44cac8de5ac95164118c11
--- /dev/null
+++ b/tor_description/robots/upload_full.launch
@@ -0,0 +1,6 @@
+<launch>
+
+ <!-- Robot description -->
+ <param name="robot_description" command="$(find xacro)/xacro.py '$(find tor_description)/robots/tor_full.urdf.xacro'" />
+
+</launch>
diff --git a/tor_description/urdf/arm/arm.transmission.xacro b/tor_description/urdf/arm/arm.transmission.xacro
new file mode 100644
index 0000000000000000000000000000000000000000..558cd993bc96bb6ec29fe236f0779131cfae368a
--- /dev/null
+++ b/tor_description/urdf/arm/arm.transmission.xacro
@@ -0,0 +1,26 @@
+<?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_arm_simple_transmission" params="name side number reduction" >
+ <transmission name="${name}_${side}_${number}_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <actuator name="${name}_${side}_${number}_motor" >
+ <mechanicalReduction>${reduction}</mechanicalReduction>
+ </actuator>
+ <joint name="${name}_${side}_${number}_joint">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ </transmission>
+ </xacro:macro>
+
+</robot>
diff --git a/tor_description/urdf/arm/arm.urdf.xacro b/tor_description/urdf/arm/arm.urdf.xacro
new file mode 100644
index 0000000000000000000000000000000000000000..3db908ad3cd66f4e6b1b9b89f4367262134fde64
--- /dev/null
+++ b/tor_description/urdf/arm/arm.urdf.xacro
@@ -0,0 +1,279 @@
+<?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/arm/wrist.urdf.xacro" />
+ <xacro:include filename="$(find tor_description)/urdf/arm/arm.transmission.xacro" />
+
+ <!--Constant parameters-->
+ <xacro:property name="arm_friction" value="1.0" />
+ <xacro:property name="arm_damping" value="1.0" />
+ <xacro:property name="arm_1_max_vel" value="2.7" />
+ <xacro:property name="arm_2_max_vel" value="3.66" />
+ <xacro:property name="arm_3_max_vel" value="4.58" />
+ <xacro:property name="arm_4_max_vel" value="4.58" />
+ <xacro:property name="arm_1_max_effort" value="44.64" />
+ <xacro:property name="arm_2_max_effort" value="22.32" />
+ <xacro:property name="arm_3_max_effort" value="17.86" />
+ <xacro:property name="arm_4_max_effort" value="17.86" />
+ <xacro:property name="arm_eps" value="0.02" />
+
+
+ <xacro:macro name="tor_arm" params="name parent side reflect">
+
+ <!--************************-->
+ <!-- SHOULDER -->
+ <!--************************-->
+ <link name="${name}_${side}_1_link">
+ <!-- TODO: Missing reflects of inertias -->
+ <inertial>
+ <origin xyz="-0.01346 0.09130 0.04812" rpy="0.00000 0.00000 0.00000"/>
+ <mass value="1.52896"/>
+ <inertia ixx="0.00555100000" ixy="-0.00073100000" ixz="0.00000800000"
+ iyy="0.00167300000" iyz="-0.00006000000"
+ izz="0.00582200000"/>
+ </inertial>
+
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://tor_description/meshes/arm/arm_1.STL" scale="1 ${reflect} 1"/>
+ </geometry>
+ <material name="DarkGrey" />
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://tor_description/meshes/arm/arm_1_collision.STL" scale="1 ${reflect} 1"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="${name}_${side}_1_joint" type="revolute">
+ <parent link="${parent}" />
+ <child link="${name}_${side}_1_link" />
+ <origin xyz="0.00000 ${0.15750*reflect} 0.23200"
+ rpy="${0.0 * deg_to_rad} ${0.0 * deg_to_rad} ${0.0 * deg_to_rad}"/>
+ <axis xyz="0 0 1" />
+ <xacro:if value="${reflect == 1}">
+ <limit lower="${-90.0 * deg_to_rad}" upper="${30.0 * deg_to_rad}" effort="${arm_1_max_effort}" velocity="${arm_1_max_vel}" />
+ </xacro:if>
+ <xacro:if value="${reflect == -1}">
+ <limit lower="${-30.0 * deg_to_rad}" upper="${90.0 * deg_to_rad}" effort="${arm_1_max_effort}" velocity="${arm_1_max_vel}" />
+ </xacro:if>
+ <dynamics friction="${arm_friction}" damping="${arm_damping}"/>
+<!-- <safety_controller k_position="20"
+ k_velocity="20"
+ soft_lower_limit="${-90.00000 * deg_to_rad + arm_eps}"
+ soft_upper_limit="${30.00000 * deg_to_rad - arm_eps}" /> -->
+ </joint>
+
+ <link name="${name}_${side}_2_link">
+ <inertial>
+ <origin xyz="0.02607 0.00049 -0.05209" rpy="0.00000 0.00000 0.00000"/>
+ <mass value="1.77729"/>
+ <inertia ixx="0.00708700000" ixy="-0.00001400000" ixz="0.00214200000"
+ iyy="0.00793600000" iyz="0.00003000000"
+ izz="0.00378800000"/>
+ </inertial>
+
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://tor_description/meshes/arm/arm_2.STL" scale="1 ${reflect} 1"/>
+ </geometry>
+ <material name="DarkGrey" />
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://tor_description/meshes/arm/arm_2_collision.STL" scale="1 ${reflect} 1"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="${name}_${side}_2_joint" type="revolute">
+ <parent link="${name}_${side}_1_link" />
+ <child link="${name}_${side}_2_link" />
+ <origin xyz="0.00493 ${0.13650*reflect} 0.04673"
+ rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
+ <axis xyz="1 0 0" />
+ <xacro:if value="${reflect == 1}">
+ <limit lower="${0.00000 * deg_to_rad}" upper="${165.00000 * deg_to_rad}" effort="${arm_2_max_effort}" velocity="${arm_2_max_vel}" />
+ </xacro:if>
+ <xacro:if value="${reflect == -1}">
+ <limit lower="${-165.00000 * deg_to_rad}" upper="${0.00000 * deg_to_rad}" effort="${arm_2_max_effort}" velocity="${arm_2_max_vel}" />
+ </xacro:if>
+ <dynamics friction="${arm_friction}" damping="${arm_damping}"/>
+<!-- <safety_controller k_position="20"
+ k_velocity="20"
+ soft_lower_limit="${0.00000 * deg_to_rad + arm_eps}"
+ soft_upper_limit="${165.00000 * deg_to_rad - arm_eps}" /> -->
+
+ </joint>
+
+ <link name="${name}_${side}_3_link">
+ <inertial>
+ <origin xyz="0.00841 0.01428 -0.22291" rpy="0.00000 0.00000 0.00000"/>
+ <mass value="1.57029"/>
+ <inertia ixx="0.00433200000" ixy="0.00015300000" ixz="-0.00049600000"
+ iyy="0.00434000000" iyz="-0.00060900000"
+ izz="0.00254300000"/>
+ </inertial>
+
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://tor_description/meshes/arm/arm_3.STL" scale="1 ${reflect} 1"/>
+ </geometry>
+ <material name="DarkGrey" />
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://tor_description/meshes/arm/arm_3_collision.STL" scale="1 ${reflect} 1"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="${name}_${side}_3_joint" type="revolute">
+ <parent link="${name}_${side}_2_link" />
+ <child link="${name}_${side}_3_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="${-140.0 * deg_to_rad}" upper="${140.0 * deg_to_rad}" effort="${arm_3_max_effort}" velocity="${arm_3_max_vel}" />
+ <dynamics friction="${arm_friction}" damping="${arm_damping}"/>
+<!-- <safety_controller k_position="20"
+ k_velocity="20"
+ soft_lower_limit="${-140.0 * deg_to_rad + arm_eps}"
+ soft_upper_limit="${140.0 * deg_to_rad - arm_eps}" /> -->
+
+ </joint>
+
+ <!--************************-->
+ <!-- ELBOW -->
+ <!--************************-->
+ <link name="${name}_${side}_4_link">
+ <inertial>
+ <origin xyz="-0.00655 -0.02107 -0.02612" rpy="0.00000 0.00000 0.00000"/>
+ <mass value="1.20216"/>
+ <inertia ixx="0.00221700000" ixy="-0.00010100000" ixz="0.00028800000"
+ iyy="0.00241800000" iyz="-0.00039300000"
+ izz="0.00111500000"/>
+ </inertial>
+
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://tor_description/meshes/arm/arm_4.STL" scale="1 ${reflect} 1"/>
+ </geometry>
+ <material name="DarkGrey" />
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://tor_description/meshes/arm/arm_4_collision.STL" scale="1 ${reflect} 1"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="${name}_${side}_4_joint" type="revolute">
+ <parent link="${name}_${side}_3_link" />
+ <child link="${name}_${side}_4_link" />
+ <origin xyz="0.02000 0.00000 -0.27300"
+ rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
+ <axis xyz="0 1 0" />
+ <limit lower="${-135.0 * deg_to_rad}" upper="${0.0 * deg_to_rad}" effort="${arm_4_max_effort}" velocity="${arm_4_max_vel}" />
+ <dynamics friction="${arm_friction}" damping="${arm_damping}"/>
+<!-- <safety_controller k_position="20"
+ k_velocity="20"
+ soft_lower_limit="${-135.0 * deg_to_rad + arm_eps}"
+ soft_upper_limit="${0.0 * deg_to_rad - arm_eps}" /> -->
+ </joint>
+
+ <gazebo reference="${name}_${side}_1_link">
+ <mu1>0.9</mu1>
+ <mu2>0.9</mu2>
+ </gazebo>
+ <gazebo reference="${name}_${side}_2_link">
+ <mu1>0.9</mu1>
+ <mu2>0.9</mu2>
+ </gazebo>
+ <gazebo reference="${name}_${side}_3_link">
+ <mu1>0.9</mu1>
+ <mu2>0.9</mu2>
+ </gazebo>
+ <gazebo reference="${name}_${side}_4_link">
+ <mu1>0.9</mu1>
+ <mu2>0.9</mu2>
+ </gazebo>
+
+ <gazebo reference="${name}_${side}_1_joint">
+ <implicitSpringDamper>1</implicitSpringDamper>
+ </gazebo>
+ <gazebo reference="${name}_${side}_2_joint">
+ <implicitSpringDamper>1</implicitSpringDamper>
+ </gazebo>
+ <gazebo reference="${name}_${side}_3_joint">
+ <implicitSpringDamper>1</implicitSpringDamper>
+ </gazebo>
+ <gazebo reference="${name}_${side}_4_joint">
+ <implicitSpringDamper>1</implicitSpringDamper>
+ </gazebo>
+
+
+ <!--************************-->
+ <!-- WRIST -->
+ <!--************************-->
+ <xacro:tor_wrist name="arm" parent="${name}_${side}_4_link" side="${side}" reflect="${reflect}" />
+
+ <!--***********************-->
+ <!-- TOOL -->
+ <!--***********************-->
+ <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" />
+ </inertial>
+ <visual>
+ <origin xyz="0.001 0 0" rpy="0 ${90.0 * deg_to_rad} 0" />
+ <geometry>
+ <cylinder radius="0.005" length="0.005"/>
+ </geometry>
+ <material name="LightGrey" />
+ </visual>
+ <collision>
+ <origin xyz="0.001 0 0" rpy="0 ${90.0 * deg_to_rad} 0" />
+ <geometry>
+ <cylinder radius="0.005" length="0.005"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <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" />
+ </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" />
+ <xacro:tor_arm_simple_transmission name="${name}" side="${side}" number="3" reduction="1.0" />
+ <xacro:tor_arm_simple_transmission name="${name}" side="${side}" number="4" reduction="1.0" />
+
+ </xacro:macro>
+
+</robot>
diff --git a/tor_description/urdf/arm/wrist.transmission.xacro b/tor_description/urdf/arm/wrist.transmission.xacro
new file mode 100644
index 0000000000000000000000000000000000000000..38e3b4b234a29174b575b5f35523533143d035f6
--- /dev/null
+++ b/tor_description/urdf/arm/wrist.transmission.xacro
@@ -0,0 +1,52 @@
+<?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_wrist_simple_transmission" params="name side number reduction" >
+ <transmission name="${name}_${side}_${number}_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <actuator name="${name}_${side}_${number}_motor" >
+ <mechanicalReduction>${reduction}</mechanicalReduction>
+ </actuator>
+ <joint name="${name}_${side}_${number}_joint">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ </transmission>
+ </xacro:macro>
+
+ <xacro:macro name="tor_wrist_differential_transmission"
+ params="name side number_1 number_2 act_reduction_1 act_reduction_2 jnt_reduction_1 jnt_reduction_2" >
+ <transmission name="wrist_${side}_trans">
+ <type>transmission_interface/DifferentialTransmission</type>
+ <actuator name="${name}_${side}_${number_1}_motor">
+ <role>actuator1</role>
+ <mechanicalReduction>${act_reduction_1}</mechanicalReduction>
+ </actuator>
+ <actuator name="${name}_${side}_${number_2}_motor">
+ <role>actuator2</role>
+ <mechanicalReduction>${act_reduction_2}</mechanicalReduction>
+ </actuator>
+ <joint name="${name}_${side}_${number_1}_joint">
+ <role>joint1</role>
+ <offset>0.0</offset>
+ <mechanicalReduction>${jnt_reduction_1}</mechanicalReduction>
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ <joint name="${name}_${side}_${number_2}_joint">
+ <role>joint2</role>
+ <offset>0.0</offset>
+ <mechanicalReduction>${jnt_reduction_2}</mechanicalReduction>
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ </transmission>
+ </xacro:macro>
+
+</robot>
diff --git a/tor_description/urdf/arm/wrist.urdf.xacro b/tor_description/urdf/arm/wrist.urdf.xacro
new file mode 100644
index 0000000000000000000000000000000000000000..f7c8fe422e063e2f5814c0e4da905ca789925b5a
--- /dev/null
+++ b/tor_description/urdf/arm/wrist.urdf.xacro
@@ -0,0 +1,191 @@
+<?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/arm/wrist.transmission.xacro" />
+
+ <!--Constant parameters-->
+ <xacro:property name="wrist_friction" value="1.0" />
+ <xacro:property name="wrist_damping" value="1.0" />
+ <xacro:property name="wrist_1_max_vel" value="1.95" />
+ <xacro:property name="wrist_2_max_vel" value="1.76" />
+ <xacro:property name="wrist_3_max_vel" value="1.76" />
+ <xacro:property name="wrist_1_max_effort" value="3" />
+ <xacro:property name="wrist_2_max_effort" value="6.6" />
+ <xacro:property name="wrist_3_max_effort" value="6.6" />
+ <xacro:property name="wrist_eps" value="0.02" />
+
+
+ <xacro:macro name="tor_wrist" params="name parent side reflect">
+
+ <!--************************-->
+ <!-- WRIST -->
+ <!--************************-->
+ <link name="${name}_${side}_5_link">
+ <inertial>
+ <origin xyz="-0.00006 0.00326 0.07963" rpy="0.00000 0.00000 0.00000"/>
+ <mass value="1.87792"/>
+ <inertia ixx="0.00349500000" ixy="-0.00001300000" ixz="-0.00001000000"
+ iyy="0.00436800000" iyz="0.00009700000"
+ izz="0.00228300000"/>
+ </inertial>
+
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://tor_description/meshes/arm/arm_5.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/arm/arm_5_collision.STL" scale="1 1 1"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <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>
+ <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}"/>
+<!-- <safety_controller k_position="20"
+ k_velocity="20"
+ soft_lower_limit="${-145.00000 * deg_to_rad + wrist_eps}"
+ soft_upper_limit="${145.00000 * deg_to_rad - wrist_eps}" /> -->
+
+ </joint>
+
+ <link name="${name}_${side}_6_link">
+ <inertial>
+ <origin xyz="0.00002 -0.00197 -0.00059" rpy="0.00000 0.00000 0.00000"/>
+ <mass value="0.40931"/>
+ <inertia ixx="0.00010700000" ixy="0.00000000000" ixz="0.00000000000"
+ iyy="0.00014100000" iyz="-0.00000000000"
+ izz="0.00015400000"/>
+ </inertial>
+
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://tor_description/meshes/arm/arm_6.STL" scale="1 ${reflect} 1"/>
+ </geometry>
+ <material name="LightGrey" />
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://tor_description/meshes/arm/arm_6_collision.STL" scale="1 ${reflect} 1"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="${name}_${side}_6_joint" type="revolute">
+ <parent link="${name}_${side}_5_link" />
+ <child link="${name}_${side}_6_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="1 0 0" />
+ <limit lower="${-80.0 * deg_to_rad}" upper="${80.0 * deg_to_rad}" effort="${wrist_2_max_effort}" velocity="${wrist_2_max_vel}" />
+ <dynamics friction="${wrist_friction}" damping="${wrist_damping}"/>
+<!-- <safety_controller k_position="20"
+ k_velocity="20"
+ soft_lower_limit="${-80.00000 * deg_to_rad + eps_radians}"
+ soft_upper_limit="${80.00000 * deg_to_rad - eps_radians}" /> -->
+
+ </joint>
+
+ <link name="${name}_${side}_7_link">
+ <inertial>
+ <origin xyz="-0.00089 0.00365 -0.07824" rpy="0.00000 0.00000 0.00000"/>
+ <mass value="1.02604"/>
+ <inertia ixx="0.00151400000" ixy="0.00000600000" ixz="0.00006600000"
+ iyy="0.00146400000" iyz="0.00001200000"
+ izz="0.00090300000"/>
+ </inertial>
+
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://tor_description/meshes/arm/arm_7.STL" scale="1 1 1"/>
+ </geometry>
+ <material name="DarkGrey" />
+ </visual>
+
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://tor_description/meshes/arm/arm_7_collision.STL" scale="1 1 1"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="${name}_${side}_7_joint" type="revolute">
+ <parent link="${name}_${side}_6_link" />
+ <child link="${name}_${side}_7_link" />
+ <origin xyz="0.0 0.0 0.0"
+ rpy="${0.0 * deg_to_rad} ${0.0 * deg_to_rad} ${0.0 * deg_to_rad}"/>
+ <axis xyz="0 1 0" />
+ <limit lower="${-40.0 * deg_to_rad}" upper="${40.0 * deg_to_rad}" effort="${wrist_3_max_effort}" velocity="${wrist_3_max_vel}"/>
+ <dynamics friction="${wrist_friction}" damping="${wrist_damping}"/>
+<!-- <safety_controller k_position="20"
+ k_velocity="20"
+ soft_lower_limit="${-40.00000 * deg_to_rad + eps_radians}"
+ soft_upper_limit="${40.00000 * deg_to_rad - eps_radians}" /> -->
+
+
+ </joint>
+
+ <gazebo reference="${name}_${side}_5_link">
+ <mu1>0.9</mu1>
+ <mu2>0.9</mu2>
+ </gazebo>
+ <gazebo reference="${name}_${side}_6_link">
+ <mu1>0.9</mu1>
+ <mu2>0.9</mu2>
+ </gazebo>
+ <gazebo reference="${name}_${side}_7_link">
+ <mu1>0.9</mu1>
+ <mu2>0.9</mu2>
+ </gazebo>
+
+ <gazebo reference="${name}_${side}_5_joint">
+ <implicitSpringDamper>1</implicitSpringDamper>
+ </gazebo>
+ <gazebo reference="${name}_${side}_6_joint">
+ <implicitSpringDamper>1</implicitSpringDamper>
+ </gazebo>
+ <gazebo reference="${name}_${side}_7_joint">
+ <implicitSpringDamper>1</implicitSpringDamper>
+ <provideFeedback>1</provideFeedback>
+ </gazebo>
+
+ <!-- extensions -->
+ <xacro:tor_wrist_simple_transmission name="${name}" side="${side}" number="5" reduction="1.0" />
+ <xacro:tor_wrist_differential_transmission name="${name}" side="${side}" number_1="6" number_2="7"
+ act_reduction_1="-1.0" act_reduction_2="1.0"
+ jnt_reduction_1="-1.0" jnt_reduction_2="${-reflect}" />
+
+ </xacro:macro>
+
+</robot>
diff --git a/tor_description/urdf/gripper/gripper.urdf.xacro b/tor_description/urdf/gripper/gripper.urdf.xacro
new file mode 100644
index 0000000000000000000000000000000000000000..098abe05f4df725b3737ac2f9c676b8b26fad74d
--- /dev/null
+++ b/tor_description/urdf/gripper/gripper.urdf.xacro
@@ -0,0 +1,47 @@
+<?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}_link">
+ <inertial>
+ <origin xyz="0.01507 -0.01177 -0.02788" rpy="0.00000 0.00000 0.00000"/>
+ <mass value="0.66262"/>
+ <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.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"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="${name}_joint" type="fixed">
+ <parent link="${parent}" />
+ <child link="${name}_link" />
+ <axis xyz="0 0 0" />
+ <origin xyz="0 0.02 -0.10975" rpy="0 0 0" />
+ </joint>
+
+
+
+ </xacro:macro>
+
+</robot>
diff --git a/tor_description/urdf/head/head.urdf.xacro b/tor_description/urdf/head/head.urdf.xacro
index 9b5935a2f557e9af25485f8fc020fcc2434a5e83..ea56e823638639adc41154868d77ec9a6c97fc66 100644
--- a/tor_description/urdf/head/head.urdf.xacro
+++ b/tor_description/urdf/head/head.urdf.xacro
@@ -39,7 +39,7 @@
<geometry>
<mesh filename="package://tor_description/meshes/head/head_1.stl" scale="1 1 1"/>
</geometry>
- <material name="LightGrey" />
+ <material name="DarkGrey" />
</visual>
<collision>
diff --git a/tor_description/urdf/materials.urdf.xacro b/tor_description/urdf/materials.urdf.xacro
index ea8e914a862a0a683f346d40b0a40e5e32983fbf..514e9ec8c420e87736e0491a2ea9a8b17a6f29a6 100644
--- a/tor_description/urdf/materials.urdf.xacro
+++ b/tor_description/urdf/materials.urdf.xacro
@@ -37,4 +37,8 @@
<material name="White">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
+
+ <material name="Orange">
+ <color rgba="1.0 0.5 0.0 1.0"/>
+ </material>
</robot>