diff --git a/src/examples/talos_box.cpp b/src/examples/talos_box.cpp
new file mode 100644
index 00000000..f8ec6bc6
--- /dev/null
+++ b/src/examples/talos_box.cpp
@@ -0,0 +1,79 @@
+#include <robot_dart/robot_dart_simu.hpp>
+#include <robot_dart/robots/talos.hpp>
+
+#ifdef GRAPHIC
+#include <robot_dart/gui/magnum/graphics.hpp>
+#endif
+
+// fast TalosFastCollision:
+// - use dart for collision detection (instead of FCL) [only handle boxes and spheres]
+// - the collisions are detected with boxes, you can have the boxes visual with talos_box.urdf
+// - the boxes are totally covering the meshes. There is one box per link
+// - the urdf does not have the mimic (used for grippers)
+int main()
+{
+    auto robot = std::make_shared<robot_dart::robots::TalosFastCollision>(1000, "talos/talos_box.urdf");
+    std::cout << "The model used is: [" << robot->model_filename() << "]" << std::endl;
+
+    // Set actuator types to VELOCITY (for speed)
+    robot->set_actuator_types("velocity");
+
+    double dt = 0.001;
+    robot_dart::RobotDARTSimu simu(dt);
+    // we can use the DART (fast) collision detector
+    simu.set_collision_detector("dart");
+#ifdef GRAPHIC
+    auto graphics = std::make_shared<robot_dart::gui::magnum::Graphics>();
+    simu.set_graphics(graphics);
+    graphics->look_at({0., 3.5, 2.}, {0., 0., 0.25});
+    graphics->record_video("talos_box.mp4");
+#endif
+    simu.add_floor();
+    simu.add_robot(robot);
+
+    simu.set_control_freq(100);
+    std::vector<std::string> dofs = {"arm_left_1_joint",
+        "arm_left_2_joint",
+        "arm_right_1_joint",
+        "arm_right_2_joint",
+        "torso_1_joint"};
+
+    Eigen::VectorXd init_positions = robot->positions(dofs);
+    std::vector<int> undamaged_joints = {0, 1, 2, 3, 4};
+    auto start = std::chrono::steady_clock::now();
+
+    while (simu.scheduler().next_time() < 20. && !simu.graphics()->done()) {
+        if (simu.scheduler().next_time() > 2. && simu.scheduler().next_time() < 2.0 + dt) {
+            robot->set_actuator_type("passive", "arm_left_1_joint");
+            robot->set_actuator_type("passive", "arm_left_2_joint");
+            robot->set_actuator_type("passive", "arm_left_3_joint");
+            robot->set_actuator_type("passive", "arm_left_4_joint");
+            undamaged_joints = {2, 3, 4};
+        }
+
+        if (simu.schedule(simu.control_freq())) {
+            Eigen::VectorXd delta_pos(5);
+            delta_pos << sin(simu.scheduler().current_time() * 2.),
+                sin(simu.scheduler().current_time() * 2.),
+                sin(simu.scheduler().current_time() * 2.),
+                sin(simu.scheduler().current_time() * 2.),
+                sin(simu.scheduler().current_time() * 2.);
+            Eigen::VectorXd commands = (init_positions + delta_pos) - robot->positions(dofs);
+            Eigen::VectorXd commands_to_send(undamaged_joints.size());
+            std::vector<std::string> dofs_to_send;
+            for (int i = 0; i < commands_to_send.size(); i++) {
+                commands_to_send[i] = commands[undamaged_joints[i]];
+                dofs_to_send.push_back(dofs[undamaged_joints[i]]);
+            }
+            robot->set_commands(commands_to_send, dofs_to_send);
+        }
+        simu.step_world();
+    }
+
+    auto end = std::chrono::steady_clock::now();
+    std::chrono::duration<double> elapsed_seconds = end - start;
+    std::cout << "benchmark time: " << elapsed_seconds.count() << "s\n";
+
+    robot.reset();
+    return 0;
+}
diff --git a/src/examples/talos_fast.cpp b/src/examples/talos_fast.cpp
index 9d50a8ac..2e64e285 100644
--- a/src/examples/talos_fast.cpp
+++ b/src/examples/talos_fast.cpp
@@ -14,15 +14,9 @@
 // - the urdf does not have the mimic (used for grippers)
 int main()
 {
-    auto robot = std::make_shared<robot_dart::robots::Talos>(1000, "talos/talos_fast.urdf");
+    auto robot = std::make_shared<robot_dart::robots::TalosLight>();
     std::cout << "The model used is: [" << robot->model_filename() << "]" << std::endl;
 
-    robot->set_position_enforced(true);
-    auto positions = robot->positions();
-    positions[2] = M_PI / 2.;
-    positions[5] = 1.1;
-    robot->set_positions(positions);
-
     // Set actuator types to VELOCITY (for speed)
     robot->set_actuator_types("velocity");
 
@@ -34,7 +28,7 @@ int main()
     auto graphics = std::make_shared<robot_dart::gui::magnum::Graphics>();
     simu.set_graphics(graphics);
     graphics->look_at({0., 3.5, 2.}, {0., 0., 0.25});
-    graphics->record_video("talos.mp4");
+    graphics->record_video("talos_light.mp4");
 #endif
     simu.add_floor();
     simu.add_robot(robot);
diff --git a/src/examples/talos_fast_collision.cpp b/src/examples/talos_fast_collision.cpp
new file mode 100644
index 00000000..5603876a
--- /dev/null
+++ b/src/examples/talos_fast_collision.cpp
@@ -0,0 +1,79 @@
+#include <robot_dart/robot_dart_simu.hpp>
+#include <robot_dart/robots/talos.hpp>
+
+#ifdef GRAPHIC
+#include <robot_dart/gui/magnum/graphics.hpp>
+#endif
+
+// fast TalosFastCollision:
+// - use dart for collision detection (instead of FCL) [only handle boxes and spheres]
+// - the collisions are detected with boxes, you can have the boxes visual with talos_box.urdf
+// - the boxes are totally covering the meshes. There is one box per link
+// - the urdf does not have the mimic (used for grippers)
+int main()
+{
+    auto robot = std::make_shared<robot_dart::robots::TalosFastCollision>();
+    std::cout << "The model used is: [" << robot->model_filename() << "]" << std::endl;
+
+    // Set actuator types to VELOCITY (for speed)
+    robot->set_actuator_types("velocity");
+
+    double dt = 0.001;
+    robot_dart::RobotDARTSimu simu(dt);
+    // we can use the DART (fast) collision detector
+    simu.set_collision_detector("dart");
+#ifdef GRAPHIC
+    auto graphics = std::make_shared<robot_dart::gui::magnum::Graphics>();
+    simu.set_graphics(graphics);
+    graphics->look_at({0., 3.5, 2.}, {0., 0., 0.25});
+    graphics->record_video("talos_fast.mp4");
+#endif
+    simu.add_floor();
+    simu.add_robot(robot);
+
+    simu.set_control_freq(100);
+    std::vector<std::string> dofs = {"arm_left_1_joint",
+        "arm_left_2_joint",
+        "arm_right_1_joint",
+        "arm_right_2_joint",
+        "torso_1_joint"};
+
+    Eigen::VectorXd init_positions = robot->positions(dofs);
+    std::vector<int> undamaged_joints = {0, 1, 2, 3, 4};
+    auto start = std::chrono::steady_clock::now();
+
+    while (simu.scheduler().next_time() < 20. && !simu.graphics()->done()) {
+        if (simu.scheduler().next_time() > 2. && simu.scheduler().next_time() < 2.0 + dt) {
+            robot->set_actuator_type("passive", "arm_left_1_joint");
+            robot->set_actuator_type("passive", "arm_left_2_joint");
+            robot->set_actuator_type("passive", "arm_left_3_joint");
+            robot->set_actuator_type("passive", "arm_left_4_joint");
+            undamaged_joints = {2, 3, 4};
+        }
+
+        if (simu.schedule(simu.control_freq())) {
+            Eigen::VectorXd delta_pos(5);
+            delta_pos << sin(simu.scheduler().current_time() * 2.),
+                sin(simu.scheduler().current_time() * 2.),
+                sin(simu.scheduler().current_time() * 2.),
+                sin(simu.scheduler().current_time() * 2.),
+                sin(simu.scheduler().current_time() * 2.);
+            Eigen::VectorXd commands = (init_positions + delta_pos) - robot->positions(dofs);
+            Eigen::VectorXd commands_to_send(undamaged_joints.size());
+            std::vector<std::string> dofs_to_send;
+            for (int i = 0; i < commands_to_send.size(); i++) {
+                commands_to_send[i] = commands[undamaged_joints[i]];
+                dofs_to_send.push_back(dofs[undamaged_joints[i]]);
+            }
+            robot->set_commands(commands_to_send, dofs_to_send);
+        }
+        simu.step_world();
+    }
+
+    auto end = std::chrono::steady_clock::now();
+    std::chrono::duration<double> elapsed_seconds = end - start;
+    std::cout << "benchmark time: " << elapsed_seconds.count() << "s\n";
+
+    robot.reset();
+    return 0;
+}
diff --git a/src/robot_dart/robots/talos.cpp b/src/robot_dart/robots/talos.cpp
index b1a06ab4..ce524dca 100644
--- a/src/robot_dart/robots/talos.cpp
+++ b/src/robot_dart/robots/talos.cpp
@@ -73,5 +73,37 @@ namespace robot_dart {
                 simu->remove_sensor(t.second);
             }
         }
+
+        void TalosFastCollision::_post_addition(RobotDARTSimu* simu)
+        {
+            Talos::_post_addition(simu);
+            auto vec = TalosFastCollision::collision_vec();
+            for (auto& t : vec) {
+                simu->set_collision_masks(simu->robots().size() - 1, std::get<0>(t), std::get<1>(t), std::get<2>(t));
+            }
+        }
+
+        std::vector<std::tuple<std::string, uint32_t, uint32_t>> TalosFastCollision::collision_vec()
+        {
+            std::vector<std::tuple<std::string, uint32_t, uint32_t>> vec;
+            vec.push_back(std::make_tuple("leg_right_6_link", 0x1, 0x1 | 0x4 | 0x8 | 0x10 | 0x20 | 0x40 | 0x80 | 0x100 | 0x200 | 0x400 | 0x800 | 0x1000 | 0x2000 | 0x4000 | 0x8000 | 0x10000));
+            vec.push_back(std::make_tuple("leg_right_4_link", 0x2, 0x2 | 0x4 | 0x8 | 0x10 | 0x20 | 0x40 | 0x80 | 0x100 | 0x200 | 0x400 | 0x800 | 0x1000 | 0x2000 | 0x4000 | 0x8000 | 0x10000));
+            vec.push_back(std::make_tuple("leg_left_6_link", 0x4, 0x1 | 0x2 | 0x4 | 0x10 | 0x20 | 0x40 | 0x80 | 0x100 | 0x200 | 0x400 | 0x800 | 0x1000 | 0x2000 | 0x4000 | 0x8000 | 0x10000));
+            vec.push_back(std::make_tuple("leg_left_4_link", 0x8, 0x1 | 0x2 | 0x8 | 0x10 | 0x20 | 0x40 | 0x80 | 0x100 | 0x200 | 0x400 | 0x800 | 0x1000 | 0x2000 | 0x4000 | 0x8000 | 0x10000));
+            vec.push_back(std::make_tuple("leg_left_1_link", 0x10, 0x1 | 0x2 | 0x4 | 0x8 | 0x10 | 0x40 | 0x80 | 0x100 | 0x200 | 0x400 | 0x800 | 0x1000 | 0x2000 | 0x4000 | 0x8000 | 0x10000));
+            vec.push_back(std::make_tuple("leg_left_3_link", 0x20, 0x1 | 0x2 | 0x4 | 0x8 | 0x20 | 0x40 | 0x80 | 0x100 | 0x200 | 0x400 | 0x800 | 0x1000 | 0x2000 | 0x4000 | 0x8000 | 0x10000));
+            vec.push_back(std::make_tuple("leg_right_1_link", 0x40, 0x1 | 0x2 | 0x4 | 0x8 | 0x10 | 0x20 | 0x40 | 0x100 | 0x200 | 0x400 | 0x800 | 0x1000 | 0x2000 | 0x4000 | 0x8000 | 0x10000));
+            vec.push_back(std::make_tuple("leg_right_3_link", 0x80, 0x1 | 0x2 | 0x4 | 0x8 | 0x10 | 0x20 | 0x80 | 0x100 | 0x200 | 0x400 | 0x800 | 0x1000 | 0x2000 | 0x4000 | 0x8000 | 0x10000));
+            vec.push_back(std::make_tuple("arm_left_7_link", 0x100, 0x1 | 0x2 | 0x4 | 0x8 | 0x10 | 0x20 | 0x40 | 0x80 | 0x100 | 0x400 | 0x800 | 0x1000 | 0x2000 | 0x4000 | 0x8000 | 0x10000));
+            vec.push_back(std::make_tuple("arm_left_5_link", 0x200, 0x1 | 0x2 | 0x4 | 0x8 | 0x10 | 0x20 | 0x40 | 0x80 | 0x200 | 0x400 | 0x800 | 0x1000 | 0x2000 | 0x4000 | 0x8000 | 0x10000));
+            vec.push_back(std::make_tuple("arm_right_7_link", 0x400, 0x1 | 0x2 | 0x4 | 0x8 | 0x10 | 0x20 | 0x40 | 0x80 | 0x100 | 0x200 | 0x400 | 0x1000 | 0x2000 | 0x4000 | 0x8000 | 0x10000));
+            vec.push_back(std::make_tuple("arm_right_5_link", 0x800, 0x1 | 0x2 | 0x4 | 0x8 | 0x10 | 0x20 | 0x40 | 0x80 | 0x100 | 0x200 | 0x800 | 0x1000 | 0x2000 | 0x4000 | 0x8000 | 0x10000));
+            vec.push_back(std::make_tuple("torso_2_link", 0x1000, 0x1 | 0x2 | 0x4 | 0x8 | 0x10 | 0x20 | 0x40 | 0x80 | 0x100 | 0x200 | 0x800 | 0x1000 | 0x4000 | 0x8000));
+            vec.push_back(std::make_tuple("base_link", 0x2000, 0x1 | 0x2 | 0x4 | 0x8 | 0x10 | 0x20 | 0x40 | 0x80 | 0x100 | 0x200 | 0x800 | 0x2000 | 0x10000));
+            vec.push_back(std::make_tuple("leg_right_2_link", 0x4000, 0x1 | 0x2 | 0x4 | 0x8 | 0x10 | 0x20 | 0x40 | 0x80 | 0x100 | 0x200 | 0x800 | 0x1000 | 0x4000 | 0x8000 | 0x10000));
+            vec.push_back(std::make_tuple("leg_left_2_link", 0x8000, 0x1 | 0x2 | 0x4 | 0x8 | 0x10 | 0x20 | 0x40 | 0x80 | 0x100 | 0x200 | 0x800 | 0x1000 | 0x4000 | 0x8000 | 0x10000));
+            vec.push_back(std::make_tuple("head_2_link", 0x10000, 0x1 | 0x2 | 0x4 | 0x8 | 0x10 | 0x20 | 0x40 | 0x80 | 0x100 | 0x200 | 0x800 | 0x2000 | 0x4000 | 0x8000 | 0x10000));
+            return vec;
+        }
     } // namespace robots
 } // namespace robot_dart
diff --git a/src/robot_dart/robots/talos.hpp b/src/robot_dart/robots/talos.hpp
index 6c69ca07..73b5d21a 100644
--- a/src/robot_dart/robots/talos.hpp
+++ b/src/robot_dart/robots/talos.hpp
@@ -39,6 +39,16 @@ namespace robot_dart {
         public:
             TalosLight(size_t frequency = 1000, const std::string& urdf = "talos/talos_fast.urdf", const std::vector<std::pair<std::string, std::string>>& packages = {{"talos_description", "talos/talos_description"}}) : Talos(frequency, urdf, packages) {}
         };
+
+        // for talos_fast_collision.urdf or talos_box.urdf which have simple box collision shapes
+        class TalosFastCollision : public Talos {
+        public:
+            TalosFastCollision(size_t frequency = 1000, const std::string& urdf = "talos/talos_fast_collision.urdf", const std::vector<std::pair<std::string, std::string>>& packages = {{"talos_description", "talos/talos_description"}}) : Talos(frequency, urdf, packages) { set_self_collision(); }
+            static std::vector<std::tuple<std::string, uint32_t, uint32_t>> collision_vec();
+
+        protected:
+            void _post_addition(RobotDARTSimu* simu) override;
+        };
     } // namespace robots
 } // namespace robot_dart
 #endif
diff --git a/utheque/talos/talos_box.urdf b/utheque/talos/talos_box.urdf
new file mode 100644
index 00000000..84e7e95a
--- /dev/null
+++ b/utheque/talos/talos_box.urdf
@@ -0,0 +1,1751 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from talos_full_v2.urdf.xacro       | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<!--
+
+  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="talos" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor">
+  <!--************************-->
+  <!--        TORSO_2  (TILT) -->
+  <!--************************-->
+  <link name="torso_2_link">
+    <inertial>
+      <mass value="16.97403"/>
+      <inertia ixx="0.44372633826" ixy="0.00069132133" ixz="-0.01218206353" iyy="0.2998576068" iyz="-0.00019623338" izz="0.32201554742"/>
+      <origin rpy="0 0 0" xyz="-0.0463563 -0.00099023 0.1452805"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="-0.06 0 0.17"/>
+      <geometry>
+        <box size="0.29 0.45 0.45"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.06 0 0.17"/>
+      <geometry>
+        <box size="0.29 0.45 0.45"/>
+        <!-- <mesh filename="package://talos_description/meshes/torso/torso_2_collision.glb" scale="1 1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <!--************************-->
+  <!--        TORSO_1  (PAN)  -->
+  <!--************************-->
+  <link name="torso_1_link">
+    <inertial>
+      <mass value="2.294658"/>
+      <inertia ixx="0.00638508087" ixy="-7.107e-08" ixz="-3.065592e-05" iyy="0.00410256102" iyz="-1.46946e-06" izz="0.00622968815"/>
+      <origin rpy="0 0 0" xyz="0.00078223 3.528e-05 -0.01782457"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/torso/torso_1.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/torso/torso_1.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="torso_1_joint" type="revolute">
+    <parent link="base_link"/>
+    <child link="torso_1_link"/>
+    <!-- XXX AS: The kinematic chain order for the torso was reversed manually. This value does not match the CAD model. XXX -->
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0722"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="200.0" lower="-1.25663706144" upper="1.25663706144" velocity="5.4"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <!-- <safety_controller k_position="20"
+                         k_velocity="20"
+                         soft_lower_limit="${-15.0 * deg_to_rad + torso_eps}"
+                         soft_upper_limit="${ 45.0 * deg_to_rad - torso_eps}" /> -->
+  </joint>
+  <!--************************-->
+  <!--        BASE_LINK       -->
+  <!--************************-->
+  <link name="base_link">
+    <inertial>
+      <mass value="15.36284"/>
+      <inertia ixx="0.20105075811" ixy="0.00023244734" ixz="0.0040167728" iyy="0.08411496729" iyz="-0.00087206649" izz="0.2318908414"/>
+      <origin rpy="0 0 0" xyz="-0.05709419 0.00153054 -0.0762521"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="-0.04 0 -0.085"/>
+      <geometry>
+        <box size="0.32 0.45 0.2"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.04 0 -0.085"/>
+      <geometry>
+        <box size="0.32 0.45 0.2"/>
+        <!-- <mesh filename="package://talos_description/meshes/torso/base_link_collision.glb" scale="1 1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="torso_2_joint" type="revolute">
+    <parent link="torso_1_link"/>
+    <child link="torso_2_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="200.0" lower="-0.226892802759" upper="0.733038285838" velocity="5.4"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <!-- <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>
+  <link name="imu_link">
+    <inertial>
+      <mass value="0.01"/>
+      <inertia ixx="1e-06" ixy="0" ixz="0" iyy="1e-06" iyz="0" izz="1e-06"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+      <material name="White"/>
+      <!-- Not sure about this -->
+    </visual>
+  </link>
+  <joint name="imu_joint" type="fixed">
+    <origin rpy="3.14159265359 0 1.57079632679" xyz="0.04925 0 0.078"/>
+    <parent link="torso_2_link"/>
+    <child link="imu_link"/>
+  </joint>
+  <link name="head_1_link">
+    <inertial>
+      <mass value="0.73746"/>
+      <inertia ixx="0.00224878584" ixy="4.69375e-06" ixz="8.55557e-05" iyy="0.00111158492" iyz="-4.132536e-05" izz="0.00205225921"/>
+      <origin rpy="0 0 0" xyz="-0.00157211 -0.00157919 0.02175767"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/head/head_1.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/head/head_1_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="head_1_joint" type="revolute">
+    <parent link="torso_2_link"/>
+    <child link="head_1_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.02000 0.00000 0.32100"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="8.0" lower="-0.209439510239" upper="0.785398163397" velocity="3.0"/>
+    <dynamics damping="0.5" friction="1.0"/>
+    <!-- <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>
+  <link name="head_2_link">
+    <inertial>
+      <mass value="1.443954"/>
+      <inertia ixx="0.01084624339" ixy="1.050889e-05" ixz="0.00041594252" iyy="0.0109569176" iyz="2.367831e-05" izz="0.00571698895"/>
+      <origin rpy="0 0 0" xyz="0.01002657 5.218e-05 0.14136068"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.01 0 0.18"/>
+      <geometry>
+        <box size="0.23 0.21 0.21"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.01 0 0.18"/>
+      <geometry>
+        <box size="0.23 0.21 0.21"/>
+        <!-- <mesh filename="package://talos_description/meshes/head/head_2_default_collision.stl" scale="1 1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="head_2_joint" type="revolute">
+    <parent link="head_1_link"/>
+    <child link="head_2_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00000 0.00000"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="4.0" lower="-1.308996939" upper="1.308996939" velocity="3.0"/>
+    <dynamics damping="0.5" friction="1.0"/>
+    <!-- <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>
+  <!-- frames in the center of the camera -->
+  <joint name="rgbd_joint" type="fixed">
+    <origin rpy="0.0 0.0 0.0" xyz="0.066 0.0 0.1982"/>
+    <axis xyz="0 0 1"/>
+    <!-- <limit lower="0" upper="0.001" effort="100" velocity="0.01"/> -->
+    <parent link="head_2_link"/>
+    <child link="rgbd_link"/>
+  </joint>
+  <link name="rgbd_link">
+    <inertial>
+      <mass value="0.01"/>
+      <inertia ixx="3e-05" ixy="0" ixz="0" iyy="2e-06" iyz="0" izz="3e-05"/>
+      <origin rpy="0 0 0" xyz="-0.01 0.0025 0"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/sensors/orbbec/orbbec.glb"/>
+      </geometry>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="-0.01 0.0025 0"/>
+      <geometry>
+        <box size="0.04 0.185 0.03"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="rgbd_optical_joint" type="fixed">
+    <origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 0 0"/>
+    <parent link="rgbd_link"/>
+    <child link="rgbd_optical_frame"/>
+  </joint>
+  <link name="rgbd_optical_frame">
+    <inertial>
+      <mass value="0.01"/>
+      <inertia ixx="1e-06" ixy="0" ixz="0" iyy="1e-06" iyz="0" izz="1e-06"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </inertial>
+  </link>
+  <!-- frames of the depth sensor -->
+  <joint name="rgbd_depth_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0.0 0.03751 0.0"/>
+    <parent link="rgbd_link"/>
+    <child link="rgbd_depth_frame"/>
+  </joint>
+  <link name="rgbd_depth_frame">
+    <inertial>
+      <mass value="0.01"/>
+      <inertia ixx="1e-06" ixy="0" ixz="0" iyy="1e-06" iyz="0" izz="1e-06"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </inertial>
+  </link>
+  <joint name="rgbd_depth_optical_joint" type="fixed">
+    <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
+    <parent link="rgbd_depth_frame"/>
+    <child link="rgbd_depth_optical_frame"/>
+  </joint>
+  <link name="rgbd_depth_optical_frame">
+    <inertial>
+      <mass value="0.01"/>
+      <inertia ixx="1e-06" ixy="0" ixz="0" iyy="1e-06" iyz="0" izz="1e-06"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </inertial>
+  </link>
+  <!-- frames of the rgb sensor -->
+  <joint name="rgbd_rgb_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0.0 0.01251 0.0"/>
+    <parent link="rgbd_link"/>
+    <child link="rgbd_rgb_frame"/>
+  </joint>
+  <link name="rgbd_rgb_frame">
+    <inertial>
+      <mass value="0.01"/>
+      <inertia ixx="1e-06" ixy="0" ixz="0" iyy="1e-06" iyz="0" izz="1e-06"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </inertial>
+  </link>
+  <joint name="rgbd_rgb_optical_joint" type="fixed">
+    <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
+    <parent link="rgbd_rgb_frame"/>
+    <child link="rgbd_rgb_optical_frame"/>
+  </joint>
+  <link name="rgbd_rgb_optical_frame">
+    <inertial>
+      <mass value="0.01"/>
+      <inertia ixx="1e-06" ixy="0" ixz="0" iyy="1e-06" iyz="0" izz="1e-06"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </inertial>
+  </link>
+  <!--************************-->
+  <!--        SHOULDER        -->
+  <!--************************-->
+  <link name="arm_left_1_link">
+    <inertial>
+      <mass value="2.714567"/>
+      <inertia ixx="0.01237818683" ixy="-3.625571e-05" ixz="7.14472e-05" iyy="0.004191372" iyz="-0.00023639064" izz="0.01358161109"/>
+      <origin rpy="0 0 0" xyz="-0.0002762 0.10060223 0.04437419"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0 0.14 0.04"/>
+      <geometry>
+        <box size="0.2 0.12 0.14"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.0 0.14 0.04"/>
+      <geometry>
+        <box size="0.2 0.12 0.14"/>
+        <!-- <mesh filename="package://talos_description/meshes/arm/arm_1_collision.glb" scale="1 1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_left_1_joint" type="revolute">
+    <parent link="torso_2_link"/>
+    <child link="arm_left_1_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.1575 0.23200"/>
+    <axis xyz="0 0 1"/>
+    <!-- v1 TALOS has 1st arm left joint (-90,30) and right joint (-30,90) -->
+    <!-- v2 TALOS has 1st arm left joint (-90,50) and right joint (-50,90) -->
+    <limit effort="100.0" lower="-1.57079632679" upper="0.785398163397" velocity="2.7"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <!--        <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="arm_left_2_link">
+    <inertial>
+      <mass value="2.425086"/>
+      <inertia ixx="0.01297822101" ixy="1.208791e-05" ixz="-0.00320370433" iyy="0.01380870278" iyz="-0.00012770059" izz="0.00478856621"/>
+      <origin rpy="0 0 0" xyz="0.01438831 0.00092938 -0.08684268"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.01 0.0 -0.1"/>
+      <geometry>
+        <box size="0.16 0.12 0.12"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.01 0.0 -0.1"/>
+      <geometry>
+        <box size="0.16 0.12 0.12"/>
+        <!-- <mesh filename="package://talos_description/meshes/arm/arm_2_collision.glb" scale="1 1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_left_2_joint" type="revolute">
+    <parent link="arm_left_1_link"/>
+    <child link="arm_left_2_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00493 0.1365 0.04673"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100.0" lower="0.00872664625997" upper="2.87106661953" velocity="3.66"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <!--        <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="arm_left_3_link">
+    <inertial>
+      <mass value="2.208741"/>
+      <inertia ixx="0.00718831493" ixy="-0.00011563551" ixz="0.00075969733" iyy="0.00693528503" iyz="0.00042134743" izz="0.00388359007"/>
+      <origin rpy="0 0 0" xyz="0.0136084 0.01241619 -0.2499004"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.01 0.01 -0.25"/>
+      <geometry>
+        <box size="0.14 0.16 0.17"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.01 0.01 -0.25"/>
+      <geometry>
+        <box size="0.14 0.16 0.17"/>
+        <!-- <mesh filename="package://talos_description/meshes/arm/arm_3_collision.glb" scale="1 1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_left_3_joint" type="revolute">
+    <parent link="arm_left_2_link"/>
+    <child link="arm_left_3_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00000 0.00000"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="70.0" lower="-2.42600766027" upper="2.42600766027" velocity="4.58"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <!--       <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="arm_left_4_link">
+    <inertial>
+      <mass value="0.877346"/>
+      <inertia ixx="0.00251207716" ixy="0.00010070062" ixz="-0.00032788214" iyy="0.00275869324" iyz="0.00040022227" izz="0.00120735959"/>
+      <origin rpy="0 0 0" xyz="-0.00742138 -0.0213895 -0.03312656"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="-0.02 -0.01 -0.07"/>
+      <geometry>
+        <box size="0.1 0.11 0.07"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.02 -0.01 -0.07"/>
+      <geometry>
+        <box size="0.1 0.11 0.07"/>
+        <!-- <mesh filename="package://talos_description/meshes/arm/arm_4_collision.glb" scale="1 1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_left_4_joint" type="revolute">
+    <parent link="arm_left_3_link"/>
+    <child link="arm_left_4_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.02000 0.00000 -0.27300"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="70.0" lower="-2.23402144255" upper="-0.00349065850399" velocity="4.58"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <!--       <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>
+  <!--************************-->
+  <!--        WRIST           -->
+  <!--************************-->
+  <link name="arm_left_5_link">
+    <inertial>
+      <mass value="1.877923"/>
+      <inertia ixx="0.00349507283" ixy="1.265489e-05" ixz="1.038286e-05" iyy="0.00436830072" iyz="-9.736042e-05" izz="0.0022826337"/>
+      <origin rpy="0 0 0" xyz="-6e-05 0.003262 0.079625"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0 0.01 0.07"/>
+      <geometry>
+        <box size="0.16 0.12 0.19"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.0 0.01 0.07"/>
+      <geometry>
+        <box size="0.16 0.12 0.19"/>
+        <!-- <mesh filename="package://talos_description/meshes/arm/arm_5_collision.glb" scale="1 1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_left_5_joint" type="revolute">
+    <parent link="arm_left_4_link"/>
+    <child link="arm_left_5_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="-0.02000 0.00000 -0.26430"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="20.0" lower="-2.51327412287" upper="2.51327412287" velocity="1.95"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <!--       <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="arm_left_6_link">
+    <inertial>
+      <mass value="0.40931"/>
+      <inertia ixx="0.00010700023" ixy="-8.899e-08" ixz="-4.392e-08" iyy="0.00014101316" iyz="4.1702e-07" izz="0.00015398658"/>
+      <origin rpy="0 0 0" xyz="2.1e-05 -0.001965 -0.000591"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/arm/arm_6.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/arm/arm_6_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="arm_left_6_joint" type="revolute">
+    <parent link="arm_left_5_link"/>
+    <child link="arm_left_6_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00000 0.00000"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="8.0" lower="-1.37008346282" upper="1.37008346282" velocity="1.76"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <!--       <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="arm_left_7_link">
+    <inertial>
+      <mass value="0.308441"/>
+      <inertia ixx="0.00030894317" ixy="-1.58687e-06" ixz="1.73418e-06" iyy="0.00021886181" iyz="-1.221167e-05" izz="0.00017519492"/>
+      <origin rpy="0 0 0" xyz="0.007525 0.001378 -0.02463"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 -0.14"/>
+      <geometry>
+        <box size="0.12 0.17 0.23"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -0.14"/>
+      <geometry>
+        <box size="0.12 0.17 0.23"/>
+        <!-- <mesh filename="package://talos_description/meshes/arm/arm_7_collision.glb" scale="1 1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_left_7_joint" type="revolute">
+    <parent link="arm_left_6_link"/>
+    <child link="arm_left_7_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="8.0" lower="-0.680678408278" upper="0.680678408278" velocity="1.76"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <!--       <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>
+  <!--************************-->
+  <!--        SHOULDER        -->
+  <!--************************-->
+  <link name="arm_right_1_link">
+    <inertial>
+      <mass value="2.714567"/>
+      <inertia ixx="0.01237818683" ixy="3.625571e-05" ixz="7.14472e-05" iyy="0.004191372" iyz="0.00023639064" izz="0.01358161109"/>
+      <origin rpy="0 0 0" xyz="-0.0002762 -0.10060223 0.04437419"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0 -0.14 0.04"/>
+      <geometry>
+        <box size="0.2 0.12 0.14"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.0 -0.14 0.04"/>
+      <geometry>
+        <box size="0.2 0.12 0.14"/>
+        <!-- <mesh filename="package://talos_description/meshes/arm/arm_1_collision.glb" scale="1 -1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_right_1_joint" type="revolute">
+    <parent link="torso_2_link"/>
+    <child link="arm_right_1_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 -0.1575 0.23200"/>
+    <axis xyz="0 0 1"/>
+    <!-- v1 TALOS has 1st arm left joint (-90,30) and right joint (-30,90) -->
+    <!-- v2 TALOS has 1st arm left joint (-90,50) and right joint (-50,90) -->
+    <limit effort="100.0" lower="-0.785398163397" upper="1.57079632679" velocity="2.7"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <!--        <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="arm_right_2_link">
+    <inertial>
+      <mass value="2.425086"/>
+      <inertia ixx="0.01297822101" ixy="-1.208791e-05" ixz="-0.00320370433" iyy="0.01380870278" iyz="0.00012770059" izz="0.00478856621"/>
+      <origin rpy="0 0 0" xyz="0.01438831 -0.00092938 -0.08684268"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.01 0.0 -0.1"/>
+      <geometry>
+        <box size="0.16 0.12 0.12"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.01 0.0 -0.1"/>
+      <geometry>
+        <box size="0.16 0.12 0.12"/>
+        <!-- <mesh filename="package://talos_description/meshes/arm/arm_2_collision.glb" scale="1 -1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_right_2_joint" type="revolute">
+    <parent link="arm_right_1_link"/>
+    <child link="arm_right_2_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00493 -0.1365 0.04673"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100.0" lower="-2.87106661953" upper="-0.00872664625997" velocity="3.66"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <!--        <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="arm_right_3_link">
+    <inertial>
+      <mass value="2.208741"/>
+      <inertia ixx="0.00718831493" ixy="0.00011563551" ixz="0.00075969733" iyy="0.00693528503" iyz="-0.00042134743" izz="0.00388359007"/>
+      <origin rpy="0 0 0" xyz="0.0136084 -0.01241619 -0.2499004"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.01 -0.01 -0.25"/>
+      <geometry>
+        <box size="0.14 0.16 0.17"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.01 -0.01 -0.25"/>
+      <geometry>
+        <box size="0.14 0.16 0.17"/>
+        <!-- <mesh filename="package://talos_description/meshes/arm/arm_3_collision.glb" scale="1 -1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_right_3_joint" type="revolute">
+    <parent link="arm_right_2_link"/>
+    <child link="arm_right_3_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00000 0.00000"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="70.0" lower="-2.42600766027" upper="2.42600766027" velocity="4.58"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <!--       <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="arm_right_4_link">
+    <inertial>
+      <mass value="0.877346"/>
+      <inertia ixx="0.00251207716" ixy="-0.00010070062" ixz="-0.00032788214" iyy="0.00275869324" iyz="-0.00040022227" izz="0.00120735959"/>
+      <origin rpy="0 0 0" xyz="-0.00742138 0.0213895 -0.03312656"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="-0.02 0.01 -0.07"/>
+      <geometry>
+        <box size="0.1 0.11 0.07"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.02 0.01 -0.07"/>
+      <geometry>
+        <box size="0.1 0.11 0.07"/>
+        <!-- <mesh filename="package://talos_description/meshes/arm/arm_4_collision.glb" scale="1 -1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_right_4_joint" type="revolute">
+    <parent link="arm_right_3_link"/>
+    <child link="arm_right_4_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.02000 0.00000 -0.27300"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="70.0" lower="-2.23402144255" upper="-0.00349065850399" velocity="4.58"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <!--       <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>
+  <!--************************-->
+  <!--        WRIST           -->
+  <!--************************-->
+  <link name="arm_right_5_link">
+    <inertial>
+      <mass value="1.877923"/>
+      <inertia ixx="0.00349507283" ixy="-1.265489e-05" ixz="1.038286e-05" iyy="0.00436830072" iyz="9.736042e-05" izz="0.0022826337"/>
+      <origin rpy="0 0 0" xyz="-6e-05 -0.003262 0.079625"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0 -0.01 0.07"/>
+      <geometry>
+        <box size="0.16 0.12 0.19"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.0 -0.01 0.07"/>
+      <geometry>
+        <box size="0.16 0.12 0.19"/>
+        <!-- <mesh filename="package://talos_description/meshes/arm/arm_5_collision.glb" scale="1 -1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_right_5_joint" type="revolute">
+    <parent link="arm_right_4_link"/>
+    <child link="arm_right_5_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="-0.02000 0.00000 -0.26430"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="20.0" lower="-2.51327412287" upper="2.51327412287" velocity="1.95"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <!--       <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="arm_right_6_link">
+    <inertial>
+      <mass value="0.40931"/>
+      <inertia ixx="0.00010700023" ixy="8.899e-08" ixz="-4.392e-08" iyy="0.00014101316" iyz="-4.1702e-07" izz="0.00015398658"/>
+      <origin rpy="0 0 0" xyz="2.1e-05 0.001965 -0.000591"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/arm/arm_6.glb" scale="1 -1 1"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/arm/arm_6_collision.glb" scale="1 -1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="arm_right_6_joint" type="revolute">
+    <parent link="arm_right_5_link"/>
+    <child link="arm_right_6_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00000 0.00000"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="8.0" lower="-1.37008346282" upper="1.37008346282" velocity="1.76"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <!--       <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="arm_right_7_link">
+    <inertial>
+      <mass value="0.308441"/>
+      <inertia ixx="0.00030894317" ixy="1.58687e-06" ixz="1.73418e-06" iyy="0.00021886181" iyz="1.221167e-05" izz="0.00017519492"/>
+      <origin rpy="0 0 0" xyz="0.007525 -0.001378 -0.02463"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 -0.14"/>
+      <geometry>
+        <box size="0.12 0.17 0.23"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -0.14"/>
+      <geometry>
+        <box size="0.12 0.17 0.23"/>
+        <!-- <mesh filename="package://talos_description/meshes/arm/arm_7_collision.glb" scale="1 -1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_right_7_joint" type="revolute">
+    <parent link="arm_right_6_link"/>
+    <child link="arm_right_7_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="8.0" lower="-0.680678408278" upper="0.680678408278" velocity="1.76"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <!--       <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>
+  <!--************************-->
+  <!--        ft sensor       -->
+  <!--************************-->
+  <link name="wrist_right_ft_link">
+    <inertial>
+      <mass value="0.095"/>
+      <inertia ixx="1.108502e-05" ixy="9.9077e-07" ixz="-2.3811e-07" iyy="1.092725e-05" iyz="2.3137e-07" izz="1.898772e-05"/>
+      <origin rpy="0 0 0" xyz="-0.001001 0.000995 -0.008108"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.0157" radius="0.0225"/>
+      </geometry>
+      <material name="LightGrey"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.0157" radius="0.0225"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="wrist_right_ft_joint" type="fixed">
+    <parent link="arm_right_7_link"/>
+    <child link="wrist_right_ft_link"/>
+    <origin rpy="0.0 0 -1.57079632679" xyz="0 0 -0.051"/>
+  </joint>
+  <!--***********************-->
+  <!--       FT TOOL         -->
+  <!--***********************-->
+  <link name="wrist_right_ft_tool_link">
+    <inertial>
+      <mass value="0.001"/>
+      <inertia ixx="1e-06" ixy="0" ixz="0" iyy="1e-06" iyz="0" izz="1e-06"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0.0 0" xyz="0.0 0 0"/>
+      <geometry>
+        <cylinder length="0.00975" radius="0.025"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0.0 0" xyz="0.0 0 0"/>
+      <geometry>
+        <cylinder length="0.00975" radius="0.025"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="wrist_right_tool_joint" type="fixed">
+    <parent link="wrist_right_ft_link"/>
+    <child link="wrist_right_ft_tool_link"/>
+    <origin rpy="0 0 -1.57079632679" xyz="0 0 -0.012725"/>
+  </joint>
+  <!--************************-->
+  <!--        ft sensor       -->
+  <!--************************-->
+  <link name="wrist_left_ft_link">
+    <inertial>
+      <mass value="0.095"/>
+      <inertia ixx="1.108502e-05" ixy="9.9077e-07" ixz="-2.3811e-07" iyy="1.092725e-05" iyz="2.3137e-07" izz="1.898772e-05"/>
+      <origin rpy="0 0 0" xyz="-0.001001 0.000995 -0.008108"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.0157" radius="0.0225"/>
+      </geometry>
+      <material name="LightGrey"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.0157" radius="0.0225"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="wrist_left_ft_joint" type="fixed">
+    <parent link="arm_left_7_link"/>
+    <child link="wrist_left_ft_link"/>
+    <origin rpy="0.0 0 -4.71238898038" xyz="0 0 -0.051"/>
+  </joint>
+  <!--***********************-->
+  <!--       FT TOOL         -->
+  <!--***********************-->
+  <link name="wrist_left_ft_tool_link">
+    <inertial>
+      <mass value="0.001"/>
+      <inertia ixx="1e-06" ixy="0" ixz="0" iyy="1e-06" iyz="0" izz="1e-06"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0.0 0" xyz="0.0 0 0"/>
+      <geometry>
+        <cylinder length="0.00975" radius="0.025"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0.0 0" xyz="0.0 0 0"/>
+      <geometry>
+        <cylinder length="0.00975" radius="0.025"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="wrist_left_tool_joint" type="fixed">
+    <parent link="wrist_left_ft_link"/>
+    <child link="wrist_left_ft_tool_link"/>
+    <origin rpy="0 0 4.71238898038" xyz="0 0 -0.012725"/>
+  </joint>
+  <link name="gripper_left_base_link">
+    <inertial>
+      <mass value="0.637534"/>
+      <inertia ixx="0.0005849692" ixy="4.37159e-06" ixz="-1.2589e-05" iyy="0.00067076439" iyz="2.824361e-05" izz="0.00085526645"/>
+      <origin rpy="0 0 0" xyz="2.8e-05 0.005394 -0.023654"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/base_link.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/base_link_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="gripper_left_base_link_joint" type="fixed">
+    <parent link="wrist_left_ft_tool_link"/>
+    <child link="gripper_left_base_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00000 -0.02875"/>
+  </joint>
+  <link name="gripper_left_motor_double_link">
+    <inertial>
+      <mass value="0.134356"/>
+      <inertia ixx="0.00013479919" ixy="6.184429e-05" ixz="-3.0798e-05" iyy="0.00019602207" iyz="4.707722e-05" izz="0.00024217879"/>
+      <origin rpy="0 0 0" xyz="0.019654 0.018572 -0.011998"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/gripper_motor_double.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="Red"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/gripper_motor_double_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="gripper_left_joint" type="revolute">
+    <parent link="gripper_left_base_link"/>
+    <child link="gripper_left_motor_double_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.02025 -0.03"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="1.0" lower="-0.959931088597" upper="0.0" velocity="1.0"/>
+    <dynamics damping="1.0" friction="1.0"/>
+  </joint>
+  <link name="gripper_left_inner_double_link">
+    <inertial>
+      <mass value="0.087986"/>
+      <inertia ixx="7.410642e-05" ixy="-7.4773e-07" ixz="1.01536e-06" iyy="0.0001279418" iyz="2.504083e-05" izz="0.00010495175"/>
+      <origin rpy="0 0 0" xyz="-0.013283 0.036852 -0.023153"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/inner_double.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="Red"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/inner_double_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="gripper_left_inner_double_joint" type="fixed">
+    <parent link="gripper_left_base_link"/>
+    <child link="gripper_left_inner_double_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00525 -0.05598"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100.0" lower="-1.0471975512" upper="0.0" velocity="1.0"/>
+    <!-- <mimic joint="gripper_left_joint" multiplier="1.0" offset="0.0"/> -->
+  </joint>
+  <link name="gripper_left_fingertip_1_link">
+    <inertial>
+      <mass value="0.026301"/>
+      <inertia ixx="8e-06" ixy="0" ixz="0" iyy="8.69179e-06" iyz="-1.43612e-06" izz="2.3082e-06"/>
+      <origin rpy="0 0 0" xyz="0 0.004604 -0.002537"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/fingertip.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/fingertip_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="gripper_left_fingertip_1_joint" type="fixed">
+    <parent link="gripper_left_inner_double_link"/>
+    <child link="gripper_left_fingertip_1_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.03200 0.04589 -0.06553"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100.0" lower="0.0" upper="1.0471975512" velocity="1.0"/>
+    <!-- <mimic joint="gripper_left_joint" multiplier="-1.0" offset="0.0"/> -->
+  </joint>
+  <link name="gripper_left_fingertip_2_link">
+    <inertial>
+      <mass value="0.026301"/>
+      <inertia ixx="8e-06" ixy="0" ixz="0" iyy="8.69179e-06" iyz="-1.43612e-06" izz="2.3082e-06"/>
+      <origin rpy="0 0 0" xyz="0 0.004604 -0.002537"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/fingertip.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/fingertip_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="gripper_left_fingertip_2_joint" type="fixed">
+    <parent link="gripper_left_inner_double_link"/>
+    <child link="gripper_left_fingertip_2_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="-0.03200 0.04589 -0.06553"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100.0" lower="0.0" upper="1.0471975512" velocity="1.0"/>
+    <!-- <mimic joint="gripper_left_joint" multiplier="-1.0" offset="0.0"/> -->
+  </joint>
+  <link name="gripper_left_motor_single_link">
+    <inertial>
+      <mass value="0.107923"/>
+      <inertia ixx="8.973662e-05" ixy="-4.082027e-05" ixz="-1.927099e-05" iyy="0.00011957255" iyz="-2.873284e-05" izz="0.00015469079"/>
+      <origin rpy="0 0 0" xyz="0.025237 -0.011231 -0.008158"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/gripper_motor_single.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="Red"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/gripper_motor_single_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="gripper_left_motor_single_joint" type="fixed">
+    <parent link="gripper_left_base_link"/>
+    <child link="gripper_left_motor_single_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 -0.02025 -0.03000"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100.0" lower="0.0" upper="1.0471975512" velocity="1.0"/>
+    <!-- <mimic joint="gripper_left_joint" multiplier="-1.0" offset="0.0"/> -->
+  </joint>
+  <link name="gripper_left_inner_single_link">
+    <inertial>
+      <mass value="0.047177"/>
+      <inertia ixx="4.199818e-05" ixy="1.3418e-07" ixz="1.9358e-07" iyy="3.231338e-05" iyz="-1.509136e-05" izz="2.168842e-05"/>
+      <origin rpy="0 0 0" xyz="0 -0.034565 -0.021412"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/inner_single.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="Red"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/inner_single_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="gripper_left_inner_single_joint" type="fixed">
+    <parent link="gripper_left_base_link"/>
+    <child link="gripper_left_inner_single_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 -0.00525 -0.05598"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100.0" lower="0.0" upper="1.0471975512" velocity="1.0"/>
+    <!-- <mimic joint="gripper_left_joint" multiplier="-1.0" offset="0.0"/> -->
+  </joint>
+  <link name="gripper_left_fingertip_3_link">
+    <inertial>
+      <mass value="0.026301"/>
+      <inertia ixx="8e-06" ixy="0" ixz="0" iyy="8.69179e-06" iyz="-1.43612e-06" izz="2.3082e-06"/>
+      <origin rpy="0 0 0" xyz="0 0.004604 -0.002537"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/fingertip.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/fingertip_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="gripper_left_fingertip_3_joint" type="fixed">
+    <parent link="gripper_left_inner_single_link"/>
+    <child link="gripper_left_fingertip_3_link"/>
+    <origin rpy="0.0 0.0 3.14159265359" xyz="0.00000 -0.04589 -0.06553"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100.0" lower="0.0" upper="1.0471975512" velocity="1.0"/>
+    <!-- <mimic joint="gripper_left_joint" multiplier="-1.0" offset="0.0"/> -->
+  </joint>
+  <!-- virtual mimic joints -->
+  <!--         <xacro:gripper_virtual_transmission name="${name}_inner_double" reduction="1.0"/>
+        <xacro:gripper_virtual_transmission name="${name}_fingertip_1" reduction="1.0"/>
+        <xacro:gripper_virtual_transmission name="${name}_fingertip_2" reduction="1.0"/>
+        <xacro:gripper_virtual_transmission name="${name}_inner_single" reduction="1.0"/>
+        <xacro:gripper_virtual_transmission name="${name}_motor_single" reduction="1.0"/>
+        <xacro:gripper_virtual_transmission name="${name}_fingertip_3" reduction="1.0"/> -->
+  <link name="gripper_right_base_link">
+    <inertial>
+      <mass value="0.637534"/>
+      <inertia ixx="0.0005849692" ixy="4.37159e-06" ixz="-1.2589e-05" iyy="0.00067076439" iyz="2.824361e-05" izz="0.00085526645"/>
+      <origin rpy="0 0 0" xyz="2.8e-05 0.005394 -0.023654"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/base_link.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/base_link_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="gripper_right_base_link_joint" type="fixed">
+    <parent link="wrist_right_ft_tool_link"/>
+    <child link="gripper_right_base_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00000 -0.02875"/>
+  </joint>
+  <link name="gripper_right_motor_double_link">
+    <inertial>
+      <mass value="0.134356"/>
+      <inertia ixx="0.00013479919" ixy="6.184429e-05" ixz="-3.0798e-05" iyy="0.00019602207" iyz="4.707722e-05" izz="0.00024217879"/>
+      <origin rpy="0 0 0" xyz="0.019654 0.018572 -0.011998"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/gripper_motor_double.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="Red"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/gripper_motor_double_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="gripper_right_joint" type="revolute">
+    <parent link="gripper_right_base_link"/>
+    <child link="gripper_right_motor_double_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.02025 -0.03"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="1.0" lower="-0.959931088597" upper="0.0" velocity="1.0"/>
+    <dynamics damping="1.0" friction="1.0"/>
+  </joint>
+  <link name="gripper_right_inner_double_link">
+    <inertial>
+      <mass value="0.087986"/>
+      <inertia ixx="7.410642e-05" ixy="-7.4773e-07" ixz="1.01536e-06" iyy="0.0001279418" iyz="2.504083e-05" izz="0.00010495175"/>
+      <origin rpy="0 0 0" xyz="-0.013283 0.036852 -0.023153"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/inner_double.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="Red"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+      <mesh filename="package://talos_description/meshes/gripper/inner_double_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="gripper_right_inner_double_joint" type="fixed">
+    <parent link="gripper_right_base_link"/>
+    <child link="gripper_right_inner_double_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00525 -0.05598"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100.0" lower="-1.0471975512" upper="0.0" velocity="1.0"/>
+    <!-- <mimic joint="gripper_right_joint" multiplier="1.0" offset="0.0"/> -->
+  </joint>
+  <link name="gripper_right_fingertip_1_link">
+    <inertial>
+      <mass value="0.026301"/>
+      <inertia ixx="8e-06" ixy="0" ixz="0" iyy="8.69179e-06" iyz="-1.43612e-06" izz="2.3082e-06"/>
+      <origin rpy="0 0 0" xyz="0 0.004604 -0.002537"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/fingertip.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/fingertip_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="gripper_right_fingertip_1_joint" type="fixed">
+    <parent link="gripper_right_inner_double_link"/>
+    <child link="gripper_right_fingertip_1_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.03200 0.04589 -0.06553"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100.0" lower="0.0" upper="1.0471975512" velocity="1.0"/>
+    <!-- <mimic joint="gripper_right_joint" multiplier="-1.0" offset="0.0"/> -->
+  </joint>
+  <link name="gripper_right_fingertip_2_link">
+    <inertial>
+      <mass value="0.026301"/>
+      <inertia ixx="8e-06" ixy="0" ixz="0" iyy="8.69179e-06" iyz="-1.43612e-06" izz="2.3082e-06"/>
+      <origin rpy="0 0 0" xyz="0 0.004604 -0.002537"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/fingertip.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/fingertip_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="gripper_right_fingertip_2_joint" type="fixed">
+    <parent link="gripper_right_inner_double_link"/>
+    <child link="gripper_right_fingertip_2_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="-0.03200 0.04589 -0.06553"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100.0" lower="0.0" upper="1.0471975512" velocity="1.0"/>
+    <!-- <mimic joint="gripper_right_joint" multiplier="-1.0" offset="0.0"/> -->
+  </joint>
+  <link name="gripper_right_motor_single_link">
+    <inertial>
+      <mass value="0.107923"/>
+      <inertia ixx="8.973662e-05" ixy="-4.082027e-05" ixz="-1.927099e-05" iyy="0.00011957255" iyz="-2.873284e-05" izz="0.00015469079"/>
+      <origin rpy="0 0 0" xyz="0.025237 -0.011231 -0.008158"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/gripper_motor_single.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="Red"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/gripper_motor_single_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="gripper_right_motor_single_joint" type="fixed">
+    <parent link="gripper_right_base_link"/>
+    <child link="gripper_right_motor_single_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 -0.02025 -0.03000"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100.0" lower="0.0" upper="1.0471975512" velocity="1.0"/>
+    <!-- <mimic joint="gripper_right_joint" multiplier="-1.0" offset="0.0"/> -->
+  </joint>
+  <link name="gripper_right_inner_single_link">
+    <inertial>
+      <mass value="0.047177"/>
+      <inertia ixx="4.199818e-05" ixy="1.3418e-07" ixz="1.9358e-07" iyy="3.231338e-05" iyz="-1.509136e-05" izz="2.168842e-05"/>
+      <origin rpy="0 0 0" xyz="0 -0.034565 -0.021412"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/inner_single.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="Red"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/inner_single_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="gripper_right_inner_single_joint" type="fixed">
+    <parent link="gripper_right_base_link"/>
+    <child link="gripper_right_inner_single_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 -0.00525 -0.05598"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100.0" lower="0.0" upper="1.0471975512" velocity="1.0"/>
+    <!-- <mimic joint="gripper_right_joint" multiplier="-1.0" offset="0.0"/> -->
+  </joint>
+  <link name="gripper_right_fingertip_3_link">
+    <inertial>
+      <mass value="0.026301"/>
+      <inertia ixx="8e-06" ixy="0" ixz="0" iyy="8.69179e-06" iyz="-1.43612e-06" izz="2.3082e-06"/>
+      <origin rpy="0 0 0" xyz="0 0.004604 -0.002537"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/fingertip.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/fingertip_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="gripper_right_fingertip_3_joint" type="fixed">
+    <parent link="gripper_right_inner_single_link"/>
+    <child link="gripper_right_fingertip_3_link"/>
+    <origin rpy="0.0 0.0 3.14159265359" xyz="0.00000 -0.04589 -0.06553"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100.0" lower="0.0" upper="1.0471975512" velocity="1.0"/>
+    <!-- <mimic joint="gripper_right_joint" multiplier="-1.0" offset="0.0"/> -->
+  </joint>
+  <!-- virtual mimic joints -->
+  <!--         <xacro:gripper_virtual_transmission name="${name}_inner_double" reduction="1.0"/>
+        <xacro:gripper_virtual_transmission name="${name}_fingertip_1" reduction="1.0"/>
+        <xacro:gripper_virtual_transmission name="${name}_fingertip_2" reduction="1.0"/>
+        <xacro:gripper_virtual_transmission name="${name}_inner_single" reduction="1.0"/>
+        <xacro:gripper_virtual_transmission name="${name}_motor_single" reduction="1.0"/>
+        <xacro:gripper_virtual_transmission name="${name}_fingertip_3" reduction="1.0"/> -->
+  <!-- Joint hip_z (mesh link) : child link hip_x -> parent link base_link -->
+  <link name="leg_left_1_link">
+    <inertial>
+      <mass value="1.845591"/>
+      <inertia ixx="0.00541533521" ixy="-0.00015428714" ixz="0.00131612622" iyy="0.00848624936" iyz="-1.906103e-05" izz="0.00474512541"/>
+      <origin rpy="0 0 0" xyz="0.02247868 0.00106736 0.03130665"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.01 0.01 0.01"/>
+      <geometry>
+        <box size="0.2 0.12 0.13"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.01 0.01 0.01"/>
+      <geometry>
+        <box size="0.2 0.12 0.13"/>
+        <!-- <mesh filename="package://talos_description/meshes/v2/hip_z_collision.stl" scale="1 1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="leg_left_1_joint" type="revolute">
+    <parent link="base_link"/>
+    <child link="leg_left_1_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="-0.02 0.085 -0.27105"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="100" lower="-0.349065850399" upper="1.57079632679" velocity="3.87"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Joint hip_x (mesh link) : child link hip_y -> parent link hip_z -->
+  <link name="leg_left_2_link">
+    <inertial>
+      <mass value="1.490952"/>
+      <inertia ixx="0.00497152259" ixy="-0.0001311358" ixz="-4.86648e-06" iyy="0.00190550993" iyz="4.585381e-05" izz="0.00474289754"/>
+      <origin rpy="0 0 0" xyz="-0.00704703 0.02592659 0.00273385"/>
+    </inertial>
+    <visual>
+      <origin rpy="0  0  0" xyz="0.0 0.06 0.015"/>
+      <geometry>
+        <box size="0.13 0.28 0.14"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0  0  0" xyz="0.0 0.06 0.015"/>
+      <geometry>
+        <box size="0.13 0.28 0.14"/>
+        <!-- <mesh filename="package://talos_description/meshes/v2/hip_x_collision.stl" scale="1 1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="leg_left_2_joint" type="revolute">
+    <parent link="leg_left_1_link"/>
+    <child link="leg_left_2_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00000 0.00000"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="160" lower="-0.5236" upper="0.5236" velocity="5.8"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Joint hip_y (mesh link) : child link knee -> parent link hip_x -->
+  <link name="leg_left_3_link">
+    <inertial>
+      <mass value="6.239871"/>
+      <inertia ixx="0.15337403915" ixy="0.00068913396" ixz="0.00168034366" iyy="0.13648139346" iyz="-0.02187635445" izz="0.03001594885"/>
+      <origin rpy="0 0 0" xyz="0.0058523 0.0636967 -0.18339564"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0 0.06 -0.25"/>
+      <geometry>
+        <box size="0.15 0.27 0.39"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.0 0.06 -0.25"/>
+      <geometry>
+        <box size="0.15 0.27 0.39"/>
+        <!-- <mesh filename="package://talos_description/meshes/v2/hip_y_collision.stl" scale="1 1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="leg_left_3_joint" type="revolute">
+    <parent link="leg_left_2_link"/>
+    <child link="leg_left_3_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00000 0.00000"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="160" lower="-2.095" upper="0.7" velocity="5.8"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Joint knee (mesh link) : child link ankle_y -> parent link hip_y -->
+  <link name="leg_left_4_link">
+    <inertial>
+      <mass value="3.759951"/>
+      <inertia ixx="0.04287677484" ixy="0.00043705397" ixz="0.00078292368" iyy="0.03598906971" iyz="0.00037717694" izz="0.01364791564"/>
+      <origin rpy="0 0 0" xyz="0.01317717 0.02917508 -0.11594602"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.01 0.04 -0.17"/>
+      <geometry>
+        <box size="0.12 0.22 0.21"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.01 0.04 -0.17"/>
+      <geometry>
+        <box size="0.12 0.22 0.21"/>
+        <!-- <mesh filename="package://talos_description/meshes/v2/knee_collision.stl" scale="1 1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="leg_left_4_joint" type="revolute">
+    <parent link="leg_left_3_link"/>
+    <child link="leg_left_4_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00000 -0.38000"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="400" lower="0" upper="2.618" velocity="7"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Joint ankle_y (mesh link) : child link ankle_x -> parent link knee -->
+  <link name="leg_left_5_link">
+    <inertial>
+      <mass value="1.29096"/>
+      <inertia ixx="0.01143430819" ixy="0.00096978273" ixz="0.00039976581" iyy="0.00888974312" iyz="-0.00277808667" izz="0.00506373875"/>
+      <origin rpy="0 0 0" xyz="-0.01400838 0.04180064 0.03820186"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="-0.02 0.04 0.0"/>
+      <geometry>
+        <box size="0.14 0.22 0.1"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.02 0.04 0.0"/>
+      <geometry>
+        <box size="0.14 0.22 0.1"/>
+        <!-- <mesh filename="package://talos_description/meshes/v2/ankle_Y_collision.stl" scale="1 1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="leg_left_5_joint" type="revolute">
+    <parent link="leg_left_4_link"/>
+    <child link="leg_left_5_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00000 -0.32500"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="160" lower="-1.27" upper="0.68" velocity="5.8"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Joint ankle_x (mesh link) : child link None -> parent link ankle_y -->
+  <link name="leg_left_6_link">
+    <inertial>
+      <mass value="1.597773"/>
+      <inertia ixx="0.00439027267" ixy="-6.743674e-05" ixz="0.00113816363" iyy="0.00743063029" iyz="3.943143e-05" izz="0.00550906699"/>
+      <origin rpy="0 0 0" xyz="-0.02034668 -0.00051514 -0.05987428"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 -0.03"/>
+      <geometry>
+        <box size="0.221 0.134 0.16"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -0.03"/>
+      <geometry>
+            <box size="0.221 0.134 0.16"/>
+<!--        <mesh filename="package://talos_description/meshes/v2/ankle_X_collision.glb" scale="1 -1 1"/>-->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="leg_left_6_joint" type="revolute">
+    <parent link="leg_left_5_link"/>
+    <child link="leg_left_6_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00000 0.00000"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100" lower="-0.5236" upper="0.5236" velocity="4.8"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- from here is copy paste -->
+  <link name="left_sole_link">
+    <inertial>
+      <mass value="0.01"/>
+      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </inertial>
+  </link>
+  <joint name="leg_left_sole_fix_joint" type="fixed">
+    <parent link="leg_left_6_link"/>
+    <child link="left_sole_link"/>
+    <origin rpy="0 0 0" xyz="0.00 0.00 -0.107"/>
+    <axis xyz="1 0 0"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Joint hip_z (mesh link) : child link hip_x -> parent link base_link -->
+  <link name="leg_right_1_link">
+    <inertial>
+      <mass value="1.845591"/>
+      <inertia ixx="0.00541533521" ixy="0.00015428714" ixz="0.00131612622" iyy="0.00848624936" iyz="1.906103e-05" izz="0.00474512541"/>
+      <origin rpy="0 0 0" xyz="0.02247868 -0.00106736 0.03130665"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.01 -0.01 0.01"/>
+      <geometry>
+        <box size="0.2 0.12 0.13"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.01 -0.01 0.01"/>
+      <geometry>
+        <box size="0.2 0.12 0.13"/>
+        <!-- <mesh filename="package://talos_description/meshes/v2/hip_z_collision.stl" scale="1 -1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="leg_right_1_joint" type="revolute">
+    <parent link="base_link"/>
+    <child link="leg_right_1_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="-0.02 -0.085 -0.27105"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="100" lower="-1.57079632679" upper="0.349065850399" velocity="3.87"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Joint hip_x (mesh link) : child link hip_y -> parent link hip_z -->
+  <link name="leg_right_2_link">
+    <inertial>
+      <mass value="1.490952"/>
+      <inertia ixx="0.00497152259" ixy="0.0001311358" ixz="-4.86648e-06" iyy="0.00190550993" iyz="-4.585381e-05" izz="0.00474289754"/>
+      <origin rpy="0 0 0" xyz="-0.00704703 -0.02592659 0.00273385"/>
+    </inertial>
+    <visual>
+      <origin rpy="0  0  0" xyz="0.0 -0.06 0.015"/>
+      <geometry>
+        <box size="0.13 0.28 0.14"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0  0  0" xyz="0.0 -0.06 0.015"/>
+      <geometry>
+        <box size="0.13 0.28 0.14"/>
+        <!-- <mesh filename="package://talos_description/meshes/v2/hip_x_collision.stl" scale="1 -1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="leg_right_2_joint" type="revolute">
+    <parent link="leg_right_1_link"/>
+    <child link="leg_right_2_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00000 0.00000"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="160" lower="-0.5236" upper="0.5236" velocity="5.8"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Joint hip_y (mesh link) : child link knee -> parent link hip_x -->
+  <link name="leg_right_3_link">
+    <inertial>
+      <mass value="6.239871"/>
+      <inertia ixx="0.15337403915" ixy="-0.00068913396" ixz="0.00168034366" iyy="0.13648139346" iyz="0.02187635445" izz="0.03001594885"/>
+      <origin rpy="0 0 0" xyz="0.0058523 -0.0636967 -0.18339564"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0 -0.06 -0.25"/>
+      <geometry>
+        <box size="0.15 0.27 0.39"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.0 -0.06 -0.25"/>
+      <geometry>
+        <box size="0.15 0.27 0.39"/>
+        <!-- <mesh filename="package://talos_description/meshes/v2/hip_y_collision.stl" scale="1 -1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="leg_right_3_joint" type="revolute">
+    <parent link="leg_right_2_link"/>
+    <child link="leg_right_3_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00000 0.00000"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="160" lower="-2.095" upper="0.7" velocity="5.8"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Joint knee (mesh link) : child link ankle_y -> parent link hip_y -->
+  <link name="leg_right_4_link">
+    <inertial>
+      <mass value="3.759951"/>
+      <inertia ixx="0.04287677484" ixy="-0.00043705397" ixz="0.00078292368" iyy="0.03598906971" iyz="-0.00037717694" izz="0.01364791564"/>
+      <origin rpy="0 0 0" xyz="0.01317717 -0.02917508 -0.11594602"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.01 -0.04 -0.17"/>
+      <geometry>
+        <box size="0.12 -0.22 0.21"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.01 -0.04 -0.17"/>
+      <geometry>
+        <box size="0.12 -0.22 0.21"/>
+        <!-- <mesh filename="package://talos_description/meshes/v2/knee_collision.stl" scale="1 -1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="leg_right_4_joint" type="revolute">
+    <parent link="leg_right_3_link"/>
+    <child link="leg_right_4_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00000 -0.38000"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="400" lower="0" upper="2.618" velocity="7"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Joint ankle_y (mesh link) : child link ankle_x -> parent link knee -->
+  <link name="leg_right_5_link">
+    <inertial>
+      <mass value="1.29096"/>
+      <inertia ixx="0.01143430819" ixy="-0.00096978273" ixz="0.00039976581" iyy="0.00888974312" iyz="0.00277808667" izz="0.00506373875"/>
+      <origin rpy="0 0 0" xyz="-0.01400838 -0.04180064 0.03820186"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="-0.02 -0.04 0.0"/>
+      <geometry>
+        <box size="0.14 0.22 0.1"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.02 -0.04 0.0"/>
+      <geometry>
+        <box size="0.14 0.22 0.1"/>
+        <!-- <mesh filename="package://talos_description/meshes/v2/ankle_Y_collision.stl" scale="1 -1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="leg_right_5_joint" type="revolute">
+    <parent link="leg_right_4_link"/>
+    <child link="leg_right_5_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00000 -0.32500"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="160" lower="-1.27" upper="0.68" velocity="5.8"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Joint ankle_x (mesh link) : child link None -> parent link ankle_y -->
+  <link name="leg_right_6_link">
+    <inertial>
+      <mass value="1.597773"/>
+      <inertia ixx="0.00439027267" ixy="6.743674e-05" ixz="0.00113816363" iyy="0.00743063029" iyz="-3.943143e-05" izz="0.00550906699"/>
+      <origin rpy="0 0 0" xyz="-0.02034668 0.00051514 -0.05987428"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 -0.03"/>
+      <geometry>
+        <box size="0.221 0.134 0.16"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -0.03"/>
+      <geometry>
+            <box size="0.221 0.134 0.16"/>
+<!--        <mesh filename="package://talos_description/meshes/v2/ankle_X_collision.glb" scale="1 -1 1"/>-->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="leg_right_6_joint" type="revolute">
+    <parent link="leg_right_5_link"/>
+    <child link="leg_right_6_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00000 0.00000"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100" lower="-0.5236" upper="0.5236" velocity="4.8"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- from here is copy paste -->
+  <link name="right_sole_link">
+    <inertial>
+      <mass value="0.01"/>
+      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </inertial>
+  </link>
+  <joint name="leg_right_sole_fix_joint" type="fixed">
+    <parent link="leg_right_6_link"/>
+    <child link="right_sole_link"/>
+    <origin rpy="0 0 0" xyz="0.00 0.00 -0.107"/>
+    <axis xyz="1 0 0"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <material name="Blue">
+    <color rgba="0.0 0.0 0.8 1.0"/>
+  </material>
+  <material name="Green">
+    <color rgba="0.0 0.8 0.0 1.0"/>
+  </material>
+  <material name="Grey">
+    <color rgba="0.7 0.7 0.7 1.0"/>
+  </material>
+  <material name="LightGrey">
+    <color rgba="0.9 0.9 0.9 1.0"/>
+  </material>
+  <material name="DarkGrey">
+    <color rgba="0.2 0.2 0.2 1.0"/>
+  </material>
+  <material name="Red">
+    <color rgba="0.8 0.0 0.0 1.0"/>
+  </material>
+  <material name="White">
+    <color rgba="1.0 1.0 1.0 1.0"/>
+  </material>
+  <material name="Black">
+    <color rgba="0.0 0.0 0.0 1.0"/>
+  </material>
+</robot>
+
diff --git a/utheque/talos/talos_fast_collision.urdf b/utheque/talos/talos_fast_collision.urdf
new file mode 100644
index 00000000..4765a3a9
--- /dev/null
+++ b/utheque/talos/talos_fast_collision.urdf
@@ -0,0 +1,1752 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from talos_full_v2.urdf.xacro       | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<!--
+
+  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="talos" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor">
+  <!--************************-->
+  <!--        TORSO_2  (TILT) -->
+  <!--************************-->
+  <link name="torso_2_link">
+    <inertial>
+      <mass value="16.97403"/>
+      <inertia ixx="0.44372633826" ixy="0.00069132133" ixz="-0.01218206353" iyy="0.2998576068" iyz="-0.00019623338" izz="0.32201554742"/>
+      <origin rpy="0 0 0" xyz="-0.0463563 -0.00099023 0.1452805"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/torso/torso_2.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.06 0 0.17"/>
+      <geometry>
+        <box size="0.29 0.45 0.45"/>
+        <!-- <mesh filename="package://talos_description/meshes/torso/torso_2_collision.glb" scale="1 1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <!--************************-->
+  <!--        TORSO_1  (PAN)  -->
+  <!--************************-->
+  <link name="torso_1_link">
+    <inertial>
+      <mass value="2.294658"/>
+      <inertia ixx="0.00638508087" ixy="-7.107e-08" ixz="-3.065592e-05" iyy="0.00410256102" iyz="-1.46946e-06" izz="0.00622968815"/>
+      <origin rpy="0 0 0" xyz="0.00078223 3.528e-05 -0.01782457"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/torso/torso_1.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/torso/torso_1.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="torso_1_joint" type="revolute">
+    <parent link="base_link"/>
+    <child link="torso_1_link"/>
+    <!-- XXX AS: The kinematic chain order for the torso was reversed manually. This value does not match the CAD model. XXX -->
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0722"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="200.0" lower="-1.25663706144" upper="1.25663706144" velocity="5.4"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <!-- <safety_controller k_position="20"
+                         k_velocity="20"
+                         soft_lower_limit="${-15.0 * deg_to_rad + torso_eps}"
+                         soft_upper_limit="${ 45.0 * deg_to_rad - torso_eps}" /> -->
+  </joint>
+  <!--************************-->
+  <!--        BASE_LINK       -->
+  <!--************************-->
+  <link name="base_link">
+    <inertial>
+      <mass value="15.36284"/>
+      <inertia ixx="0.20105075811" ixy="0.00023244734" ixz="0.0040167728" iyy="0.08411496729" iyz="-0.00087206649" izz="0.2318908414"/>
+      <origin rpy="0 0 0" xyz="-0.05709419 0.00153054 -0.0762521"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/torso/base_link.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.04 0 -0.085"/>
+      <geometry>
+        <box size="0.32 0.45 0.2"/>
+        <!-- <mesh filename="package://talos_description/meshes/torso/base_link_collision.glb" scale="1 1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="torso_2_joint" type="revolute">
+    <parent link="torso_1_link"/>
+    <child link="torso_2_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="200.0" lower="-0.226892802759" upper="0.733038285838" velocity="5.4"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <!-- <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>
+  <link name="imu_link">
+    <inertial>
+      <mass value="0.01"/>
+      <inertia ixx="1e-06" ixy="0" ixz="0" iyy="1e-06" iyz="0" izz="1e-06"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+      <material name="White"/>
+      <!-- Not sure about this -->
+    </visual>
+  </link>
+  <joint name="imu_joint" type="fixed">
+    <origin rpy="3.14159265359 0 1.57079632679" xyz="0.04925 0 0.078"/>
+    <parent link="torso_2_link"/>
+    <child link="imu_link"/>
+  </joint>
+  <link name="head_1_link">
+    <inertial>
+      <mass value="0.73746"/>
+      <inertia ixx="0.00224878584" ixy="4.69375e-06" ixz="8.55557e-05" iyy="0.00111158492" iyz="-4.132536e-05" izz="0.00205225921"/>
+      <origin rpy="0 0 0" xyz="-0.00157211 -0.00157919 0.02175767"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/head/head_1.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/head/head_1_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="head_1_joint" type="revolute">
+    <parent link="torso_2_link"/>
+    <child link="head_1_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.02000 0.00000 0.32100"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="8.0" lower="-0.209439510239" upper="0.785398163397" velocity="3.0"/>
+    <dynamics damping="0.5" friction="1.0"/>
+    <!-- <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>
+  <link name="head_2_link">
+    <inertial>
+      <mass value="1.443954"/>
+      <inertia ixx="0.01084624339" ixy="1.050889e-05" ixz="0.00041594252" iyy="0.0109569176" iyz="2.367831e-05" izz="0.00571698895"/>
+      <origin rpy="0 0 0" xyz="0.01002657 5.218e-05 0.14136068"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/head/head_2.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.01 0 0.18"/>
+      <geometry>
+        <box size="0.23 0.21 0.21"/>
+        <!-- <mesh filename="package://talos_description/meshes/head/head_2_default_collision.stl" scale="1 1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="head_2_joint" type="revolute">
+    <parent link="head_1_link"/>
+    <child link="head_2_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00000 0.00000"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="4.0" lower="-1.308996939" upper="1.308996939" velocity="3.0"/>
+    <dynamics damping="0.5" friction="1.0"/>
+    <!-- <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>
+  <!-- frames in the center of the camera -->
+  <joint name="rgbd_joint" type="fixed">
+    <origin rpy="0.0 0.0 0.0" xyz="0.066 0.0 0.1982"/>
+    <axis xyz="0 0 1"/>
+    <!-- <limit lower="0" upper="0.001" effort="100" velocity="0.01"/> -->
+    <parent link="head_2_link"/>
+    <child link="rgbd_link"/>
+  </joint>
+  <link name="rgbd_link">
+    <inertial>
+      <mass value="0.01"/>
+      <inertia ixx="3e-05" ixy="0" ixz="0" iyy="2e-06" iyz="0" izz="3e-05"/>
+      <origin rpy="0 0 0" xyz="-0.01 0.0025 0"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/sensors/orbbec/orbbec.glb"/>
+      </geometry>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="-0.01 0.0025 0"/>
+      <geometry>
+        <box size="0.04 0.185 0.03"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="rgbd_optical_joint" type="fixed">
+    <origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 0 0"/>
+    <parent link="rgbd_link"/>
+    <child link="rgbd_optical_frame"/>
+  </joint>
+  <link name="rgbd_optical_frame">
+    <inertial>
+      <mass value="0.01"/>
+      <inertia ixx="1e-06" ixy="0" ixz="0" iyy="1e-06" iyz="0" izz="1e-06"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </inertial>
+  </link>
+  <!-- frames of the depth sensor -->
+  <joint name="rgbd_depth_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0.0 0.03751 0.0"/>
+    <parent link="rgbd_link"/>
+    <child link="rgbd_depth_frame"/>
+  </joint>
+  <link name="rgbd_depth_frame">
+    <inertial>
+      <mass value="0.01"/>
+      <inertia ixx="1e-06" ixy="0" ixz="0" iyy="1e-06" iyz="0" izz="1e-06"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </inertial>
+  </link>
+  <joint name="rgbd_depth_optical_joint" type="fixed">
+    <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
+    <parent link="rgbd_depth_frame"/>
+    <child link="rgbd_depth_optical_frame"/>
+  </joint>
+  <link name="rgbd_depth_optical_frame">
+    <inertial>
+      <mass value="0.01"/>
+      <inertia ixx="1e-06" ixy="0" ixz="0" iyy="1e-06" iyz="0" izz="1e-06"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </inertial>
+  </link>
+  <!-- frames of the rgb sensor -->
+  <joint name="rgbd_rgb_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0.0 0.01251 0.0"/>
+    <parent link="rgbd_link"/>
+    <child link="rgbd_rgb_frame"/>
+  </joint>
+  <link name="rgbd_rgb_frame">
+    <inertial>
+      <mass value="0.01"/>
+      <inertia ixx="1e-06" ixy="0" ixz="0" iyy="1e-06" iyz="0" izz="1e-06"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </inertial>
+  </link>
+  <joint name="rgbd_rgb_optical_joint" type="fixed">
+    <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
+    <parent link="rgbd_rgb_frame"/>
+    <child link="rgbd_rgb_optical_frame"/>
+  </joint>
+  <link name="rgbd_rgb_optical_frame">
+    <inertial>
+      <mass value="0.01"/>
+      <inertia ixx="1e-06" ixy="0" ixz="0" iyy="1e-06" iyz="0" izz="1e-06"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </inertial>
+  </link>
+  <!--************************-->
+  <!--        SHOULDER        -->
+  <!--************************-->
+  <link name="arm_left_1_link">
+    <inertial>
+      <mass value="2.714567"/>
+      <inertia ixx="0.01237818683" ixy="-3.625571e-05" ixz="7.14472e-05" iyy="0.004191372" iyz="-0.00023639064" izz="0.01358161109"/>
+      <origin rpy="0 0 0" xyz="-0.0002762 0.10060223 0.04437419"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/arm/arm_1.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.0 0.14 0.04"/>
+      <geometry>
+        <box size="0.2 0.12 0.14"/>
+        <!-- <mesh filename="package://talos_description/meshes/arm/arm_1_collision.glb" scale="1 1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_left_1_joint" type="revolute">
+    <parent link="torso_2_link"/>
+    <child link="arm_left_1_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.1575 0.23200"/>
+    <axis xyz="0 0 1"/>
+    <!-- v1 TALOS has 1st arm left joint (-90,30) and right joint (-30,90) -->
+    <!-- v2 TALOS has 1st arm left joint (-90,50) and right joint (-50,90) -->
+    <limit effort="100.0" lower="-1.57079632679" upper="0.785398163397" velocity="2.7"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <!--        <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="arm_left_2_link">
+    <inertial>
+      <mass value="2.425086"/>
+      <inertia ixx="0.01297822101" ixy="1.208791e-05" ixz="-0.00320370433" iyy="0.01380870278" iyz="-0.00012770059" izz="0.00478856621"/>
+      <origin rpy="0 0 0" xyz="0.01438831 0.00092938 -0.08684268"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/arm/arm_2.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.01 0.0 -0.1"/>
+      <geometry>
+        <box size="0.16 0.12 0.12"/>
+        <!-- <mesh filename="package://talos_description/meshes/arm/arm_2_collision.glb" scale="1 1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_left_2_joint" type="revolute">
+    <parent link="arm_left_1_link"/>
+    <child link="arm_left_2_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00493 0.1365 0.04673"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100.0" lower="0.00872664625997" upper="2.87106661953" velocity="3.66"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <!--        <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="arm_left_3_link">
+    <inertial>
+      <mass value="2.208741"/>
+      <inertia ixx="0.00718831493" ixy="-0.00011563551" ixz="0.00075969733" iyy="0.00693528503" iyz="0.00042134743" izz="0.00388359007"/>
+      <origin rpy="0 0 0" xyz="0.0136084 0.01241619 -0.2499004"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/arm/arm_3.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.01 0.01 -0.25"/>
+      <geometry>
+        <box size="0.14 0.16 0.17"/>
+        <!-- <mesh filename="package://talos_description/meshes/arm/arm_3_collision.glb" scale="1 1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_left_3_joint" type="revolute">
+    <parent link="arm_left_2_link"/>
+    <child link="arm_left_3_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00000 0.00000"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="70.0" lower="-2.42600766027" upper="2.42600766027" velocity="4.58"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <!--       <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="arm_left_4_link">
+    <inertial>
+      <mass value="0.877346"/>
+      <inertia ixx="0.00251207716" ixy="0.00010070062" ixz="-0.00032788214" iyy="0.00275869324" iyz="0.00040022227" izz="0.00120735959"/>
+      <origin rpy="0 0 0" xyz="-0.00742138 -0.0213895 -0.03312656"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/arm/arm_4.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.02 -0.01 -0.07"/>
+      <geometry>
+        <box size="0.1 0.11 0.07"/>
+        <!-- <mesh filename="package://talos_description/meshes/arm/arm_4_collision.glb" scale="1 1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_left_4_joint" type="revolute">
+    <parent link="arm_left_3_link"/>
+    <child link="arm_left_4_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.02000 0.00000 -0.27300"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="70.0" lower="-2.23402144255" upper="-0.00349065850399" velocity="4.58"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <!--       <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>
+  <!--************************-->
+  <!--        WRIST           -->
+  <!--************************-->
+  <link name="arm_left_5_link">
+    <inertial>
+      <mass value="1.877923"/>
+      <inertia ixx="0.00349507283" ixy="1.265489e-05" ixz="1.038286e-05" iyy="0.00436830072" iyz="-9.736042e-05" izz="0.0022826337"/>
+      <origin rpy="0 0 0" xyz="-6e-05 0.003262 0.079625"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/arm/arm_5.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.0 0.01 0.07"/>
+      <geometry>
+        <box size="0.16 0.12 0.19"/>
+        <!-- <mesh filename="package://talos_description/meshes/arm/arm_5_collision.glb" scale="1 1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_left_5_joint" type="revolute">
+    <parent link="arm_left_4_link"/>
+    <child link="arm_left_5_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="-0.02000 0.00000 -0.26430"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="20.0" lower="-2.51327412287" upper="2.51327412287" velocity="1.95"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <!--       <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="arm_left_6_link">
+    <inertial>
+      <mass value="0.40931"/>
+      <inertia ixx="0.00010700023" ixy="-8.899e-08" ixz="-4.392e-08" iyy="0.00014101316" iyz="4.1702e-07" izz="0.00015398658"/>
+      <origin rpy="0 0 0" xyz="2.1e-05 -0.001965 -0.000591"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/arm/arm_6.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/arm/arm_6_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="arm_left_6_joint" type="revolute">
+    <parent link="arm_left_5_link"/>
+    <child link="arm_left_6_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00000 0.00000"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="8.0" lower="-1.37008346282" upper="1.37008346282" velocity="1.76"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <!--       <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="arm_left_7_link">
+    <inertial>
+      <mass value="0.308441"/>
+      <inertia ixx="0.00030894317" ixy="-1.58687e-06" ixz="1.73418e-06" iyy="0.00021886181" iyz="-1.221167e-05" izz="0.00017519492"/>
+      <origin rpy="0 0 0" xyz="0.007525 0.001378 -0.02463"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/arm/arm_7.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -0.14"/>
+      <geometry>
+        <box size="0.12 0.17 0.23"/>
+        <!-- <mesh filename="package://talos_description/meshes/arm/arm_7_collision.glb" scale="1 1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_left_7_joint" type="revolute">
+    <parent link="arm_left_6_link"/>
+    <child link="arm_left_7_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="8.0" lower="-0.680678408278" upper="0.680678408278" velocity="1.76"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <!--       <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>
+  <!--************************-->
+  <!--        SHOULDER        -->
+  <!--************************-->
+  <link name="arm_right_1_link">
+    <inertial>
+      <mass value="2.714567"/>
+      <inertia ixx="0.01237818683" ixy="3.625571e-05" ixz="7.14472e-05" iyy="0.004191372" iyz="0.00023639064" izz="0.01358161109"/>
+      <origin rpy="0 0 0" xyz="-0.0002762 -0.10060223 0.04437419"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/arm/arm_1.glb" scale="1 -1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.0 -0.14 0.04"/>
+      <geometry>
+        <box size="0.2 0.12 0.14"/>
+        <!-- <mesh filename="package://talos_description/meshes/arm/arm_1_collision.glb" scale="1 -1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_right_1_joint" type="revolute">
+    <parent link="torso_2_link"/>
+    <child link="arm_right_1_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 -0.1575 0.23200"/>
+    <axis xyz="0 0 1"/>
+    <!-- v1 TALOS has 1st arm left joint (-90,30) and right joint (-30,90) -->
+    <!-- v2 TALOS has 1st arm left joint (-90,50) and right joint (-50,90) -->
+    <limit effort="100.0" lower="-0.785398163397" upper="1.57079632679" velocity="2.7"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <!--        <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="arm_right_2_link">
+    <inertial>
+      <mass value="2.425086"/>
+      <inertia ixx="0.01297822101" ixy="-1.208791e-05" ixz="-0.00320370433" iyy="0.01380870278" iyz="0.00012770059" izz="0.00478856621"/>
+      <origin rpy="0 0 0" xyz="0.01438831 -0.00092938 -0.08684268"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/arm/arm_2.glb" scale="1 -1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.01 0.0 -0.1"/>
+      <geometry>
+        <box size="0.16 0.12 0.12"/>
+        <!-- <mesh filename="package://talos_description/meshes/arm/arm_2_collision.glb" scale="1 -1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_right_2_joint" type="revolute">
+    <parent link="arm_right_1_link"/>
+    <child link="arm_right_2_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00493 -0.1365 0.04673"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100.0" lower="-2.87106661953" upper="-0.00872664625997" velocity="3.66"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <!--        <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="arm_right_3_link">
+    <inertial>
+      <mass value="2.208741"/>
+      <inertia ixx="0.00718831493" ixy="0.00011563551" ixz="0.00075969733" iyy="0.00693528503" iyz="-0.00042134743" izz="0.00388359007"/>
+      <origin rpy="0 0 0" xyz="0.0136084 -0.01241619 -0.2499004"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/arm/arm_3.glb" scale="1 -1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.01 -0.01 -0.25"/>
+      <geometry>
+        <box size="0.14 0.16 0.17"/>
+        <!-- <mesh filename="package://talos_description/meshes/arm/arm_3_collision.glb" scale="1 -1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_right_3_joint" type="revolute">
+    <parent link="arm_right_2_link"/>
+    <child link="arm_right_3_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00000 0.00000"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="70.0" lower="-2.42600766027" upper="2.42600766027" velocity="4.58"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <!--       <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="arm_right_4_link">
+    <inertial>
+      <mass value="0.877346"/>
+      <inertia ixx="0.00251207716" ixy="-0.00010070062" ixz="-0.00032788214" iyy="0.00275869324" iyz="-0.00040022227" izz="0.00120735959"/>
+      <origin rpy="0 0 0" xyz="-0.00742138 0.0213895 -0.03312656"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/arm/arm_4.glb" scale="1 -1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.02 0.01 -0.07"/>
+      <geometry>
+        <box size="0.1 0.11 0.07"/>
+        <!-- <mesh filename="package://talos_description/meshes/arm/arm_4_collision.glb" scale="1 -1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_right_4_joint" type="revolute">
+    <parent link="arm_right_3_link"/>
+    <child link="arm_right_4_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.02000 0.00000 -0.27300"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="70.0" lower="-2.23402144255" upper="-0.00349065850399" velocity="4.58"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <!--       <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>
+  <!--************************-->
+  <!--        WRIST           -->
+  <!--************************-->
+  <link name="arm_right_5_link">
+    <inertial>
+      <mass value="1.877923"/>
+      <inertia ixx="0.00349507283" ixy="-1.265489e-05" ixz="1.038286e-05" iyy="0.00436830072" iyz="9.736042e-05" izz="0.0022826337"/>
+      <origin rpy="0 0 0" xyz="-6e-05 -0.003262 0.079625"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/arm/arm_5.glb" scale="1 -1 1"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.0 -0.01 0.07"/>
+      <geometry>
+        <box size="0.16 0.12 0.19"/>
+        <!-- <mesh filename="package://talos_description/meshes/arm/arm_5_collision.glb" scale="1 -1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_right_5_joint" type="revolute">
+    <parent link="arm_right_4_link"/>
+    <child link="arm_right_5_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="-0.02000 0.00000 -0.26430"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="20.0" lower="-2.51327412287" upper="2.51327412287" velocity="1.95"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <!--       <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="arm_right_6_link">
+    <inertial>
+      <mass value="0.40931"/>
+      <inertia ixx="0.00010700023" ixy="8.899e-08" ixz="-4.392e-08" iyy="0.00014101316" iyz="-4.1702e-07" izz="0.00015398658"/>
+      <origin rpy="0 0 0" xyz="2.1e-05 0.001965 -0.000591"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/arm/arm_6.glb" scale="1 -1 1"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/arm/arm_6_collision.glb" scale="1 -1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="arm_right_6_joint" type="revolute">
+    <parent link="arm_right_5_link"/>
+    <child link="arm_right_6_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00000 0.00000"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="8.0" lower="-1.37008346282" upper="1.37008346282" velocity="1.76"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <!--       <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="arm_right_7_link">
+    <inertial>
+      <mass value="0.308441"/>
+      <inertia ixx="0.00030894317" ixy="1.58687e-06" ixz="1.73418e-06" iyy="0.00021886181" iyz="1.221167e-05" izz="0.00017519492"/>
+      <origin rpy="0 0 0" xyz="0.007525 -0.001378 -0.02463"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/arm/arm_7.glb" scale="1 -1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -0.14"/>
+      <geometry>
+        <box size="0.12 0.17 0.23"/>
+        <!-- <mesh filename="package://talos_description/meshes/arm/arm_7_collision.glb" scale="1 -1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_right_7_joint" type="revolute">
+    <parent link="arm_right_6_link"/>
+    <child link="arm_right_7_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="8.0" lower="-0.680678408278" upper="0.680678408278" velocity="1.76"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <!--       <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>
+  <!--************************-->
+  <!--        ft sensor       -->
+  <!--************************-->
+  <link name="wrist_right_ft_link">
+    <inertial>
+      <mass value="0.095"/>
+      <inertia ixx="1.108502e-05" ixy="9.9077e-07" ixz="-2.3811e-07" iyy="1.092725e-05" iyz="2.3137e-07" izz="1.898772e-05"/>
+      <origin rpy="0 0 0" xyz="-0.001001 0.000995 -0.008108"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.0157" radius="0.0225"/>
+      </geometry>
+      <material name="LightGrey"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.0157" radius="0.0225"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="wrist_right_ft_joint" type="fixed">
+    <parent link="arm_right_7_link"/>
+    <child link="wrist_right_ft_link"/>
+    <origin rpy="0.0 0 -1.57079632679" xyz="0 0 -0.051"/>
+  </joint>
+  <!--***********************-->
+  <!--       FT TOOL         -->
+  <!--***********************-->
+  <link name="wrist_right_ft_tool_link">
+    <inertial>
+      <mass value="0.001"/>
+      <inertia ixx="1e-06" ixy="0" ixz="0" iyy="1e-06" iyz="0" izz="1e-06"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0.0 0" xyz="0.0 0 0"/>
+      <geometry>
+        <cylinder length="0.00975" radius="0.025"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0.0 0" xyz="0.0 0 0"/>
+      <geometry>
+        <cylinder length="0.00975" radius="0.025"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="wrist_right_tool_joint" type="fixed">
+    <parent link="wrist_right_ft_link"/>
+    <child link="wrist_right_ft_tool_link"/>
+    <origin rpy="0 0 -1.57079632679" xyz="0 0 -0.012725"/>
+  </joint>
+  <!--************************-->
+  <!--        ft sensor       -->
+  <!--************************-->
+  <link name="wrist_left_ft_link">
+    <inertial>
+      <mass value="0.095"/>
+      <inertia ixx="1.108502e-05" ixy="9.9077e-07" ixz="-2.3811e-07" iyy="1.092725e-05" iyz="2.3137e-07" izz="1.898772e-05"/>
+      <origin rpy="0 0 0" xyz="-0.001001 0.000995 -0.008108"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.0157" radius="0.0225"/>
+      </geometry>
+      <material name="LightGrey"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.0157" radius="0.0225"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="wrist_left_ft_joint" type="fixed">
+    <parent link="arm_left_7_link"/>
+    <child link="wrist_left_ft_link"/>
+    <origin rpy="0.0 0 -4.71238898038" xyz="0 0 -0.051"/>
+  </joint>
+  <!--***********************-->
+  <!--       FT TOOL         -->
+  <!--***********************-->
+  <link name="wrist_left_ft_tool_link">
+    <inertial>
+      <mass value="0.001"/>
+      <inertia ixx="1e-06" ixy="0" ixz="0" iyy="1e-06" iyz="0" izz="1e-06"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0.0 0" xyz="0.0 0 0"/>
+      <geometry>
+        <cylinder length="0.00975" radius="0.025"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0.0 0" xyz="0.0 0 0"/>
+      <geometry>
+        <cylinder length="0.00975" radius="0.025"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="wrist_left_tool_joint" type="fixed">
+    <parent link="wrist_left_ft_link"/>
+    <child link="wrist_left_ft_tool_link"/>
+    <origin rpy="0 0 4.71238898038" xyz="0 0 -0.012725"/>
+  </joint>
+  <link name="gripper_left_base_link">
+    <inertial>
+      <mass value="0.637534"/>
+      <inertia ixx="0.0005849692" ixy="4.37159e-06" ixz="-1.2589e-05" iyy="0.00067076439" iyz="2.824361e-05" izz="0.00085526645"/>
+      <origin rpy="0 0 0" xyz="2.8e-05 0.005394 -0.023654"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/base_link.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/base_link_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="gripper_left_base_link_joint" type="fixed">
+    <parent link="wrist_left_ft_tool_link"/>
+    <child link="gripper_left_base_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00000 -0.02875"/>
+  </joint>
+  <link name="gripper_left_motor_double_link">
+    <inertial>
+      <mass value="0.134356"/>
+      <inertia ixx="0.00013479919" ixy="6.184429e-05" ixz="-3.0798e-05" iyy="0.00019602207" iyz="4.707722e-05" izz="0.00024217879"/>
+      <origin rpy="0 0 0" xyz="0.019654 0.018572 -0.011998"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/gripper_motor_double.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="Red"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/gripper_motor_double_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="gripper_left_joint" type="revolute">
+    <parent link="gripper_left_base_link"/>
+    <child link="gripper_left_motor_double_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.02025 -0.03"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="1.0" lower="-0.959931088597" upper="0.0" velocity="1.0"/>
+    <dynamics damping="1.0" friction="1.0"/>
+  </joint>
+  <link name="gripper_left_inner_double_link">
+    <inertial>
+      <mass value="0.087986"/>
+      <inertia ixx="7.410642e-05" ixy="-7.4773e-07" ixz="1.01536e-06" iyy="0.0001279418" iyz="2.504083e-05" izz="0.00010495175"/>
+      <origin rpy="0 0 0" xyz="-0.013283 0.036852 -0.023153"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/inner_double.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="Red"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/inner_double_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="gripper_left_inner_double_joint" type="fixed">
+    <parent link="gripper_left_base_link"/>
+    <child link="gripper_left_inner_double_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00525 -0.05598"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100.0" lower="-1.0471975512" upper="0.0" velocity="1.0"/>
+    <!-- <mimic joint="gripper_left_joint" multiplier="1.0" offset="0.0"/> -->
+  </joint>
+  <link name="gripper_left_fingertip_1_link">
+    <inertial>
+      <mass value="0.026301"/>
+      <inertia ixx="8e-06" ixy="0" ixz="0" iyy="8.69179e-06" iyz="-1.43612e-06" izz="2.3082e-06"/>
+      <origin rpy="0 0 0" xyz="0 0.004604 -0.002537"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/fingertip.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/fingertip_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="gripper_left_fingertip_1_joint" type="fixed">
+    <parent link="gripper_left_inner_double_link"/>
+    <child link="gripper_left_fingertip_1_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.03200 0.04589 -0.06553"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100.0" lower="0.0" upper="1.0471975512" velocity="1.0"/>
+    <!-- <mimic joint="gripper_left_joint" multiplier="-1.0" offset="0.0"/> -->
+  </joint>
+  <link name="gripper_left_fingertip_2_link">
+    <inertial>
+      <mass value="0.026301"/>
+      <inertia ixx="8e-06" ixy="0" ixz="0" iyy="8.69179e-06" iyz="-1.43612e-06" izz="2.3082e-06"/>
+      <origin rpy="0 0 0" xyz="0 0.004604 -0.002537"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/fingertip.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/fingertip_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="gripper_left_fingertip_2_joint" type="fixed">
+    <parent link="gripper_left_inner_double_link"/>
+    <child link="gripper_left_fingertip_2_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="-0.03200 0.04589 -0.06553"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100.0" lower="0.0" upper="1.0471975512" velocity="1.0"/>
+    <!-- <mimic joint="gripper_left_joint" multiplier="-1.0" offset="0.0"/> -->
+  </joint>
+  <link name="gripper_left_motor_single_link">
+    <inertial>
+      <mass value="0.107923"/>
+      <inertia ixx="8.973662e-05" ixy="-4.082027e-05" ixz="-1.927099e-05" iyy="0.00011957255" iyz="-2.873284e-05" izz="0.00015469079"/>
+      <origin rpy="0 0 0" xyz="0.025237 -0.011231 -0.008158"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/gripper_motor_single.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="Red"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/gripper_motor_single_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="gripper_left_motor_single_joint" type="fixed">
+    <parent link="gripper_left_base_link"/>
+    <child link="gripper_left_motor_single_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 -0.02025 -0.03000"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100.0" lower="0.0" upper="1.0471975512" velocity="1.0"/>
+    <!-- <mimic joint="gripper_left_joint" multiplier="-1.0" offset="0.0"/> -->
+  </joint>
+  <link name="gripper_left_inner_single_link">
+    <inertial>
+      <mass value="0.047177"/>
+      <inertia ixx="4.199818e-05" ixy="1.3418e-07" ixz="1.9358e-07" iyy="3.231338e-05" iyz="-1.509136e-05" izz="2.168842e-05"/>
+      <origin rpy="0 0 0" xyz="0 -0.034565 -0.021412"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/inner_single.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="Red"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/inner_single_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="gripper_left_inner_single_joint" type="fixed">
+    <parent link="gripper_left_base_link"/>
+    <child link="gripper_left_inner_single_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 -0.00525 -0.05598"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100.0" lower="0.0" upper="1.0471975512" velocity="1.0"/>
+    <!-- <mimic joint="gripper_left_joint" multiplier="-1.0" offset="0.0"/> -->
+  </joint>
+  <link name="gripper_left_fingertip_3_link">
+    <inertial>
+      <mass value="0.026301"/>
+      <inertia ixx="8e-06" ixy="0" ixz="0" iyy="8.69179e-06" iyz="-1.43612e-06" izz="2.3082e-06"/>
+      <origin rpy="0 0 0" xyz="0 0.004604 -0.002537"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/fingertip.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/fingertip_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="gripper_left_fingertip_3_joint" type="fixed">
+    <parent link="gripper_left_inner_single_link"/>
+    <child link="gripper_left_fingertip_3_link"/>
+    <origin rpy="0.0 0.0 3.14159265359" xyz="0.00000 -0.04589 -0.06553"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100.0" lower="0.0" upper="1.0471975512" velocity="1.0"/>
+    <!-- <mimic joint="gripper_left_joint" multiplier="-1.0" offset="0.0"/> -->
+  </joint>
+  <!-- virtual mimic joints -->
+  <!--         <xacro:gripper_virtual_transmission name="${name}_inner_double" reduction="1.0"/>
+        <xacro:gripper_virtual_transmission name="${name}_fingertip_1" reduction="1.0"/>
+        <xacro:gripper_virtual_transmission name="${name}_fingertip_2" reduction="1.0"/>
+        <xacro:gripper_virtual_transmission name="${name}_inner_single" reduction="1.0"/>
+        <xacro:gripper_virtual_transmission name="${name}_motor_single" reduction="1.0"/>
+        <xacro:gripper_virtual_transmission name="${name}_fingertip_3" reduction="1.0"/> -->
+  <link name="gripper_right_base_link">
+    <inertial>
+      <mass value="0.637534"/>
+      <inertia ixx="0.0005849692" ixy="4.37159e-06" ixz="-1.2589e-05" iyy="0.00067076439" iyz="2.824361e-05" izz="0.00085526645"/>
+      <origin rpy="0 0 0" xyz="2.8e-05 0.005394 -0.023654"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/base_link.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/base_link_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="gripper_right_base_link_joint" type="fixed">
+    <parent link="wrist_right_ft_tool_link"/>
+    <child link="gripper_right_base_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00000 -0.02875"/>
+  </joint>
+  <link name="gripper_right_motor_double_link">
+    <inertial>
+      <mass value="0.134356"/>
+      <inertia ixx="0.00013479919" ixy="6.184429e-05" ixz="-3.0798e-05" iyy="0.00019602207" iyz="4.707722e-05" izz="0.00024217879"/>
+      <origin rpy="0 0 0" xyz="0.019654 0.018572 -0.011998"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/gripper_motor_double.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="Red"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/gripper_motor_double_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="gripper_right_joint" type="revolute">
+    <parent link="gripper_right_base_link"/>
+    <child link="gripper_right_motor_double_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.02025 -0.03"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="1.0" lower="-0.959931088597" upper="0.0" velocity="1.0"/>
+    <dynamics damping="1.0" friction="1.0"/>
+  </joint>
+  <link name="gripper_right_inner_double_link">
+    <inertial>
+      <mass value="0.087986"/>
+      <inertia ixx="7.410642e-05" ixy="-7.4773e-07" ixz="1.01536e-06" iyy="0.0001279418" iyz="2.504083e-05" izz="0.00010495175"/>
+      <origin rpy="0 0 0" xyz="-0.013283 0.036852 -0.023153"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/inner_double.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="Red"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+      <mesh filename="package://talos_description/meshes/gripper/inner_double_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="gripper_right_inner_double_joint" type="fixed">
+    <parent link="gripper_right_base_link"/>
+    <child link="gripper_right_inner_double_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00525 -0.05598"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100.0" lower="-1.0471975512" upper="0.0" velocity="1.0"/>
+    <!-- <mimic joint="gripper_right_joint" multiplier="1.0" offset="0.0"/> -->
+  </joint>
+  <link name="gripper_right_fingertip_1_link">
+    <inertial>
+      <mass value="0.026301"/>
+      <inertia ixx="8e-06" ixy="0" ixz="0" iyy="8.69179e-06" iyz="-1.43612e-06" izz="2.3082e-06"/>
+      <origin rpy="0 0 0" xyz="0 0.004604 -0.002537"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/fingertip.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/fingertip_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="gripper_right_fingertip_1_joint" type="fixed">
+    <parent link="gripper_right_inner_double_link"/>
+    <child link="gripper_right_fingertip_1_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.03200 0.04589 -0.06553"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100.0" lower="0.0" upper="1.0471975512" velocity="1.0"/>
+    <!-- <mimic joint="gripper_right_joint" multiplier="-1.0" offset="0.0"/> -->
+  </joint>
+  <link name="gripper_right_fingertip_2_link">
+    <inertial>
+      <mass value="0.026301"/>
+      <inertia ixx="8e-06" ixy="0" ixz="0" iyy="8.69179e-06" iyz="-1.43612e-06" izz="2.3082e-06"/>
+      <origin rpy="0 0 0" xyz="0 0.004604 -0.002537"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/fingertip.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/fingertip_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="gripper_right_fingertip_2_joint" type="fixed">
+    <parent link="gripper_right_inner_double_link"/>
+    <child link="gripper_right_fingertip_2_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="-0.03200 0.04589 -0.06553"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100.0" lower="0.0" upper="1.0471975512" velocity="1.0"/>
+    <!-- <mimic joint="gripper_right_joint" multiplier="-1.0" offset="0.0"/> -->
+  </joint>
+  <link name="gripper_right_motor_single_link">
+    <inertial>
+      <mass value="0.107923"/>
+      <inertia ixx="8.973662e-05" ixy="-4.082027e-05" ixz="-1.927099e-05" iyy="0.00011957255" iyz="-2.873284e-05" izz="0.00015469079"/>
+      <origin rpy="0 0 0" xyz="0.025237 -0.011231 -0.008158"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/gripper_motor_single.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="Red"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/gripper_motor_single_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="gripper_right_motor_single_joint" type="fixed">
+    <parent link="gripper_right_base_link"/>
+    <child link="gripper_right_motor_single_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 -0.02025 -0.03000"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100.0" lower="0.0" upper="1.0471975512" velocity="1.0"/>
+    <!-- <mimic joint="gripper_right_joint" multiplier="-1.0" offset="0.0"/> -->
+  </joint>
+  <link name="gripper_right_inner_single_link">
+    <inertial>
+      <mass value="0.047177"/>
+      <inertia ixx="4.199818e-05" ixy="1.3418e-07" ixz="1.9358e-07" iyy="3.231338e-05" iyz="-1.509136e-05" izz="2.168842e-05"/>
+      <origin rpy="0 0 0" xyz="0 -0.034565 -0.021412"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/inner_single.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="Red"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/inner_single_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="gripper_right_inner_single_joint" type="fixed">
+    <parent link="gripper_right_base_link"/>
+    <child link="gripper_right_inner_single_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 -0.00525 -0.05598"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100.0" lower="0.0" upper="1.0471975512" velocity="1.0"/>
+    <!-- <mimic joint="gripper_right_joint" multiplier="-1.0" offset="0.0"/> -->
+  </joint>
+  <link name="gripper_right_fingertip_3_link">
+    <inertial>
+      <mass value="0.026301"/>
+      <inertia ixx="8e-06" ixy="0" ixz="0" iyy="8.69179e-06" iyz="-1.43612e-06" izz="2.3082e-06"/>
+      <origin rpy="0 0 0" xyz="0 0.004604 -0.002537"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/fingertip.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/gripper/fingertip_collision.glb" scale="1 1 1"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="gripper_right_fingertip_3_joint" type="fixed">
+    <parent link="gripper_right_inner_single_link"/>
+    <child link="gripper_right_fingertip_3_link"/>
+    <origin rpy="0.0 0.0 3.14159265359" xyz="0.00000 -0.04589 -0.06553"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100.0" lower="0.0" upper="1.0471975512" velocity="1.0"/>
+    <!-- <mimic joint="gripper_right_joint" multiplier="-1.0" offset="0.0"/> -->
+  </joint>
+  <!-- virtual mimic joints -->
+  <!--         <xacro:gripper_virtual_transmission name="${name}_inner_double" reduction="1.0"/>
+        <xacro:gripper_virtual_transmission name="${name}_fingertip_1" reduction="1.0"/>
+        <xacro:gripper_virtual_transmission name="${name}_fingertip_2" reduction="1.0"/>
+        <xacro:gripper_virtual_transmission name="${name}_inner_single" reduction="1.0"/>
+        <xacro:gripper_virtual_transmission name="${name}_motor_single" reduction="1.0"/>
+        <xacro:gripper_virtual_transmission name="${name}_fingertip_3" reduction="1.0"/> -->
+  <!-- Joint hip_z (mesh link) : child link hip_x -> parent link base_link -->
+  <link name="leg_left_1_link">
+    <inertial>
+      <mass value="1.845591"/>
+      <inertia ixx="0.00541533521" ixy="-0.00015428714" ixz="0.00131612622" iyy="0.00848624936" iyz="-1.906103e-05" izz="0.00474512541"/>
+      <origin rpy="0 0 0" xyz="0.02247868 0.00106736 0.03130665"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/v2/hip_z_lo_res.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.01 0.01 0.01"/>
+      <geometry>
+        <box size="0.2 0.12 0.13"/>
+        <!-- <mesh filename="package://talos_description/meshes/v2/hip_z_collision.stl" scale="1 1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="leg_left_1_joint" type="revolute">
+    <parent link="base_link"/>
+    <child link="leg_left_1_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="-0.02 0.085 -0.27105"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="100" lower="-0.349065850399" upper="1.57079632679" velocity="3.87"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Joint hip_x (mesh link) : child link hip_y -> parent link hip_z -->
+  <link name="leg_left_2_link">
+    <inertial>
+      <mass value="1.490952"/>
+      <inertia ixx="0.00497152259" ixy="-0.0001311358" ixz="-4.86648e-06" iyy="0.00190550993" iyz="4.585381e-05" izz="0.00474289754"/>
+      <origin rpy="0 0 0" xyz="-0.00704703 0.02592659 0.00273385"/>
+    </inertial>
+    <visual>
+      <origin rpy="0  0  0" xyz="0 0 0.0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/v2/hip_x_lo_res.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0  0  0" xyz="0.0 0.06 0.015"/>
+      <geometry>
+        <box size="0.13 0.28 0.14"/>
+        <!-- <mesh filename="package://talos_description/meshes/v2/hip_x_collision.stl" scale="1 1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="leg_left_2_joint" type="revolute">
+    <parent link="leg_left_1_link"/>
+    <child link="leg_left_2_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00000 0.00000"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="160" lower="-0.5236" upper="0.5236" velocity="5.8"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Joint hip_y (mesh link) : child link knee -> parent link hip_x -->
+  <link name="leg_left_3_link">
+    <inertial>
+      <mass value="6.239871"/>
+      <inertia ixx="0.15337403915" ixy="0.00068913396" ixz="0.00168034366" iyy="0.13648139346" iyz="-0.02187635445" izz="0.03001594885"/>
+      <origin rpy="0 0 0" xyz="0.0058523 0.0636967 -0.18339564"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/v2/hip_y_lo_res.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.0 0.06 -0.25"/>
+      <geometry>
+        <box size="0.15 0.27 0.39"/>
+        <!-- <mesh filename="package://talos_description/meshes/v2/hip_y_collision.stl" scale="1 1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="leg_left_3_joint" type="revolute">
+    <parent link="leg_left_2_link"/>
+    <child link="leg_left_3_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00000 0.00000"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="160" lower="-2.095" upper="0.7" velocity="5.8"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Joint knee (mesh link) : child link ankle_y -> parent link hip_y -->
+  <link name="leg_left_4_link">
+    <inertial>
+      <mass value="3.759951"/>
+      <inertia ixx="0.04287677484" ixy="0.00043705397" ixz="0.00078292368" iyy="0.03598906971" iyz="0.00037717694" izz="0.01364791564"/>
+      <origin rpy="0 0 0" xyz="0.01317717 0.02917508 -0.11594602"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/v2/knee_lo_res.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.01 0.04 -0.17"/>
+      <geometry>
+        <box size="0.12 0.22 0.21"/>
+        <!-- <mesh filename="package://talos_description/meshes/v2/knee_collision.stl" scale="1 1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="leg_left_4_joint" type="revolute">
+    <parent link="leg_left_3_link"/>
+    <child link="leg_left_4_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00000 -0.38000"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="400" lower="0" upper="2.618" velocity="7"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Joint ankle_y (mesh link) : child link ankle_x -> parent link knee -->
+  <link name="leg_left_5_link">
+    <inertial>
+      <mass value="1.29096"/>
+      <inertia ixx="0.01143430819" ixy="0.00096978273" ixz="0.00039976581" iyy="0.00888974312" iyz="-0.00277808667" izz="0.00506373875"/>
+      <origin rpy="0 0 0" xyz="-0.01400838 0.04180064 0.03820186"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0.0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/v2/ankle_Y_lo_res.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.02 0.04 0.0"/>
+      <geometry>
+        <box size="0.14 0.22 0.1"/>
+        <!-- <mesh filename="package://talos_description/meshes/v2/ankle_Y_collision.stl" scale="1 1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="leg_left_5_joint" type="revolute">
+    <parent link="leg_left_4_link"/>
+    <child link="leg_left_5_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00000 -0.32500"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="160" lower="-1.27" upper="0.68" velocity="5.8"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Joint ankle_x (mesh link) : child link None -> parent link ankle_y -->
+  <link name="leg_left_6_link">
+    <inertial>
+      <mass value="1.597773"/>
+      <inertia ixx="0.00439027267" ixy="-6.743674e-05" ixz="0.00113816363" iyy="0.00743063029" iyz="3.943143e-05" izz="0.00550906699"/>
+      <origin rpy="0 0 0" xyz="-0.02034668 -0.00051514 -0.05987428"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <geometry>
+
+       <mesh filename="package://talos_description/meshes/v2/ankle_X_lo_res.glb" scale="1 1 1"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -0.03"/>
+      <geometry>
+            <box size="0.221 0.134 0.16"/>
+<!--        <mesh filename="package://talos_description/meshes/v2/ankle_X_collision.glb" scale="1 -1 1"/>-->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="leg_left_6_joint" type="revolute">
+    <parent link="leg_left_5_link"/>
+    <child link="leg_left_6_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00000 0.00000"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100" lower="-0.5236" upper="0.5236" velocity="4.8"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- from here is copy paste -->
+  <link name="left_sole_link">
+    <inertial>
+      <mass value="0.01"/>
+      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </inertial>
+  </link>
+  <joint name="leg_left_sole_fix_joint" type="fixed">
+    <parent link="leg_left_6_link"/>
+    <child link="left_sole_link"/>
+    <origin rpy="0 0 0" xyz="0.00 0.00 -0.107"/>
+    <axis xyz="1 0 0"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Joint hip_z (mesh link) : child link hip_x -> parent link base_link -->
+  <link name="leg_right_1_link">
+    <inertial>
+      <mass value="1.845591"/>
+      <inertia ixx="0.00541533521" ixy="0.00015428714" ixz="0.00131612622" iyy="0.00848624936" iyz="1.906103e-05" izz="0.00474512541"/>
+      <origin rpy="0 0 0" xyz="0.02247868 -0.00106736 0.03130665"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/v2/hip_z_lo_res.glb" scale="1 -1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.01 -0.01 0.01"/>
+      <geometry>
+        <box size="0.2 0.12 0.13"/>
+        <!-- <mesh filename="package://talos_description/meshes/v2/hip_z_collision.stl" scale="1 -1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="leg_right_1_joint" type="revolute">
+    <parent link="base_link"/>
+    <child link="leg_right_1_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="-0.02 -0.085 -0.27105"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="100" lower="-1.57079632679" upper="0.349065850399" velocity="3.87"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Joint hip_x (mesh link) : child link hip_y -> parent link hip_z -->
+  <link name="leg_right_2_link">
+    <inertial>
+      <mass value="1.490952"/>
+      <inertia ixx="0.00497152259" ixy="0.0001311358" ixz="-4.86648e-06" iyy="0.00190550993" iyz="-4.585381e-05" izz="0.00474289754"/>
+      <origin rpy="0 0 0" xyz="-0.00704703 -0.02592659 0.00273385"/>
+    </inertial>
+    <visual>
+      <origin rpy="0  0  0" xyz="0 0 0.0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/v2/hip_x_lo_res.glb" scale="1 -1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0  0  0" xyz="0.0 -0.06 0.015"/>
+      <geometry>
+        <box size="0.13 0.28 0.14"/>
+        <!-- <mesh filename="package://talos_description/meshes/v2/hip_x_collision.stl" scale="1 -1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="leg_right_2_joint" type="revolute">
+    <parent link="leg_right_1_link"/>
+    <child link="leg_right_2_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00000 0.00000"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="160" lower="-0.5236" upper="0.5236" velocity="5.8"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Joint hip_y (mesh link) : child link knee -> parent link hip_x -->
+  <link name="leg_right_3_link">
+    <inertial>
+      <mass value="6.239871"/>
+      <inertia ixx="0.15337403915" ixy="-0.00068913396" ixz="0.00168034366" iyy="0.13648139346" iyz="0.02187635445" izz="0.03001594885"/>
+      <origin rpy="0 0 0" xyz="0.0058523 -0.0636967 -0.18339564"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/v2/hip_y_lo_res.glb" scale="1 -1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.0 -0.06 -0.25"/>
+      <geometry>
+        <box size="0.15 0.27 0.39"/>
+        <!-- <mesh filename="package://talos_description/meshes/v2/hip_y_collision.stl" scale="1 -1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="leg_right_3_joint" type="revolute">
+    <parent link="leg_right_2_link"/>
+    <child link="leg_right_3_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00000 0.00000"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="160" lower="-2.095" upper="0.7" velocity="5.8"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Joint knee (mesh link) : child link ankle_y -> parent link hip_y -->
+  <link name="leg_right_4_link">
+    <inertial>
+      <mass value="3.759951"/>
+      <inertia ixx="0.04287677484" ixy="-0.00043705397" ixz="0.00078292368" iyy="0.03598906971" iyz="-0.00037717694" izz="0.01364791564"/>
+      <origin rpy="0 0 0" xyz="0.01317717 -0.02917508 -0.11594602"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/v2/knee_lo_res.glb" scale="1 -1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.01 -0.04 -0.17"/>
+      <geometry>
+        <box size="0.12 -0.22 0.21"/>
+        <!-- <mesh filename="package://talos_description/meshes/v2/knee_collision.stl" scale="1 -1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="leg_right_4_joint" type="revolute">
+    <parent link="leg_right_3_link"/>
+    <child link="leg_right_4_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00000 -0.38000"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="400" lower="0" upper="2.618" velocity="7"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Joint ankle_y (mesh link) : child link ankle_x -> parent link knee -->
+  <link name="leg_right_5_link">
+    <inertial>
+      <mass value="1.29096"/>
+      <inertia ixx="0.01143430819" ixy="-0.00096978273" ixz="0.00039976581" iyy="0.00888974312" iyz="0.00277808667" izz="0.00506373875"/>
+      <origin rpy="0 0 0" xyz="-0.01400838 -0.04180064 0.03820186"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0.0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/v2/ankle_Y_lo_res.glb" scale="1 -1 1"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.02 -0.04 0.0"/>
+      <geometry>
+        <box size="0.14 0.22 0.1"/>
+        <!-- <mesh filename="package://talos_description/meshes/v2/ankle_Y_collision.stl" scale="1 -1 1"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="leg_right_5_joint" type="revolute">
+    <parent link="leg_right_4_link"/>
+    <child link="leg_right_5_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00000 -0.32500"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="160" lower="-1.27" upper="0.68" velocity="5.8"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Joint ankle_x (mesh link) : child link None -> parent link ankle_y -->
+  <link name="leg_right_6_link">
+    <inertial>
+      <mass value="1.597773"/>
+      <inertia ixx="0.00439027267" ixy="6.743674e-05" ixz="0.00113816363" iyy="0.00743063029" iyz="-3.943143e-05" izz="0.00550906699"/>
+      <origin rpy="0 0 0" xyz="-0.02034668 0.00051514 -0.05987428"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://talos_description/meshes/v2/ankle_X_lo_res.glb" scale="1 -1 1"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -0.03"/>
+      <geometry>
+            <box size="0.221 0.134 0.16"/>
+<!--        <mesh filename="package://talos_description/meshes/v2/ankle_X_collision.glb" scale="1 -1 1"/>-->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="leg_right_6_joint" type="revolute">
+    <parent link="leg_right_5_link"/>
+    <child link="leg_right_6_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.00000 0.00000 0.00000"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100" lower="-0.5236" upper="0.5236" velocity="4.8"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- from here is copy paste -->
+  <link name="right_sole_link">
+    <inertial>
+      <mass value="0.01"/>
+      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </inertial>
+  </link>
+  <joint name="leg_right_sole_fix_joint" type="fixed">
+    <parent link="leg_right_6_link"/>
+    <child link="right_sole_link"/>
+    <origin rpy="0 0 0" xyz="0.00 0.00 -0.107"/>
+    <axis xyz="1 0 0"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <material name="Blue">
+    <color rgba="0.0 0.0 0.8 1.0"/>
+  </material>
+  <material name="Green">
+    <color rgba="0.0 0.8 0.0 1.0"/>
+  </material>
+  <material name="Grey">
+    <color rgba="0.7 0.7 0.7 1.0"/>
+  </material>
+  <material name="LightGrey">
+    <color rgba="0.9 0.9 0.9 1.0"/>
+  </material>
+  <material name="DarkGrey">
+    <color rgba="0.2 0.2 0.2 1.0"/>
+  </material>
+  <material name="Red">
+    <color rgba="0.8 0.0 0.0 1.0"/>
+  </material>
+  <material name="White">
+    <color rgba="1.0 1.0 1.0 1.0"/>
+  </material>
+  <material name="Black">
+    <color rgba="0.0 0.0 0.0 1.0"/>
+  </material>
+</robot>
+