From 9b76198a002298f693a7cd997f235d78c9d60bef Mon Sep 17 00:00:00 2001
From: Shubham <skpawar1305@gmail.com>
Date: Mon, 10 Jun 2024 01:05:30 +0530
Subject: [PATCH] urdf parser throws error if links are missing in the control
 urdf

skip moveit for now
---
 .github/workflows/test_ros2_jazzy.yml |   93 +
 launch/spot_launch.py                 |   10 +-
 resource/spot.urdf                    | 2822 +-----------------------
 resource/spot_control.urdf            |   18 -
 resource/spotarm.urdf                 | 2875 +++++++++++++++++++++++++
 resource/spotarm_control.urdf         |   69 -
 6 files changed, 2989 insertions(+), 2898 deletions(-)
 create mode 100644 .github/workflows/test_ros2_jazzy.yml
 delete mode 100644 resource/spot_control.urdf
 create mode 100644 resource/spotarm.urdf
 delete mode 100644 resource/spotarm_control.urdf

diff --git a/.github/workflows/test_ros2_jazzy.yml b/.github/workflows/test_ros2_jazzy.yml
new file mode 100644
index 0000000..c067dcf
--- /dev/null
+++ b/.github/workflows/test_ros2_jazzy.yml
@@ -0,0 +1,93 @@
+name: ROS2 Jazzy
+
+on:
+  pull_request:
+    types: [opened, synchronize, reopened, labeled, unlabeled]
+  push:
+    branches:
+      - jazzy
+  schedule:
+    - cron:  '0 10 * * *'
+
+concurrency:
+  group: ${{ github.workflow }}-${{ github.ref }}
+  cancel-in-progress: true
+
+jobs:
+  lint:
+    name: Lint package
+    runs-on: ubuntu-latest
+    strategy:
+      matrix:
+        config:
+        - { python: "3.12" }
+    steps:
+      - name: Checkout repository
+        uses: actions/checkout@v4
+      
+      - uses: actions/setup-python@v5
+        with:
+          python-version: ${{ matrix.config.python }}
+
+      - name: Lint sources
+        uses: pre-commit/action@v3.0.1
+  webots_spot:
+    name: Webots Spot with Nav2 and Moveit2
+    runs-on: ubuntu-24.04
+    steps:
+      - name: Checkout
+        uses: actions/checkout@v4
+      - name: Install ROS2 Jazzy
+        run: >
+          sudo apt update
+
+          sudo apt install -y software-properties-common
+
+          sudo add-apt-repository universe
+
+          curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc
+          | sudo apt-key add -
+
+          sudo sh -c 'echo "deb [arch=amd64,arm64]
+          http://packages.ros.org/ros2/ubuntu `lsb_release -cs` main" >
+          /etc/apt/sources.list.d/ros2-latest.list'
+
+          sudo apt update
+
+          sudo apt install -y ros-jazzy-ros-base
+          python3-colcon-common-extensions ros-dev-tools
+      - name: Install Webots 2023b
+        run: |
+          sudo apt update
+          sudo apt install -y wget dialog apt-utils psmisc lsb-release git
+          sudo wget https://github.com/cyberbotics/webots/releases/download/R2023b/webots_2023b_amd64.deb -O /tmp/webots.deb
+          sudo apt install -y /tmp/webots.deb xvfb
+      - name: Create Workspace
+        run: |
+          mkdir -p ~/ros2_ws/src
+          cp -r $GITHUB_WORKSPACE/. ~/ros2_ws/src/webots_ros2_spot
+      - name: Install Webots Spot Dependencies
+        run: |
+          cd ~/ros2_ws
+          source /opt/ros/jazzy/setup.bash
+          sudo rosdep init
+          rosdep update
+          sudo apt install -y libunwind-dev # Nav2 dependency
+          rosdep install --from-paths src --ignore-src -r -y --skip-keys ros-jazzy-webots-ros2 ros-jazzy-moveit
+          vcs import --recursive src --skip-existing --input src/webots_ros2_spot/webots_ros2_spot.repos
+          chmod +x src/webots_ros2/webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py
+      - name: Build Packages
+        run: |
+          cd ~/ros2_ws
+          source /opt/ros/jazzy/setup.bash
+          colcon build --symlink-install
+          source install/setup.bash
+      - name: Test
+        run: |
+          export WEBOTS_RELEASE_VERSION=2023b-rev1
+          export WEBOTS_OFFSCREEN=1
+          export CI=1
+          export DEBIAN_FRONTEND=noninteractive
+          cd ~/ros2_ws
+          source install/setup.bash
+          colcon test --ctest-args tests --packages-select webots_spot --event-handlers console_direct+ --return-code-on-test-failure
diff --git a/launch/spot_launch.py b/launch/spot_launch.py
index 8c670b8..ef7f696 100644
--- a/launch/spot_launch.py
+++ b/launch/spot_launch.py
@@ -27,7 +27,7 @@ def get_ros2_nodes(*args):
         parameters=[
             {
                 "robot_description": os.path.join(
-                    package_dir, "resource", "spotarm_control.urdf"
+                    package_dir, "resource", "spotarm.urdf"
                 )
             },
             {"use_sim_time": True},
@@ -37,7 +37,7 @@ def get_ros2_nodes(*args):
     )
 
     # ROS2 control spawners for SpotArm
-    controller_manager_timeout = ["--controller-manager-timeout", "500"]
+    controller_manager_timeout = ["--controller-manager-timeout", "50"]
     controller_manager_prefix = "python.exe" if os.name == "nt" else ""
     trajectory_controller_spawner = Node(
         package="controller_manager",
@@ -98,9 +98,7 @@ def generate_launch_description():
         robot_name="Spot",
         parameters=[
             {
-                "robot_description": os.path.join(
-                    package_dir, "resource", "spot_control.urdf"
-                ),
+                "robot_description": os.path.join(package_dir, "resource", "spot.urdf"),
                 "use_sim_time": True,
                 "set_robot_state_publisher": False,  # foot positions are wrong with webot's urdf
             }
@@ -108,7 +106,7 @@ def generate_launch_description():
         respawn=True,
     )
 
-    with open(os.path.join(package_dir, "resource", "spot.urdf")) as f:
+    with open(os.path.join(package_dir, "resource", "spotarm.urdf")) as f:
         robot_desc = f.read()
 
     robot_state_publisher = Node(
diff --git a/resource/spot.urdf b/resource/spot.urdf
index 36830a0..d6c1025 100644
--- a/resource/spot.urdf
+++ b/resource/spot.urdf
@@ -1,2808 +1,20 @@
 <?xml version="1.0"?>
 <robot name="Spot" xmlns:xacro="http://ros.org/wiki/xacro">
-  <link name="base_link">
-    <visual>
-      <origin xyz="0.462094 0 -0.000232" rpy="1.127615 -0.00001 -1.570793"/>
-      <geometry>
-        <box size="0.192453 0.177358 0.043396"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.462094 0 -0.000232" rpy="1.127615 -0.00001 -1.570793"/>
-      <geometry>
-        <box size="0.192453 0.177358 0.043396"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.477932 0 0.084013" rpy="1.217205 -0.000011 -1.570792"/>
-      <geometry>
-        <box size="0.192453 0.018113 0.08717"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.477932 0 0.084013" rpy="1.217205 -0.000011 -1.570792"/>
-      <geometry>
-        <box size="0.192453 0.018113 0.08717"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.451833 0 0.041304" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.037736 0.192453 0.102642"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.451833 0 0.041304" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.037736 0.192453 0.102642"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.031113 0 0.018833" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.535849 0.222642 0.189434"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.031113 0 0.018833" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.535849 0.222642 0.189434"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.376019 0 0.107135" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.158491 0.166038 0.013208"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.376019 0 0.107135" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.158491 0.166038 0.013208"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.363578 0 -0.079657" rpy="1.537614 -0.00001 -1.570793"/>
-      <geometry>
-        <box size="0.166038 0.013208 0.124528"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.363578 0 -0.079657" rpy="1.537614 -0.00001 -1.570793"/>
-      <geometry>
-        <box size="0.166038 0.013208 0.124528"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.319453 0 0.107135" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.158491 0.166038 0.013208"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.319453 0 0.107135" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.158491 0.166038 0.013208"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.313025 0 -0.064563" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.14717 0.166038 0.013208"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.313025 0 -0.064563" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.14717 0.166038 0.013208"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.120195 0.111629 0.074886" rpy="1.5708 -0.423195 -1.570794"/>
-      <geometry>
-        <box size="0.030189 0.064528 0.182264"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.120195 0.111629 0.074886" rpy="1.5708 -0.423195 -1.570794"/>
-      <geometry>
-        <box size="0.030189 0.064528 0.182264"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.120195 -0.111629 0.074886" rpy="1.5708 0.423174 -1.570793"/>
-      <geometry>
-        <box size="0.030189 0.064528 0.182264"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.120195 -0.111629 0.074886" rpy="1.5708 0.423174 -1.570793"/>
-      <geometry>
-        <box size="0.030189 0.064528 0.182264"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.427377 0 0.059588" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.089057 0.192453 0.05283"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.427377 0 0.059588" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.089057 0.192453 0.05283"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.404 0 0.096611" rpy="1.730798 -0.000011 -1.570794"/>
-      <geometry>
-        <box size="0.193585 0.003774 0.135849"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.404 0 0.096611" rpy="1.730798 -0.000011 -1.570794"/>
-      <geometry>
-        <box size="0.193585 0.003774 0.135849"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.400585 0 -0.018903" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.04566 0.191698 0.103396"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.400585 0 -0.018903" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.04566 0.191698 0.103396"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.431528 0 -0.008337" rpy="2.0308 -0.00001 -1.570793"/>
-      <geometry>
-        <box size="0.193585 0.109434 0.037736"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.431528 0 -0.008337" rpy="2.0308 -0.00001 -1.570793"/>
-      <geometry>
-        <box size="0.193585 0.109434 0.037736"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.28 0 0.121" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.05 0.1 0.02"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.28 0 0.121" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.05 0.1 0.02"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.02 0 0.121" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.05 0.1 0.02"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.02 0 0.121" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.05 0.1 0.02"/>
-      </geometry>
-    </collision>
-  </link>
-  <link name="Velodyne_Puck">
-    <visual>
-      <origin xyz="0 -0.00165 0" rpy="0 0 0"/>
-      <geometry>
-        <cylinder radius="0.052" length="0.0727"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0 -0.00165 0" rpy="0 0 0"/>
-      <geometry>
-        <cylinder radius="0.052" length="0.0727"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="base_link_Velodyne_Puck_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="Velodyne_Puck"/>
-    <origin xyz="-0.063 0 0.1518" rpy="-0.000008 -0.000008 0.000001"/>
-  </joint>
-  <link name="solid">
-  </link>
-  <joint name="base_link_solid_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="solid"/>
-    <origin xyz="0.496831 0.000001 0.104801" rpy="1.2208 -0.00001 -1.570793"/>
-  </joint>
-  <link name="kinect range">
-  </link>
-  <joint name="solid_kinect range_joint" type="fixed">
-    <parent link="solid"/>
-    <child link="kinect range"/>
-    <origin xyz="0 0 -0.0127" rpy="-0.785397 1.570792 0.785397"/>
-  </joint>
-  <link name="kinect color">
-  </link>
-  <joint name="solid_kinect color_joint" type="fixed">
-    <parent link="solid"/>
-    <child link="kinect color"/>
-    <origin xyz="0 0 -0.0127" rpy="-2.356193 1.570792 -0.7854"/>
-  </joint>
-  <link name="SpotArm">
-    <visual>
-      <origin xyz="0 0 0.021" rpy="0 0 0"/>
-      <geometry>
-        <cylinder radius="0.075" length="0.035"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0.021" rpy="0 0 0"/>
-      <geometry>
-        <cylinder radius="0.075" length="0.035"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="base_link_SpotArm_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="SpotArm"/>
-    <origin xyz="0.35 0 0.1118" rpy="-0.000017 -0.000003 0.000005"/>
-  </joint>
-  <joint name="spotarm_1_joint" type="revolute">
-    <parent link="SpotArm"/>
-    <child link="spotarm_link1"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="500" lower="-2.61799" upper="3.14159" velocity="2.5"/>
-    <origin xyz="0 0 0.04" rpy="0 0 0"/>
-  </joint>
-  <link name="spotarm_link1">
-    <visual>
-      <origin xyz="0 -0.035 0.076" rpy="3.141593 1.570793 3.141593"/>
-      <geometry>
-        <box size="0.14 0.01 0.08"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0 -0.035 0.076" rpy="3.141593 1.570793 3.141593"/>
-      <geometry>
-        <box size="0.14 0.01 0.08"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0 0.035 0.076" rpy="3.141593 1.570793 3.141593"/>
-      <geometry>
-        <box size="0.14 0.01 0.08"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0 0.035 0.076" rpy="3.141593 1.570793 3.141593"/>
-      <geometry>
-        <box size="0.14 0.01 0.08"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0 -0.0555 0.1" rpy="1.5708 0 0"/>
-      <geometry>
-        <cylinder radius="0.032" length="0.031"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0 -0.0555 0.1" rpy="1.5708 0 0"/>
-      <geometry>
-        <cylinder radius="0.032" length="0.031"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0 0.0555 0.1" rpy="1.5708 0 0"/>
-      <geometry>
-        <cylinder radius="0.032" length="0.031"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0 0.0555 0.1" rpy="1.5708 0 0"/>
-      <geometry>
-        <cylinder radius="0.032" length="0.031"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="spotarm_2_joint" type="revolute">
-    <parent link="spotarm_link1"/>
-    <child link="spotarm_link2"/>
-    <axis xyz="0 -1 0"/>
-    <limit effort="500" lower="-0.523599" upper="3.14159" velocity="2.5"/>
-    <origin xyz="0 0.03 0.1" rpy="0 0 0"/>
-  </joint>
-  <link name="spotarm_link2">
-    <visual>
-      <origin xyz="0.017 -0.03 0" rpy="3.141593 1.570793 3.141593"/>
-      <geometry>
-        <box size="0.06 0.059 0.093"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.017 -0.03 0" rpy="3.141593 1.570793 3.141593"/>
-      <geometry>
-        <box size="0.06 0.059 0.093"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.185 -0.03 0" rpy="3.141593 1.570793 3.141593"/>
-      <geometry>
-        <cylinder radius="0.0456" length="0.24"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.185 -0.03 0" rpy="3.141593 1.570793 3.141593"/>
-      <geometry>
-        <cylinder radius="0.0456" length="0.24"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="spotarm_3_joint" type="revolute">
-    <parent link="spotarm_link2"/>
-    <child link="spotarm_link3"/>
-    <axis xyz="0 1 0"/>
-    <limit effort="500" lower="0" upper="3.0" velocity="2.5"/>
-    <origin xyz="0.3385 -0.02 0.000137" rpy="0 0 0"/>
-  </joint>
-  <link name="spotarm_link3">
-    <visual>
-      <origin xyz="0.044 -0.01 0.0514" rpy="3.141593 0.851593 3.141593"/>
-      <geometry>
-        <box size="0.16 0.041 0.04"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.044 -0.01 0.0514" rpy="3.141593 0.851593 3.141593"/>
-      <geometry>
-        <box size="0.16 0.041 0.04"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.1065 -0.01 0.0973" rpy="3.141593 1.570793 3.141593"/>
-      <geometry>
-        <cylinder radius="0.0477" length="0.059"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.1065 -0.01 0.0973" rpy="3.141593 1.570793 3.141593"/>
-      <geometry>
-        <cylinder radius="0.0477" length="0.059"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="spotarm_4_joint" type="revolute">
-    <parent link="spotarm_link3"/>
-    <child link="spotarm_link4"/>
-    <axis xyz="1 0 0"/>
-    <limit effort="500" lower="-4.3633" upper="1.22173" velocity="2.5"/>
-    <origin xyz="0.107 -0.01 0.09734" rpy="0 0 0"/>
-  </joint>
-  <link name="spotarm_link4">
-  </link>
-  <joint name="Slider11" type="prismatic">
-    <parent link="spotarm_link4"/>
-    <child link="spotarm_link5"/>
-    <axis xyz="1 0 0"/>
-    <origin xyz="0.035 0 0" rpy="0 0 0"/>
-    <limit effort="100" lower="0" upper="0.075" velocity="100"/>
-  </joint>
-  <link name="spotarm_link5">
-    <visual>
-      <origin xyz="0.232 0 0" rpy="3.141593 1.570793 3.141593"/>
-      <geometry>
-        <box size="0.04 0.044 0.042"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.232 0 0" rpy="3.141593 1.570793 3.141593"/>
-      <geometry>
-        <box size="0.04 0.044 0.042"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.106 0 0" rpy="3.141593 1.570793 3.141593"/>
-      <geometry>
-        <cylinder radius="0.0477" length="0.21"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.106 0 0" rpy="3.141593 1.570793 3.141593"/>
-      <geometry>
-        <cylinder radius="0.0477" length="0.21"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="spotarm_5_joint" type="revolute">
-    <parent link="spotarm_link5"/>
-    <child link="spotarm_link6"/>
-    <axis xyz="0 1 0"/>
-    <limit effort="500" lower="-1.0472" upper="1.0472" velocity="2.5"/>
-    <origin xyz="0.2413 0.01 0" rpy="0 0 0"/>
-  </joint>
-  <link name="spotarm_link6">
-    <visual>
-      <origin xyz="0.0671 -0.0105 0" rpy="3.141593 1.570793 3.141593"/>
-      <geometry>
-        <cylinder radius="0.0477" length="0.065"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.0671 -0.0105 0" rpy="3.141593 1.570793 3.141593"/>
-      <geometry>
-        <cylinder radius="0.0477" length="0.065"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="spotarm_6_joint" type="revolute">
-    <parent link="spotarm_link6"/>
-    <child link="front"/>
-    <axis xyz="0.000004 0 1"/>
-    <limit effort="500" lower="-4.45059" upper="1.30899" velocity="2.5"/>
-    <origin xyz="0.1 -0.0105 0" rpy="3.141593 -1.570793 3.141593"/>
-  </joint>
-  <link name="front">
-    <visual>
-      <origin xyz="0 0.0005 -0.007" rpy="0 0 0"/>
-      <geometry>
-        <cylinder radius="0.0227" length="0.028"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0 0.0005 -0.007" rpy="0 0 0"/>
-      <geometry>
-        <cylinder radius="0.0227" length="0.028"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.029 -0.01784 -0.053" rpy="0 0 0.59"/>
-      <geometry>
-        <box size="0.04 0.0217 0.105"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.029 -0.01784 -0.053" rpy="0 0 0.59"/>
-      <geometry>
-        <box size="0.04 0.0217 0.105"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.029 0.01884 -0.053" rpy="0 0 0.59"/>
-      <geometry>
-        <box size="0.04 0.0217 0.105"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.029 0.01884 -0.053" rpy="0 0 0.59"/>
-      <geometry>
-        <box size="0.04 0.0217 0.105"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.0172 0.0114 -0.053" rpy="3.141593 -1.570793 3.141593"/>
-      <geometry>
-        <box size="0.106 0.056 0.071"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.0172 0.0114 -0.053" rpy="3.141593 -1.570793 3.141593"/>
-      <geometry>
-        <box size="0.106 0.056 0.071"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.0172 -0.0104 -0.053" rpy="3.141593 -1.570793 3.141593"/>
-      <geometry>
-        <box size="0.106 0.056 0.071"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.0172 -0.0104 -0.053" rpy="3.141593 -1.570793 3.141593"/>
-      <geometry>
-        <box size="0.106 0.056 0.071"/>
-      </geometry>
-    </collision>
-  </link>
-  <link name="gripper_centre">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <sphere radius="0.01"/>
-      </geometry>
-    </visual>
-  </link>
-  <joint name="gripper_centre_joint" type="fixed">
-    <parent link="front"/>
-    <child link="gripper_centre"/>
-    <origin xyz="0 0 -0.18" rpy="0 0 0"/>
-  </joint>
-  <joint name="gripper_right_finger_joint" type="prismatic">
-    <parent link="front"/>
-    <child link="gripper_right_finger_link"/>
-    <axis xyz="1 0 0"/>
-    <origin xyz="0 0.0005 -0.007" rpy="0 0 0"/>
-    <limit effort="16" lower="0" upper="0.045" velocity="0.05"/>
-  </joint>
-  <link name="gripper_right_finger_link">
-    <visual>
-      <origin xyz="0.004 0 -0.1741" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.0076 0.05091 0.08982"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.004 0 -0.1741" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.0076 0.05091 0.08982"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.0049 0.0153 -0.1208" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.0055 0.0295 0.018"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.0049 0.0153 -0.1208" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.0055 0.0295 0.018"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.0085 0.0156 -0.1048" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.03213 0.02959 0.01487"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.0085 0.0156 -0.1048" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.03213 0.02959 0.01487"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="gripper_left_finger_joint" type="prismatic">
-    <parent link="front"/>
-    <child link="gripper_left_finger_link"/>
-    <axis xyz="1 0.000093 0"/>
-    <origin xyz="0 0 0" rpy="0 0 3.1415"/>
-    <limit effort="16" lower="0" upper="0.045" velocity="0.05"/>
-  </joint>
-  <link name="gripper_left_finger_link">
-    <visual>
-      <origin xyz="0.004 0 -0.1741" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.0076 0.05091 0.08982"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.004 0 -0.1741" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.0076 0.05091 0.08982"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.0049 0.0153 -0.1208" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.0055 0.0295 0.018"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.0049 0.0153 -0.1208" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.0055 0.0295 0.018"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.0085 0.0156 -0.1048" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.03213 0.02959 0.01487"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.0085 0.0156 -0.1048" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.03213 0.02959 0.01487"/>
-      </geometry>
-    </collision>
-  </link>
-  <link name="cap screw(19)">
-  </link>
-  <joint name="front_cap screw(19)_joint" type="fixed">
-    <parent link="front"/>
-    <child link="cap screw(19)"/>
-    <origin xyz="0.0415 -0.0029 -0.0004" rpy="3.14159 0 0"/>
-  </joint>
-  <link name="cap screw(18)">
-  </link>
-  <joint name="front_cap screw(18)_joint" type="fixed">
-    <parent link="front"/>
-    <child link="cap screw(18)"/>
-    <origin xyz="-0.0415 0.0029 -0.0004" rpy="3.14159 0 0"/>
-  </joint>
-  <link name="cap screw(17)">
-  </link>
-  <joint name="front_cap screw(17)_joint" type="fixed">
-    <parent link="front"/>
-    <child link="cap screw(17)"/>
-    <origin xyz="0.0433 0.0315 0.0026" rpy="3.14159 0 0"/>
-  </joint>
-  <link name="cap screw(16)">
-  </link>
-  <joint name="front_cap screw(16)_joint" type="fixed">
-    <parent link="front"/>
-    <child link="cap screw(16)"/>
-    <origin xyz="-0.0433 -0.0315 0.0026" rpy="3.14159 0 0"/>
-  </joint>
-  <link name="cap screw(15)">
-  </link>
-  <joint name="front_cap screw(15)_joint" type="fixed">
-    <parent link="front"/>
-    <child link="cap screw(15)"/>
-    <origin xyz="0 0.033 0.0026" rpy="3.14159 0 0"/>
-  </joint>
-  <link name="cap screw(14)">
-  </link>
-  <joint name="front_cap screw(14)_joint" type="fixed">
-    <parent link="front"/>
-    <child link="cap screw(14)"/>
-    <origin xyz="0 -0.033 0.0026" rpy="3.14159 0 0"/>
-  </joint>
-  <link name="cap screw(13)">
-  </link>
-  <joint name="front_cap screw(13)_joint" type="fixed">
-    <parent link="front"/>
-    <child link="cap screw(13)"/>
-    <origin xyz="0.0426 -0.0094 -0.09448" rpy="0 0 0"/>
-  </joint>
-  <link name="cap screw(12)">
-  </link>
-  <joint name="front_cap screw(12)_joint" type="fixed">
-    <parent link="front"/>
-    <child link="cap screw(12)"/>
-    <origin xyz="-0.0425 0.0094 -0.0945" rpy="0 0 0"/>
-  </joint>
-  <link name="cap screw(11)">
-  </link>
-  <joint name="front_cap screw(11)_joint" type="fixed">
-    <parent link="front"/>
-    <child link="cap screw(11)"/>
-    <origin xyz="-0.0477 -0.008 -0.0108" rpy="3.141593 1.570793 3.141593"/>
-  </joint>
-  <link name="cap screw(10)">
-  </link>
-  <joint name="front_cap screw(10)_joint" type="fixed">
-    <parent link="front"/>
-    <child link="cap screw(10)"/>
-    <origin xyz="-0.0477 -0.008 -0.043" rpy="3.141593 1.570793 3.141593"/>
-  </joint>
-  <link name="cap screw(9)">
-  </link>
-  <joint name="front_cap screw(9)_joint" type="fixed">
-    <parent link="front"/>
-    <child link="cap screw(9)"/>
-    <origin xyz="-0.0477 -0.008 -0.075" rpy="3.141593 1.570793 3.141593"/>
-  </joint>
-  <link name="cap screw(8)">
-  </link>
-  <joint name="front_cap screw(8)_joint" type="fixed">
-    <parent link="front"/>
-    <child link="cap screw(8)"/>
-    <origin xyz="-0.0477 0.008 -0.075" rpy="3.141593 1.570793 3.141593"/>
-  </joint>
-  <link name="cap screw(7)">
-  </link>
-  <joint name="front_cap screw(7)_joint" type="fixed">
-    <parent link="front"/>
-    <child link="cap screw(7)"/>
-    <origin xyz="-0.0477 0.008 -0.043" rpy="3.141593 1.570793 3.141593"/>
-  </joint>
-  <link name="cap screw(6)">
-  </link>
-  <joint name="front_cap screw(6)_joint" type="fixed">
-    <parent link="front"/>
-    <child link="cap screw(6)"/>
-    <origin xyz="-0.0477 0.008 -0.0108" rpy="3.141593 1.570793 3.141593"/>
-  </joint>
-  <link name="cap screw(5)">
-  </link>
-  <joint name="front_cap screw(5)_joint" type="fixed">
-    <parent link="front"/>
-    <child link="cap screw(5)"/>
-    <origin xyz="0.0477 -0.008 -0.0108" rpy="3.141593 -1.570793 3.141593"/>
-  </joint>
-  <link name="cap screw(4)">
-  </link>
-  <joint name="front_cap screw(4)_joint" type="fixed">
-    <parent link="front"/>
-    <child link="cap screw(4)"/>
-    <origin xyz="0.0477 -0.008 -0.043" rpy="3.141593 -1.570793 3.141593"/>
-  </joint>
-  <link name="cap screw(3)">
-  </link>
-  <joint name="front_cap screw(3)_joint" type="fixed">
-    <parent link="front"/>
-    <child link="cap screw(3)"/>
-    <origin xyz="0.0477 -0.008 -0.075" rpy="3.141593 -1.570793 3.141593"/>
-  </joint>
-  <link name="cap screw(2)">
-  </link>
-  <joint name="front_cap screw(2)_joint" type="fixed">
-    <parent link="front"/>
-    <child link="cap screw(2)"/>
-    <origin xyz="0.0477 0.008 -0.075" rpy="3.141593 -1.570793 3.141593"/>
-  </joint>
-  <link name="cap screw(1)">
-  </link>
-  <joint name="front_cap screw(1)_joint" type="fixed">
-    <parent link="front"/>
-    <child link="cap screw(1)"/>
-    <origin xyz="0.0477 0.008 -0.043" rpy="3.141593 -1.570793 3.141593"/>
-  </joint>
-  <link name="cap screw">
-  </link>
-  <joint name="front_cap screw_joint" type="fixed">
-    <parent link="front"/>
-    <child link="cap screw"/>
-    <origin xyz="0.0477 0.008 -0.0108" rpy="3.141593 -1.570793 3.141593"/>
-  </joint>
-  <joint name="rear right shoulder abduction motor" type="revolute">
-    <parent link="base_link"/>
-    <child link="rear right shoulder"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="50" lower="-0.6" upper="0.5" velocity="10"/>
-    <origin xyz="-0.3084 -0.0528 0.012301" rpy="1.5708 -0.00001 -1.570793"/>
-  </joint>
-  <link name="rear right shoulder">
-    <visual>
-      <origin xyz="-0.004874 0.000873 -0.000643" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.086792 0.124528 0.120755"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.004874 0.000873 -0.000643" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.086792 0.124528 0.120755"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.006447 0.000873 -0.000643" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.098113 0.098113 0.098113"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.006447 0.000873 -0.000643" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.098113 0.098113 0.098113"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.058145 0.000873 -0.000643" rpy="-1.5708 0 1.5708"/>
-      <geometry>
-        <cylinder radius="0.047925" length="0.006038"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.058145 0.000873 -0.000643" rpy="-1.5708 0 1.5708"/>
-      <geometry>
-        <cylinder radius="0.047925" length="0.006038"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="rear right shoulder rotation motor" type="revolute">
-    <parent link="rear right shoulder"/>
-    <child link="rear right upperarm"/>
-    <axis xyz="1 0 0"/>
-    <limit effort="50" lower="-1.7" upper="1.7" velocity="10"/>
-    <origin xyz="0 -0.00053 0" rpy="0 0 0"/>
-  </joint>
-  <link name="rear right upperarm">
-    <visual>
-      <origin xyz="0.117011 -0.32073 0.1829" rpy="-1.5708 0 1.5708"/>
-      <geometry>
-        <cylinder radius="0.02717" length="0.060377"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.117011 -0.32073 0.1829" rpy="-1.5708 0 1.5708"/>
-      <geometry>
-        <cylinder radius="0.02717" length="0.060377"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.117011 -0.25867 0.180358" rpy="0 0.05 1.570795"/>
-      <geometry>
-        <box size="0.09434 0.060377 0.053208"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.117011 -0.25867 0.180358" rpy="0 0.05 1.570795"/>
-      <geometry>
-        <box size="0.09434 0.060377 0.053208"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.149087 -0.023654 0.007772" rpy="-0.656375 0 0"/>
-      <geometry>
-        <box size="0.016226 0.177358 0.071698"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.149087 -0.023654 0.007772" rpy="-0.656375 0 0"/>
-      <geometry>
-        <box size="0.016226 0.177358 0.071698"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.14944 -0.001095 0.048195" rpy="-0.688653 0.282831 -0.225783"/>
-      <geometry>
-        <box size="0.004528 0.173585 0.015094"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.14944 -0.001095 0.048195" rpy="-0.688653 0.282831 -0.225783"/>
-      <geometry>
-        <box size="0.004528 0.173585 0.015094"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.143963 -0.142325 0.149419" rpy="-0.689784 0.390399 -0.237371"/>
-      <geometry>
-        <box size="0.004528 0.173585 0.015094"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.143963 -0.142325 0.149419" rpy="-0.689784 0.390399 -0.237371"/>
-      <geometry>
-        <box size="0.004528 0.173585 0.015094"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.143376 -0.16295 0.113632" rpy="-0.64577 0.030504 0.039623"/>
-      <geometry>
-        <box size="0.016226 0.173585 0.071698"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.143376 -0.16295 0.113632" rpy="-0.64577 0.030504 0.039623"/>
-      <geometry>
-        <box size="0.016226 0.173585 0.071698"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.113992 -0.21476 0.154045" rpy="-0.656375 0 0"/>
-      <geometry>
-        <box size="0.054717 0.098113 0.064151"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.113992 -0.21476 0.154045" rpy="-0.656375 0 0"/>
-      <geometry>
-        <box size="0.054717 0.098113 0.064151"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.113238 -0.119427 0.086296" rpy="-2.227175 0 0"/>
-      <geometry>
-        <cylinder radius="0.028679" length="0.150943"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.113238 -0.119427 0.086296" rpy="-2.227175 0 0"/>
-      <geometry>
-        <cylinder radius="0.028679" length="0.150943"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.113238 -0.04459 0.028636" rpy="-2.227175 0 0"/>
-      <geometry>
-        <cylinder radius="0.037736" length="0.041509"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.113238 -0.04459 0.028636" rpy="-2.227175 0 0"/>
-      <geometry>
-        <cylinder radius="0.037736" length="0.041509"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.113738 -0.02284 -0.046726" rpy="-0.656375 0 0"/>
-      <geometry>
-        <box size="0.061509 0.069811 0.011321"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.113738 -0.02284 -0.046726" rpy="-0.656375 0 0"/>
-      <geometry>
-        <box size="0.061509 0.069811 0.011321"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.073615 -0.00019 -0.000689" rpy="-1.5708 0 1.5708"/>
-      <geometry>
-        <cylinder radius="0.04" length="0.037736"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.073615 -0.00019 -0.000689" rpy="-1.5708 0 1.5708"/>
-      <geometry>
-        <cylinder radius="0.04" length="0.037736"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.115879 0.008331 -0.011344" rpy="-2.227175 0 0"/>
-      <geometry>
-        <cylinder radius="0.033962" length="0.086792"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.115879 0.008331 -0.011344" rpy="-2.227175 0 0"/>
-      <geometry>
-        <cylinder radius="0.033962" length="0.086792"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.12154 0.053961 -0.047932" rpy="-0.656375 0 0"/>
-      <geometry>
-        <box size="0.064151 0.003774 0.071698"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.12154 0.053961 -0.047932" rpy="-0.656375 0 0"/>
-      <geometry>
-        <box size="0.064151 0.003774 0.071698"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.12003 0.073308 -0.006499" rpy="0.013625 0 0"/>
-      <geometry>
-        <box size="0.060377 0.007547 0.022642"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.12003 0.073308 -0.006499" rpy="0.013625 0 0"/>
-      <geometry>
-        <box size="0.060377 0.007547 0.022642"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.117011 -0.13635 0.154336" rpy="0.99 0 0"/>
-      <geometry>
-        <box size="0.049057 0.005283 0.169811"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.117011 -0.13635 0.154336" rpy="0.99 0 0"/>
-      <geometry>
-        <box size="0.049057 0.005283 0.169811"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.083111 -0.31378 0.182934" rpy="0 -0.076371 1.570798"/>
-      <geometry>
-        <box size="0.044 0.008 0.0387"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.083111 -0.31378 0.182934" rpy="0 -0.076371 1.570798"/>
-      <geometry>
-        <box size="0.044 0.008 0.0387"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="rear right elbow motor" type="revolute">
-    <parent link="rear right upperarm"/>
-    <child link="rear right forearm"/>
-    <axis xyz="1 0 0"/>
-    <limit effort="50" lower="-0.45" upper="1.6" velocity="10"/>
-    <origin xyz="0.1122 -0.319729 0.182338" rpy="0 0 0"/>
-  </joint>
-  <link name="rear right forearm">
-    <visual>
-      <origin xyz="0.001038 -0.271321 -0.225038" rpy="-1.5708 0 1.570801"/>
-      <geometry>
-        <cylinder radius="0.035094" length="0.039623"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.001038 -0.271321 -0.225038" rpy="-1.5708 0 1.570801"/>
-      <geometry>
-        <cylinder radius="0.035094" length="0.039623"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.001038 -0.227421 -0.230239" rpy="-0.947675 0 0"/>
-      <geometry>
-        <box size="0.037736 0.006038 0.041509"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.001038 -0.227421 -0.230239" rpy="-0.947675 0 0"/>
-      <geometry>
-        <box size="0.037736 0.006038 0.041509"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.001038 -0.232272 -0.198255" rpy="-0.093185 0 0"/>
-      <geometry>
-        <box size="0.037736 0.045283 0.006792"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.001038 -0.232272 -0.198255" rpy="-0.093185 0 0"/>
-      <geometry>
-        <box size="0.037736 0.045283 0.006792"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.001038 -0.205751 -0.206348" rpy="0.616815 0 0"/>
-      <geometry>
-        <box size="0.025283 0.022642 0.020755"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.001038 -0.205751 -0.206348" rpy="0.616815 0 0"/>
-      <geometry>
-        <box size="0.025283 0.022642 0.020755"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.001038 -0.168611 -0.18111" rpy="-2.513185 0 0"/>
-      <geometry>
-        <box size="0.019245 0.071698 0.017358"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.001038 -0.168611 -0.18111" rpy="-2.513185 0 0"/>
-      <geometry>
-        <box size="0.019245 0.071698 0.017358"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.001038 -0.125111 -0.148236" rpy="0.663625 0 0"/>
-      <geometry>
-        <box size="0.021887 0.037736 0.018491"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.001038 -0.125111 -0.148236" rpy="0.663625 0 0"/>
-      <geometry>
-        <box size="0.021887 0.037736 0.018491"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.001038 -0.100861 -0.126163" rpy="0.760415 0 0"/>
-      <geometry>
-        <box size="0.026415 0.030189 0.021132"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.001038 -0.100861 -0.126163" rpy="0.760415 0 0"/>
-      <geometry>
-        <box size="0.026415 0.030189 0.021132"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.001038 -0.084241 -0.106843" rpy="0.867225 0 0"/>
-      <geometry>
-        <box size="0.032453 0.022642 0.024528"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.001038 -0.084241 -0.106843" rpy="0.867225 0 0"/>
-      <geometry>
-        <box size="0.032453 0.022642 0.024528"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.001038 -0.068551 -0.08661" rpy="0.907225 0 0"/>
-      <geometry>
-        <box size="0.037736 0.033962 0.026415"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.001038 -0.068551 -0.08661" rpy="0.907225 0 0"/>
-      <geometry>
-        <box size="0.037736 0.033962 0.026415"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.001038 -0.049721 -0.059227" rpy="0.933245 0 0"/>
-      <geometry>
-        <cylinder radius="0.024528" length="0.030566"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.001038 -0.049721 -0.059227" rpy="0.933245 0 0"/>
-      <geometry>
-        <cylinder radius="0.024528" length="0.030566"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.001038 -0.034991 -0.038166" rpy="0.937225 0 0"/>
-      <geometry>
-        <box size="0.038113 0.022642 0.032075"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.001038 -0.034991 -0.038166" rpy="0.937225 0 0"/>
-      <geometry>
-        <box size="0.038113 0.022642 0.032075"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.001038 -0.022931 -0.021744" rpy="0.937225 0 0"/>
-      <geometry>
-        <box size="0.031321 0.018868 0.03283"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.001038 -0.022931 -0.021744" rpy="0.937225 0 0"/>
-      <geometry>
-        <box size="0.031321 0.018868 0.03283"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.001038 -0.010711 -0.006332" rpy="0.834035 0 0"/>
-      <geometry>
-        <box size="0.027925 0.018868 0.03283"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.001038 -0.010711 -0.006332" rpy="0.834035 0 0"/>
-      <geometry>
-        <box size="0.027925 0.018868 0.03283"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.001038 0.004299 0.006862" rpy="0.680845 0 0"/>
-      <geometry>
-        <box size="0.027547 0.018868 0.034717"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.001038 0.004299 0.006862" rpy="0.680845 0 0"/>
-      <geometry>
-        <box size="0.027547 0.018868 0.034717"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.001038 0.021259 0.019851" rpy="0.604465 0 0"/>
-      <geometry>
-        <box size="0.020377 0.026415 0.03434"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.001038 0.021259 0.019851" rpy="0.604465 0 0"/>
-      <geometry>
-        <box size="0.020377 0.026415 0.03434"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.001038 0.019179 -0.009531" rpy="0.494895 0 0"/>
-      <geometry>
-        <box size="0.010189 0.05283 0.00717"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.001038 0.019179 -0.009531" rpy="0.494895 0 0"/>
-      <geometry>
-        <box size="0.010189 0.05283 0.00717"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.001038 0.037459 0.024742" rpy="-1.5708 0 1.5708"/>
-      <geometry>
-        <cylinder radius="0.021887" length="0.019736"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.001038 0.037459 0.024742" rpy="-1.5708 0 1.5708"/>
-      <geometry>
-        <cylinder radius="0.021887" length="0.019736"/>
-      </geometry>
-    </collision>
-  </link>
-  <link name="rear right touch sensor">
-  </link>
-  <joint name="rear right forearm_rear right touch sensor_joint" type="fixed">
-    <parent link="rear right forearm"/>
-    <child link="rear right touch sensor"/>
-    <origin xyz="0.001038 -0.271321 -0.225038" rpy="-1.5708 0 1.570801"/>
-  </joint>
-  <joint name="rear right piston motor" type="continuous">
-    <parent link="rear right upperarm"/>
-    <child link="solid_0"/>
-    <axis xyz="1 0 0"/>
-    <limit effort="10" velocity="10"/>
-    <origin xyz="0.1172 -0.29077 0.2083" rpy="-0.5236 0 0"/>
-  </joint>
-  <link name="solid_0">
-  </link>
-  <joint name="rear left shoulder abduction motor" type="revolute">
-    <parent link="base_link"/>
-    <child link="rear left shoulder"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="50" lower="-0.6" upper="0.5" velocity="10"/>
-    <origin xyz="-0.3084 0.0528 0.012299" rpy="1.5708 -0.00001 -1.570793"/>
-  </joint>
-  <link name="rear left shoulder">
-    <visual>
-      <origin xyz="0.004874 0.000873 -0.000643" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.086792 0.124528 0.120755"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.004874 0.000873 -0.000643" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.086792 0.124528 0.120755"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.006447 0.000873 -0.000643" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.098113 0.098113 0.098113"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.006447 0.000873 -0.000643" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.098113 0.098113 0.098113"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.058145 0.000873 -0.000643" rpy="-1.5708 0 1.5708"/>
-      <geometry>
-        <cylinder radius="0.047925" length="0.006038"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.058145 0.000873 -0.000643" rpy="-1.5708 0 1.5708"/>
-      <geometry>
-        <cylinder radius="0.047925" length="0.006038"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="rear left shoulder rotation motor" type="revolute">
-    <parent link="rear left shoulder"/>
-    <child link="rear left upperarm"/>
-    <axis xyz="1 0 0"/>
-    <limit effort="50" lower="-1.7" upper="1.7" velocity="10"/>
-    <origin xyz="0 -0.00053 0" rpy="0 0 0"/>
-  </joint>
-  <link name="rear left upperarm">
-    <visual>
-      <origin xyz="-0.117011 -0.32073 0.1829" rpy="-1.5708 0 1.5708"/>
-      <geometry>
-        <cylinder radius="0.02717" length="0.060377"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.117011 -0.32073 0.1829" rpy="-1.5708 0 1.5708"/>
-      <geometry>
-        <cylinder radius="0.02717" length="0.060377"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.117011 -0.25867 0.180358" rpy="0 0.05 1.570795"/>
-      <geometry>
-        <box size="0.09434 0.060377 0.053208"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.117011 -0.25867 0.180358" rpy="0 0.05 1.570795"/>
-      <geometry>
-        <box size="0.09434 0.060377 0.053208"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.149087 -0.023654 0.007772" rpy="-0.656375 0 0"/>
-      <geometry>
-        <box size="0.016226 0.177358 0.071698"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.149087 -0.023654 0.007772" rpy="-0.656375 0 0"/>
-      <geometry>
-        <box size="0.016226 0.177358 0.071698"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.14944 -0.001095 0.048195" rpy="-0.688653 0.282831 -0.225783"/>
-      <geometry>
-        <box size="0.004528 0.173585 0.015094"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.14944 -0.001095 0.048195" rpy="-0.688653 0.282831 -0.225783"/>
-      <geometry>
-        <box size="0.004528 0.173585 0.015094"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.143963 -0.142325 0.149419" rpy="-0.689784 0.390399 -0.237371"/>
-      <geometry>
-        <box size="0.004528 0.173585 0.015094"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.143963 -0.142325 0.149419" rpy="-0.689784 0.390399 -0.237371"/>
-      <geometry>
-        <box size="0.004528 0.173585 0.015094"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.143376 -0.16295 0.113632" rpy="-0.64577 0.030504 0.039623"/>
-      <geometry>
-        <box size="0.016226 0.173585 0.071698"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.143376 -0.16295 0.113632" rpy="-0.64577 0.030504 0.039623"/>
-      <geometry>
-        <box size="0.016226 0.173585 0.071698"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.113992 -0.21476 0.154045" rpy="-0.656375 0 0"/>
-      <geometry>
-        <box size="0.054717 0.098113 0.064151"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.113992 -0.21476 0.154045" rpy="-0.656375 0 0"/>
-      <geometry>
-        <box size="0.054717 0.098113 0.064151"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.113238 -0.119427 0.086296" rpy="-2.227175 0 0"/>
-      <geometry>
-        <cylinder radius="0.028679" length="0.150943"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.113238 -0.119427 0.086296" rpy="-2.227175 0 0"/>
-      <geometry>
-        <cylinder radius="0.028679" length="0.150943"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.113238 -0.04459 0.028636" rpy="-2.227175 0 0"/>
-      <geometry>
-        <cylinder radius="0.037736" length="0.041509"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.113238 -0.04459 0.028636" rpy="-2.227175 0 0"/>
-      <geometry>
-        <cylinder radius="0.037736" length="0.041509"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.113738 -0.02284 -0.046726" rpy="-0.656375 0 0"/>
-      <geometry>
-        <box size="0.061509 0.069811 0.011321"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.113738 -0.02284 -0.046726" rpy="-0.656375 0 0"/>
-      <geometry>
-        <box size="0.061509 0.069811 0.011321"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.073615 -0.00019 -0.000689" rpy="-1.5708 0 1.5708"/>
-      <geometry>
-        <cylinder radius="0.04" length="0.037736"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.073615 -0.00019 -0.000689" rpy="-1.5708 0 1.5708"/>
-      <geometry>
-        <cylinder radius="0.04" length="0.037736"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.115879 0.008331 -0.011344" rpy="-2.227175 0 0"/>
-      <geometry>
-        <cylinder radius="0.033962" length="0.086792"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.115879 0.008331 -0.011344" rpy="-2.227175 0 0"/>
-      <geometry>
-        <cylinder radius="0.033962" length="0.086792"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.12154 0.053961 -0.047932" rpy="-0.656375 0 0"/>
-      <geometry>
-        <box size="0.064151 0.003774 0.071698"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.12154 0.053961 -0.047932" rpy="-0.656375 0 0"/>
-      <geometry>
-        <box size="0.064151 0.003774 0.071698"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.12003 0.073308 -0.006499" rpy="0.013625 0 0"/>
-      <geometry>
-        <box size="0.060377 0.007547 0.022642"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.12003 0.073308 -0.006499" rpy="0.013625 0 0"/>
-      <geometry>
-        <box size="0.060377 0.007547 0.022642"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.117011 -0.13635 0.154336" rpy="0.99 0 0"/>
-      <geometry>
-        <box size="0.049057 0.005283 0.169811"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.117011 -0.13635 0.154336" rpy="0.99 0 0"/>
-      <geometry>
-        <box size="0.049057 0.005283 0.169811"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.083111 -0.31378 0.182934" rpy="0 -0.076371 1.570798"/>
-      <geometry>
-        <box size="0.044 0.008 0.0387"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.083111 -0.31378 0.182934" rpy="0 -0.076371 1.570798"/>
-      <geometry>
-        <box size="0.044 0.008 0.0387"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="rear left elbow motor" type="revolute">
-    <parent link="rear left upperarm"/>
-    <child link="rear left forearm"/>
-    <axis xyz="1 0 0"/>
-    <limit effort="50" lower="-0.45" upper="1.6" velocity="10"/>
-    <origin xyz="-0.1122 -0.319729 0.182338" rpy="0 0 0"/>
-  </joint>
-  <link name="rear left forearm">
-    <visual>
-      <origin xyz="-0.001038 -0.271321 -0.225038" rpy="-1.5708 0 1.570801"/>
-      <geometry>
-        <cylinder radius="0.035094" length="0.039623"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.001038 -0.271321 -0.225038" rpy="-1.5708 0 1.570801"/>
-      <geometry>
-        <cylinder radius="0.035094" length="0.039623"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.001038 -0.227421 -0.230239" rpy="-0.947675 0 0"/>
-      <geometry>
-        <box size="0.037736 0.006038 0.041509"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.001038 -0.227421 -0.230239" rpy="-0.947675 0 0"/>
-      <geometry>
-        <box size="0.037736 0.006038 0.041509"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.001038 -0.232272 -0.198255" rpy="-0.093185 0 0"/>
-      <geometry>
-        <box size="0.037736 0.045283 0.006792"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.001038 -0.232272 -0.198255" rpy="-0.093185 0 0"/>
-      <geometry>
-        <box size="0.037736 0.045283 0.006792"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.001038 -0.205751 -0.206348" rpy="0.616815 0 0"/>
-      <geometry>
-        <box size="0.025283 0.022642 0.020755"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.001038 -0.205751 -0.206348" rpy="0.616815 0 0"/>
-      <geometry>
-        <box size="0.025283 0.022642 0.020755"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.001038 -0.168611 -0.18111" rpy="-2.513185 0 0"/>
-      <geometry>
-        <box size="0.019245 0.071698 0.017358"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.001038 -0.168611 -0.18111" rpy="-2.513185 0 0"/>
-      <geometry>
-        <box size="0.019245 0.071698 0.017358"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.001038 -0.125111 -0.148236" rpy="0.663625 0 0"/>
-      <geometry>
-        <box size="0.021887 0.037736 0.018491"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.001038 -0.125111 -0.148236" rpy="0.663625 0 0"/>
-      <geometry>
-        <box size="0.021887 0.037736 0.018491"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.001038 -0.100861 -0.126163" rpy="0.760415 0 0"/>
-      <geometry>
-        <box size="0.026415 0.030189 0.021132"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.001038 -0.100861 -0.126163" rpy="0.760415 0 0"/>
-      <geometry>
-        <box size="0.026415 0.030189 0.021132"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.001038 -0.084241 -0.106843" rpy="0.867225 0 0"/>
-      <geometry>
-        <box size="0.032453 0.022642 0.024528"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.001038 -0.084241 -0.106843" rpy="0.867225 0 0"/>
-      <geometry>
-        <box size="0.032453 0.022642 0.024528"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.001038 -0.068551 -0.08661" rpy="0.907225 0 0"/>
-      <geometry>
-        <box size="0.037736 0.033962 0.026415"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.001038 -0.068551 -0.08661" rpy="0.907225 0 0"/>
-      <geometry>
-        <box size="0.037736 0.033962 0.026415"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.001038 -0.049721 -0.059227" rpy="0.933245 0 0"/>
-      <geometry>
-        <cylinder radius="0.024528" length="0.030566"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.001038 -0.049721 -0.059227" rpy="0.933245 0 0"/>
-      <geometry>
-        <cylinder radius="0.024528" length="0.030566"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.001038 -0.034991 -0.038166" rpy="0.937225 0 0"/>
-      <geometry>
-        <box size="0.038113 0.022642 0.032075"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.001038 -0.034991 -0.038166" rpy="0.937225 0 0"/>
-      <geometry>
-        <box size="0.038113 0.022642 0.032075"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.001038 -0.022931 -0.021744" rpy="0.937225 0 0"/>
-      <geometry>
-        <box size="0.031321 0.018868 0.03283"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.001038 -0.022931 -0.021744" rpy="0.937225 0 0"/>
-      <geometry>
-        <box size="0.031321 0.018868 0.03283"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.001038 -0.010711 -0.006332" rpy="0.834035 0 0"/>
-      <geometry>
-        <box size="0.027925 0.018868 0.03283"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.001038 -0.010711 -0.006332" rpy="0.834035 0 0"/>
-      <geometry>
-        <box size="0.027925 0.018868 0.03283"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.001038 0.004299 0.006862" rpy="0.680845 0 0"/>
-      <geometry>
-        <box size="0.027547 0.018868 0.034717"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.001038 0.004299 0.006862" rpy="0.680845 0 0"/>
-      <geometry>
-        <box size="0.027547 0.018868 0.034717"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.001038 0.021259 0.019851" rpy="0.604465 0 0"/>
-      <geometry>
-        <box size="0.020377 0.026415 0.03434"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.001038 0.021259 0.019851" rpy="0.604465 0 0"/>
-      <geometry>
-        <box size="0.020377 0.026415 0.03434"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.001038 0.019179 -0.009531" rpy="0.494895 0 0"/>
-      <geometry>
-        <box size="0.010189 0.05283 0.00717"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.001038 0.019179 -0.009531" rpy="0.494895 0 0"/>
-      <geometry>
-        <box size="0.010189 0.05283 0.00717"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.001038 0.037459 0.024742" rpy="-1.5708 0 1.5708"/>
-      <geometry>
-        <cylinder radius="0.021887" length="0.019736"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.001038 0.037459 0.024742" rpy="-1.5708 0 1.5708"/>
-      <geometry>
-        <cylinder radius="0.021887" length="0.019736"/>
-      </geometry>
-    </collision>
-  </link>
-  <link name="rear left touch sensor">
-  </link>
-  <joint name="rear left forearm_rear left touch sensor_joint" type="fixed">
-    <parent link="rear left forearm"/>
-    <child link="rear left touch sensor"/>
-    <origin xyz="0.001038 -0.271321 -0.225038" rpy="-1.5708 0 1.570801"/>
-  </joint>
-  <joint name="rear left piston motor" type="continuous">
-    <parent link="rear left upperarm"/>
-    <child link="solid_1"/>
-    <axis xyz="1 0 0"/>
-    <limit effort="10" velocity="10"/>
-    <origin xyz="-0.1172 -0.29077 0.2083" rpy="-0.5236 0 0"/>
-  </joint>
-  <link name="solid_1">
-  </link>
-  <joint name="front right shoulder abduction motor" type="revolute">
-    <parent link="base_link"/>
-    <child link="front right shoulder"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="50" lower="-0.6" upper="0.5" velocity="10"/>
-    <origin xyz="0.3635 -0.0528 0.012401" rpy="1.5708 -0.00001 -1.570793"/>
-  </joint>
-  <link name="front right shoulder">
-    <visual>
-      <origin xyz="-0.004874 0.000873 -0.000643" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.086792 0.124528 0.120755"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.004874 0.000873 -0.000643" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.086792 0.124528 0.120755"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.006447 0.000873 -0.000643" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.098113 0.098113 0.098113"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.006447 0.000873 -0.000643" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.098113 0.098113 0.098113"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.058145 0.000873 -0.000643" rpy="-1.5708 0 1.5708"/>
-      <geometry>
-        <cylinder radius="0.047925" length="0.006038"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.058145 0.000873 -0.000643" rpy="-1.5708 0 1.5708"/>
-      <geometry>
-        <cylinder radius="0.047925" length="0.006038"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="front right shoulder rotation motor" type="revolute">
-    <parent link="front right shoulder"/>
-    <child link="front right upperarm"/>
-    <axis xyz="1 0 0"/>
-    <limit effort="50" lower="-1.7" upper="1.7" velocity="10"/>
-    <origin xyz="0 -0.00053 0" rpy="0 0 0"/>
-  </joint>
-  <link name="front right upperarm">
-    <visual>
-      <origin xyz="0.117011 -0.32073 0.1829" rpy="-1.5708 0 1.5708"/>
-      <geometry>
-        <cylinder radius="0.02717" length="0.060377"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.117011 -0.32073 0.1829" rpy="-1.5708 0 1.5708"/>
-      <geometry>
-        <cylinder radius="0.02717" length="0.060377"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.117011 -0.25867 0.180358" rpy="0 0.05 1.570795"/>
-      <geometry>
-        <box size="0.09434 0.060377 0.053208"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.117011 -0.25867 0.180358" rpy="0 0.05 1.570795"/>
-      <geometry>
-        <box size="0.09434 0.060377 0.053208"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.149087 -0.023654 0.007772" rpy="-0.656375 0 0"/>
-      <geometry>
-        <box size="0.016226 0.177358 0.071698"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.149087 -0.023654 0.007772" rpy="-0.656375 0 0"/>
-      <geometry>
-        <box size="0.016226 0.177358 0.071698"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.14944 -0.001095 0.048195" rpy="-0.688653 0.282831 -0.225783"/>
-      <geometry>
-        <box size="0.004528 0.173585 0.015094"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.14944 -0.001095 0.048195" rpy="-0.688653 0.282831 -0.225783"/>
-      <geometry>
-        <box size="0.004528 0.173585 0.015094"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.143963 -0.142325 0.149419" rpy="-0.689784 0.390399 -0.237371"/>
-      <geometry>
-        <box size="0.004528 0.173585 0.015094"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.143963 -0.142325 0.149419" rpy="-0.689784 0.390399 -0.237371"/>
-      <geometry>
-        <box size="0.004528 0.173585 0.015094"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.143376 -0.16295 0.113632" rpy="-0.64577 0.030504 0.039623"/>
-      <geometry>
-        <box size="0.016226 0.173585 0.071698"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.143376 -0.16295 0.113632" rpy="-0.64577 0.030504 0.039623"/>
-      <geometry>
-        <box size="0.016226 0.173585 0.071698"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.113992 -0.21476 0.154045" rpy="-0.656375 0 0"/>
-      <geometry>
-        <box size="0.054717 0.098113 0.064151"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.113992 -0.21476 0.154045" rpy="-0.656375 0 0"/>
-      <geometry>
-        <box size="0.054717 0.098113 0.064151"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.113238 -0.119427 0.086296" rpy="-2.227175 0 0"/>
-      <geometry>
-        <cylinder radius="0.028679" length="0.150943"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.113238 -0.119427 0.086296" rpy="-2.227175 0 0"/>
-      <geometry>
-        <cylinder radius="0.028679" length="0.150943"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.113238 -0.04459 0.028636" rpy="-2.227175 0 0"/>
-      <geometry>
-        <cylinder radius="0.037736" length="0.041509"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.113238 -0.04459 0.028636" rpy="-2.227175 0 0"/>
-      <geometry>
-        <cylinder radius="0.037736" length="0.041509"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.113738 -0.02284 -0.046726" rpy="-0.656375 0 0"/>
-      <geometry>
-        <box size="0.061509 0.069811 0.011321"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.113738 -0.02284 -0.046726" rpy="-0.656375 0 0"/>
-      <geometry>
-        <box size="0.061509 0.069811 0.011321"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.073615 -0.00019 -0.000689" rpy="-1.5708 0 1.5708"/>
-      <geometry>
-        <cylinder radius="0.04" length="0.037736"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.073615 -0.00019 -0.000689" rpy="-1.5708 0 1.5708"/>
-      <geometry>
-        <cylinder radius="0.04" length="0.037736"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.115879 0.008331 -0.011344" rpy="-2.227175 0 0"/>
-      <geometry>
-        <cylinder radius="0.033962" length="0.086792"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.115879 0.008331 -0.011344" rpy="-2.227175 0 0"/>
-      <geometry>
-        <cylinder radius="0.033962" length="0.086792"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.12154 0.053961 -0.047932" rpy="-0.656375 0 0"/>
-      <geometry>
-        <box size="0.064151 0.003774 0.071698"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.12154 0.053961 -0.047932" rpy="-0.656375 0 0"/>
-      <geometry>
-        <box size="0.064151 0.003774 0.071698"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.12003 0.073308 -0.006499" rpy="0.013625 0 0"/>
-      <geometry>
-        <box size="0.060377 0.007547 0.022642"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.12003 0.073308 -0.006499" rpy="0.013625 0 0"/>
-      <geometry>
-        <box size="0.060377 0.007547 0.022642"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.117011 -0.13635 0.154336" rpy="0.99 0 0"/>
-      <geometry>
-        <box size="0.049057 0.005283 0.169811"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.117011 -0.13635 0.154336" rpy="0.99 0 0"/>
-      <geometry>
-        <box size="0.049057 0.005283 0.169811"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.083111 -0.31378 0.182934" rpy="0 -0.076371 1.570798"/>
-      <geometry>
-        <box size="0.044 0.008 0.0387"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.083111 -0.31378 0.182934" rpy="0 -0.076371 1.570798"/>
-      <geometry>
-        <box size="0.044 0.008 0.0387"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="front right elbow motor" type="revolute">
-    <parent link="front right upperarm"/>
-    <child link="front right forearm"/>
-    <axis xyz="1 0 0"/>
-    <limit effort="50" lower="-0.45" upper="1.6" velocity="10"/>
-    <origin xyz="0.1122 -0.319729 0.182338" rpy="0 0 0"/>
-  </joint>
-  <link name="front right forearm">
-    <visual>
-      <origin xyz="0.001038 -0.271321 -0.225038" rpy="-1.5708 0 1.570801"/>
-      <geometry>
-        <cylinder radius="0.035094" length="0.039623"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.001038 -0.271321 -0.225038" rpy="-1.5708 0 1.570801"/>
-      <geometry>
-        <cylinder radius="0.035094" length="0.039623"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.001038 -0.227421 -0.230239" rpy="-0.947675 0 0"/>
-      <geometry>
-        <box size="0.037736 0.006038 0.041509"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.001038 -0.227421 -0.230239" rpy="-0.947675 0 0"/>
-      <geometry>
-        <box size="0.037736 0.006038 0.041509"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.001038 -0.232272 -0.198255" rpy="-0.093185 0 0"/>
-      <geometry>
-        <box size="0.037736 0.045283 0.006792"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.001038 -0.232272 -0.198255" rpy="-0.093185 0 0"/>
-      <geometry>
-        <box size="0.037736 0.045283 0.006792"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.001038 -0.205751 -0.206348" rpy="0.616815 0 0"/>
-      <geometry>
-        <box size="0.025283 0.022642 0.020755"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.001038 -0.205751 -0.206348" rpy="0.616815 0 0"/>
-      <geometry>
-        <box size="0.025283 0.022642 0.020755"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.001038 -0.168611 -0.18111" rpy="-2.513185 0 0"/>
-      <geometry>
-        <box size="0.019245 0.071698 0.017358"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.001038 -0.168611 -0.18111" rpy="-2.513185 0 0"/>
-      <geometry>
-        <box size="0.019245 0.071698 0.017358"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.001038 -0.125111 -0.148236" rpy="0.663625 0 0"/>
-      <geometry>
-        <box size="0.021887 0.037736 0.018491"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.001038 -0.125111 -0.148236" rpy="0.663625 0 0"/>
-      <geometry>
-        <box size="0.021887 0.037736 0.018491"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.001038 -0.100861 -0.126163" rpy="0.760415 0 0"/>
-      <geometry>
-        <box size="0.026415 0.030189 0.021132"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.001038 -0.100861 -0.126163" rpy="0.760415 0 0"/>
-      <geometry>
-        <box size="0.026415 0.030189 0.021132"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.001038 -0.084241 -0.106843" rpy="0.867225 0 0"/>
-      <geometry>
-        <box size="0.032453 0.022642 0.024528"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.001038 -0.084241 -0.106843" rpy="0.867225 0 0"/>
-      <geometry>
-        <box size="0.032453 0.022642 0.024528"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.001038 -0.068551 -0.08661" rpy="0.907225 0 0"/>
-      <geometry>
-        <box size="0.037736 0.033962 0.026415"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.001038 -0.068551 -0.08661" rpy="0.907225 0 0"/>
-      <geometry>
-        <box size="0.037736 0.033962 0.026415"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.001038 -0.049721 -0.059227" rpy="0.933245 0 0"/>
-      <geometry>
-        <cylinder radius="0.024528" length="0.030566"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.001038 -0.049721 -0.059227" rpy="0.933245 0 0"/>
-      <geometry>
-        <cylinder radius="0.024528" length="0.030566"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.001038 -0.034991 -0.038166" rpy="0.937225 0 0"/>
-      <geometry>
-        <box size="0.038113 0.022642 0.032075"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.001038 -0.034991 -0.038166" rpy="0.937225 0 0"/>
-      <geometry>
-        <box size="0.038113 0.022642 0.032075"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.001038 -0.022931 -0.021744" rpy="0.937225 0 0"/>
-      <geometry>
-        <box size="0.031321 0.018868 0.03283"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.001038 -0.022931 -0.021744" rpy="0.937225 0 0"/>
-      <geometry>
-        <box size="0.031321 0.018868 0.03283"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.001038 -0.010711 -0.006332" rpy="0.834035 0 0"/>
-      <geometry>
-        <box size="0.027925 0.018868 0.03283"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.001038 -0.010711 -0.006332" rpy="0.834035 0 0"/>
-      <geometry>
-        <box size="0.027925 0.018868 0.03283"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.001038 0.004299 0.006862" rpy="0.680845 0 0"/>
-      <geometry>
-        <box size="0.027547 0.018868 0.034717"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.001038 0.004299 0.006862" rpy="0.680845 0 0"/>
-      <geometry>
-        <box size="0.027547 0.018868 0.034717"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.001038 0.021259 0.019851" rpy="0.604465 0 0"/>
-      <geometry>
-        <box size="0.020377 0.026415 0.03434"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.001038 0.021259 0.019851" rpy="0.604465 0 0"/>
-      <geometry>
-        <box size="0.020377 0.026415 0.03434"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.001038 0.019179 -0.009531" rpy="0.494895 0 0"/>
-      <geometry>
-        <box size="0.010189 0.05283 0.00717"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.001038 0.019179 -0.009531" rpy="0.494895 0 0"/>
-      <geometry>
-        <box size="0.010189 0.05283 0.00717"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0.001038 0.037459 0.024742" rpy="-1.5708 0 1.5708"/>
-      <geometry>
-        <cylinder radius="0.021887" length="0.019736"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.001038 0.037459 0.024742" rpy="-1.5708 0 1.5708"/>
-      <geometry>
-        <cylinder radius="0.021887" length="0.019736"/>
-      </geometry>
-    </collision>
-  </link>
-  <link name="front right touch sensor">
-  </link>
-  <joint name="front right forearm_front right touch sensor_joint" type="fixed">
-    <parent link="front right forearm"/>
-    <child link="front right touch sensor"/>
-    <origin xyz="0.001038 -0.271321 -0.225038" rpy="-1.5708 0 1.570801"/>
-  </joint>
-  <joint name="front right piston motor" type="continuous">
-    <parent link="front right upperarm"/>
-    <child link="solid_2"/>
-    <axis xyz="1 0 0"/>
-    <limit effort="10" velocity="10"/>
-    <origin xyz="0.1172 -0.29077 0.2083" rpy="-0.5236 0 0"/>
-  </joint>
-  <link name="solid_2">
-  </link>
-  <joint name="front left shoulder abduction motor" type="revolute">
-    <parent link="base_link"/>
-    <child link="front left shoulder"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="50" lower="-0.6" upper="0.5" velocity="10"/>
-    <origin xyz="0.3635 0.0528 0.012399" rpy="1.5708 -0.00001 -1.570793"/>
-  </joint>
-  <link name="front left shoulder">
-    <visual>
-      <origin xyz="0.004874 0.000873 -0.000643" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.086792 0.124528 0.120755"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.004874 0.000873 -0.000643" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.086792 0.124528 0.120755"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.006447 0.000873 -0.000643" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.098113 0.098113 0.098113"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.006447 0.000873 -0.000643" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.098113 0.098113 0.098113"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.058145 0.000873 -0.000643" rpy="-1.5708 0 1.5708"/>
-      <geometry>
-        <cylinder radius="0.047925" length="0.006038"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.058145 0.000873 -0.000643" rpy="-1.5708 0 1.5708"/>
-      <geometry>
-        <cylinder radius="0.047925" length="0.006038"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="front left shoulder rotation motor" type="revolute">
-    <parent link="front left shoulder"/>
-    <child link="front left upperarm"/>
-    <axis xyz="1 0 0"/>
-    <limit effort="50" lower="-1.7" upper="1.7" velocity="10"/>
-    <origin xyz="0 -0.00053 0" rpy="0 0 0"/>
-  </joint>
-  <link name="front left upperarm">
-    <visual>
-      <origin xyz="-0.117011 -0.32073 0.1829" rpy="-1.5708 0 1.5708"/>
-      <geometry>
-        <cylinder radius="0.02717" length="0.060377"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.117011 -0.32073 0.1829" rpy="-1.5708 0 1.5708"/>
-      <geometry>
-        <cylinder radius="0.02717" length="0.060377"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.117011 -0.25867 0.180358" rpy="0 0.05 1.570795"/>
-      <geometry>
-        <box size="0.09434 0.060377 0.053208"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.117011 -0.25867 0.180358" rpy="0 0.05 1.570795"/>
-      <geometry>
-        <box size="0.09434 0.060377 0.053208"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.149087 -0.023654 0.007772" rpy="-0.656375 0 0"/>
-      <geometry>
-        <box size="0.016226 0.177358 0.071698"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.149087 -0.023654 0.007772" rpy="-0.656375 0 0"/>
-      <geometry>
-        <box size="0.016226 0.177358 0.071698"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.14944 -0.001095 0.048195" rpy="-0.688653 0.282831 -0.225783"/>
-      <geometry>
-        <box size="0.004528 0.173585 0.015094"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.14944 -0.001095 0.048195" rpy="-0.688653 0.282831 -0.225783"/>
-      <geometry>
-        <box size="0.004528 0.173585 0.015094"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.143963 -0.142325 0.149419" rpy="-0.689784 0.390399 -0.237371"/>
-      <geometry>
-        <box size="0.004528 0.173585 0.015094"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.143963 -0.142325 0.149419" rpy="-0.689784 0.390399 -0.237371"/>
-      <geometry>
-        <box size="0.004528 0.173585 0.015094"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.143376 -0.16295 0.113632" rpy="-0.64577 0.030504 0.039623"/>
-      <geometry>
-        <box size="0.016226 0.173585 0.071698"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.143376 -0.16295 0.113632" rpy="-0.64577 0.030504 0.039623"/>
-      <geometry>
-        <box size="0.016226 0.173585 0.071698"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.113992 -0.21476 0.154045" rpy="-0.656375 0 0"/>
-      <geometry>
-        <box size="0.054717 0.098113 0.064151"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.113992 -0.21476 0.154045" rpy="-0.656375 0 0"/>
-      <geometry>
-        <box size="0.054717 0.098113 0.064151"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.113238 -0.119427 0.086296" rpy="-2.227175 0 0"/>
-      <geometry>
-        <cylinder radius="0.028679" length="0.150943"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.113238 -0.119427 0.086296" rpy="-2.227175 0 0"/>
-      <geometry>
-        <cylinder radius="0.028679" length="0.150943"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.113238 -0.04459 0.028636" rpy="-2.227175 0 0"/>
-      <geometry>
-        <cylinder radius="0.037736" length="0.041509"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.113238 -0.04459 0.028636" rpy="-2.227175 0 0"/>
-      <geometry>
-        <cylinder radius="0.037736" length="0.041509"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.113738 -0.02284 -0.046726" rpy="-0.656375 0 0"/>
-      <geometry>
-        <box size="0.061509 0.069811 0.011321"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.113738 -0.02284 -0.046726" rpy="-0.656375 0 0"/>
-      <geometry>
-        <box size="0.061509 0.069811 0.011321"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.073615 -0.00019 -0.000689" rpy="-1.5708 0 1.5708"/>
-      <geometry>
-        <cylinder radius="0.04" length="0.037736"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.073615 -0.00019 -0.000689" rpy="-1.5708 0 1.5708"/>
-      <geometry>
-        <cylinder radius="0.04" length="0.037736"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.115879 0.008331 -0.011344" rpy="-2.227175 0 0"/>
-      <geometry>
-        <cylinder radius="0.033962" length="0.086792"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.115879 0.008331 -0.011344" rpy="-2.227175 0 0"/>
-      <geometry>
-        <cylinder radius="0.033962" length="0.086792"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.12154 0.053961 -0.047932" rpy="-0.656375 0 0"/>
-      <geometry>
-        <box size="0.064151 0.003774 0.071698"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.12154 0.053961 -0.047932" rpy="-0.656375 0 0"/>
-      <geometry>
-        <box size="0.064151 0.003774 0.071698"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.12003 0.073308 -0.006499" rpy="0.013625 0 0"/>
-      <geometry>
-        <box size="0.060377 0.007547 0.022642"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.12003 0.073308 -0.006499" rpy="0.013625 0 0"/>
-      <geometry>
-        <box size="0.060377 0.007547 0.022642"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.117011 -0.13635 0.154336" rpy="0.99 0 0"/>
-      <geometry>
-        <box size="0.049057 0.005283 0.169811"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.117011 -0.13635 0.154336" rpy="0.99 0 0"/>
-      <geometry>
-        <box size="0.049057 0.005283 0.169811"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.083111 -0.31378 0.182934" rpy="0 -0.076371 1.570798"/>
-      <geometry>
-        <box size="0.044 0.008 0.0387"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.083111 -0.31378 0.182934" rpy="0 -0.076371 1.570798"/>
-      <geometry>
-        <box size="0.044 0.008 0.0387"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="front left elbow motor" type="revolute">
-    <parent link="front left upperarm"/>
-    <child link="front left forearm"/>
-    <axis xyz="1 0 0"/>
-    <limit effort="50" lower="-0.45" upper="1.6" velocity="10"/>
-    <origin xyz="-0.1122 -0.319729 0.182338" rpy="0 0 0"/>
-  </joint>
-  <link name="front left forearm">
-    <visual>
-      <origin xyz="-0.001038 -0.271321 -0.225038" rpy="-1.5708 0 1.570801"/>
-      <geometry>
-        <cylinder radius="0.035094" length="0.039623"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.001038 -0.271321 -0.225038" rpy="-1.5708 0 1.570801"/>
-      <geometry>
-        <cylinder radius="0.035094" length="0.039623"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.001038 -0.227421 -0.230239" rpy="-0.947675 0 0"/>
-      <geometry>
-        <box size="0.037736 0.006038 0.041509"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.001038 -0.227421 -0.230239" rpy="-0.947675 0 0"/>
-      <geometry>
-        <box size="0.037736 0.006038 0.041509"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.001038 -0.232272 -0.198255" rpy="-0.093185 0 0"/>
-      <geometry>
-        <box size="0.037736 0.045283 0.006792"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.001038 -0.232272 -0.198255" rpy="-0.093185 0 0"/>
-      <geometry>
-        <box size="0.037736 0.045283 0.006792"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.001038 -0.205751 -0.206348" rpy="0.616815 0 0"/>
-      <geometry>
-        <box size="0.025283 0.022642 0.020755"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.001038 -0.205751 -0.206348" rpy="0.616815 0 0"/>
-      <geometry>
-        <box size="0.025283 0.022642 0.020755"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.001038 -0.168611 -0.18111" rpy="-2.513185 0 0"/>
-      <geometry>
-        <box size="0.019245 0.071698 0.017358"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.001038 -0.168611 -0.18111" rpy="-2.513185 0 0"/>
-      <geometry>
-        <box size="0.019245 0.071698 0.017358"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.001038 -0.125111 -0.148236" rpy="0.663625 0 0"/>
-      <geometry>
-        <box size="0.021887 0.037736 0.018491"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.001038 -0.125111 -0.148236" rpy="0.663625 0 0"/>
-      <geometry>
-        <box size="0.021887 0.037736 0.018491"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.001038 -0.100861 -0.126163" rpy="0.760415 0 0"/>
-      <geometry>
-        <box size="0.026415 0.030189 0.021132"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.001038 -0.100861 -0.126163" rpy="0.760415 0 0"/>
-      <geometry>
-        <box size="0.026415 0.030189 0.021132"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.001038 -0.084241 -0.106843" rpy="0.867225 0 0"/>
-      <geometry>
-        <box size="0.032453 0.022642 0.024528"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.001038 -0.084241 -0.106843" rpy="0.867225 0 0"/>
-      <geometry>
-        <box size="0.032453 0.022642 0.024528"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.001038 -0.068551 -0.08661" rpy="0.907225 0 0"/>
-      <geometry>
-        <box size="0.037736 0.033962 0.026415"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.001038 -0.068551 -0.08661" rpy="0.907225 0 0"/>
-      <geometry>
-        <box size="0.037736 0.033962 0.026415"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.001038 -0.049721 -0.059227" rpy="0.933245 0 0"/>
-      <geometry>
-        <cylinder radius="0.024528" length="0.030566"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.001038 -0.049721 -0.059227" rpy="0.933245 0 0"/>
-      <geometry>
-        <cylinder radius="0.024528" length="0.030566"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.001038 -0.034991 -0.038166" rpy="0.937225 0 0"/>
-      <geometry>
-        <box size="0.038113 0.022642 0.032075"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.001038 -0.034991 -0.038166" rpy="0.937225 0 0"/>
-      <geometry>
-        <box size="0.038113 0.022642 0.032075"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.001038 -0.022931 -0.021744" rpy="0.937225 0 0"/>
-      <geometry>
-        <box size="0.031321 0.018868 0.03283"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.001038 -0.022931 -0.021744" rpy="0.937225 0 0"/>
-      <geometry>
-        <box size="0.031321 0.018868 0.03283"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.001038 -0.010711 -0.006332" rpy="0.834035 0 0"/>
-      <geometry>
-        <box size="0.027925 0.018868 0.03283"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.001038 -0.010711 -0.006332" rpy="0.834035 0 0"/>
-      <geometry>
-        <box size="0.027925 0.018868 0.03283"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.001038 0.004299 0.006862" rpy="0.680845 0 0"/>
-      <geometry>
-        <box size="0.027547 0.018868 0.034717"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.001038 0.004299 0.006862" rpy="0.680845 0 0"/>
-      <geometry>
-        <box size="0.027547 0.018868 0.034717"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.001038 0.021259 0.019851" rpy="0.604465 0 0"/>
-      <geometry>
-        <box size="0.020377 0.026415 0.03434"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.001038 0.021259 0.019851" rpy="0.604465 0 0"/>
-      <geometry>
-        <box size="0.020377 0.026415 0.03434"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.001038 0.019179 -0.009531" rpy="0.494895 0 0"/>
-      <geometry>
-        <box size="0.010189 0.05283 0.00717"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.001038 0.019179 -0.009531" rpy="0.494895 0 0"/>
-      <geometry>
-        <box size="0.010189 0.05283 0.00717"/>
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="-0.001038 0.037459 0.024742" rpy="-1.5708 0 1.5708"/>
-      <geometry>
-        <cylinder radius="0.021887" length="0.019736"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="-0.001038 0.037459 0.024742" rpy="-1.5708 0 1.5708"/>
-      <geometry>
-        <cylinder radius="0.021887" length="0.019736"/>
-      </geometry>
-    </collision>
-  </link>
-  <link name="front left touch sensor">
-  </link>
-  <joint name="front left forearm_front left touch sensor_joint" type="fixed">
-    <parent link="front left forearm"/>
-    <child link="front left touch sensor"/>
-    <origin xyz="0.001038 -0.271321 -0.225038" rpy="-1.5708 0 1.570801"/>
-  </joint>
-  <joint name="front left piston motor" type="continuous">
-    <parent link="front left upperarm"/>
-    <child link="solid_3"/>
-    <axis xyz="1 0 0"/>
-    <limit effort="10" velocity="10"/>
-    <origin xyz="-0.1172 -0.29077 0.2083" rpy="-0.5236 0 0"/>
-  </joint>
-  <link name="solid_3">
-  </link>
-  <link name="rear depth">
-  </link>
-  <joint name="base_link_rear depth_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="rear depth"/>
-    <origin xyz="-0.448136 -0.018115 0.012558" rpy="0.000015 0.474984 -3.141579"/>
-  </joint>
-  <link name="rear camera">
-  </link>
-  <joint name="base_link_rear camera_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="rear camera"/>
-    <origin xyz="-0.448136 -0.018115 0.012558" rpy="-1.570781 0.474984 -4.712375"/>
-  </joint>
-  <link name="left flank depth">
-  </link>
-  <joint name="base_link_left flank depth_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="left flank depth"/>
-    <origin xyz="-0.13568 0.110127 0.065528" rpy="-0.000003 0.403191 1.570802"/>
-  </joint>
-  <link name="left flank camera">
-  </link>
-  <joint name="base_link_left flank camera_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="left flank camera"/>
-    <origin xyz="-0.13568 0.110127 0.065528" rpy="-1.570799 0.403191 0.000005"/>
-  </joint>
-  <link name="right flank depth">
-  </link>
-  <joint name="base_link_right flank depth_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="right flank depth"/>
-    <origin xyz="-0.135678 -0.110163 0.06561" rpy="0.000005 0.413171 -1.570794"/>
-  </joint>
-  <link name="right flank camera">
-  </link>
-  <joint name="base_link_right flank camera_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="right flank camera"/>
-    <origin xyz="-0.135678 -0.110163 0.06561" rpy="-1.570791 0.413171 -3.141591"/>
-  </joint>
-  <link name="left head depth">
-  </link>
-  <joint name="base_link_left head depth_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="left head depth"/>
-    <origin xyz="0.46325 0.045111 0.029551" rpy="-0.158862 0.433786 -0.408004"/>
-  </joint>
-  <link name="left head camera">
-  </link>
-  <joint name="base_link_left head camera_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="left head camera"/>
-    <origin xyz="0.46325 0.045111 0.029551" rpy="-1.729658 0.433786 -1.9788"/>
-  </joint>
-  <link name="right head depth">
-  </link>
-  <joint name="base_link_right head depth_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="right head depth"/>
-    <origin xyz="0.462603 -0.044896 0.027972" rpy="0.193959 0.3931 0.409605"/>
-  </joint>
-  <link name="right head camera">
-  </link>
-  <joint name="base_link_right head camera_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="right head camera"/>
-    <origin xyz="0.462603 -0.044896 0.027972" rpy="-1.376837 0.3931 -1.161192"/>
-  </joint>
-  <link name="left bottom led">
-  </link>
-  <joint name="base_link_left bottom led_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="left bottom led"/>
-    <origin xyz="0.481728 0.081967 0.001191" rpy="1.873112 0.329178 0.767486"/>
-  </joint>
-  <link name="right bottom led">
-  </link>
-  <joint name="base_link_right bottom led_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="right bottom led"/>
-    <origin xyz="0.481728 -0.081963 0.001193" rpy="1.279539 0.338923 -0.733718"/>
-  </joint>
-  <link name="right middle down led">
-  </link>
-  <joint name="base_link_right middle down led_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="right middle down led"/>
-    <origin xyz="0.489879 -0.081963 0.018293" rpy="1.279539 0.338923 -0.733718"/>
-  </joint>
-  <link name="right middle up led">
-  </link>
-  <joint name="base_link_right middle up led_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="right middle up led"/>
-    <origin xyz="0.497601 -0.081963 0.034493" rpy="1.279539 0.338923 -0.733718"/>
-  </joint>
-  <link name="right top led">
-  </link>
-  <joint name="base_link_right top led_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="right top led"/>
-    <origin xyz="0.505323 -0.081963 0.050723" rpy="1.279539 0.338923 -0.733718"/>
-  </joint>
-  <link name="left middle down led">
-  </link>
-  <joint name="base_link_left middle down led_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="left middle down led"/>
-    <origin xyz="0.489879 0.081966 0.018291" rpy="1.873112 0.329178 0.767486"/>
-  </joint>
-  <link name="left middle up led">
-  </link>
-  <joint name="base_link_left middle up led_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="left middle up led"/>
-    <origin xyz="0.497601 0.081967 0.034491" rpy="1.873112 0.329178 0.767486"/>
-  </joint>
-  <link name="left top led">
-  </link>
-  <joint name="base_link_left top led_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="left top led"/>
-    <origin xyz="0.505323 0.081967 0.050721" rpy="1.873112 0.329178 0.767486"/>
-  </joint>
-  <link name="inertial unit">
-  </link>
-  <joint name="base_link_inertial unit_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="inertial unit"/>
-    <origin xyz="0 0 0" rpy="0 0 0"/>
-  </joint>
-  <link name="gyro">
-  </link>
-  <joint name="base_link_gyro_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="gyro"/>
-    <origin xyz="0 0 0" rpy="0 0 0"/>
-  </joint>
-  <link name="gps">
-  </link>
-  <joint name="base_link_gps_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="gps"/>
-    <origin xyz="0 0 0" rpy="0 0 0"/>
-  </joint>
-  <link name="accelerometer">
-  </link>
-  <joint name="base_link_accelerometer_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="accelerometer"/>
-    <origin xyz="0 0 0" rpy="0 0 0"/>
-  </joint>
+
+  <webots>
+    <plugin type="webots_spot.spot_driver.SpotDriver" >
+      <arena3>false</arena3>
+    </plugin>
+
+    <plugin type="webots_ros2_driver::Ros2IMU">
+      <enabled>true</enabled>
+      <topicName>/Spot/imu</topicName>
+      <alwaysOn>true</alwaysOn>
+      <frameName>base_link</frameName>
+      <inertialUnitName>inertial unit</inertialUnitName>
+      <gyroName>gyro</gyroName>
+      <accelerometerName>accelerometer</accelerometerName>
+    </plugin>
+  </webots>
+  
 </robot>
diff --git a/resource/spot_control.urdf b/resource/spot_control.urdf
deleted file mode 100644
index acb8d40..0000000
--- a/resource/spot_control.urdf
+++ /dev/null
@@ -1,18 +0,0 @@
-<?xml version="1.0" ?>
-<robot name="Spot">
-    <webots>
-        <plugin type="webots_spot.spot_driver.SpotDriver" >
-            <arena3>false</arena3>
-        </plugin>
-
-        <plugin type="webots_ros2_driver::Ros2IMU">
-            <enabled>true</enabled>
-            <topicName>/Spot/imu</topicName>
-            <alwaysOn>true</alwaysOn>
-            <frameName>base_link</frameName>
-            <inertialUnitName>inertial unit</inertialUnitName>
-            <gyroName>gyro</gyroName>
-            <accelerometerName>accelerometer</accelerometerName>
-        </plugin>
-    </webots>
-</robot>
diff --git a/resource/spotarm.urdf b/resource/spotarm.urdf
new file mode 100644
index 0000000..aa560a7
--- /dev/null
+++ b/resource/spotarm.urdf
@@ -0,0 +1,2875 @@
+<?xml version="1.0"?>
+<robot name="SpotArm">
+  <webots>
+    <plugin type="webots_ros2_control::Ros2Control"/>
+  </webots>
+  <ros2_control name="WebotsControl" type="system">
+    <hardware>
+      <plugin>webots_ros2_control::Ros2ControlSystem</plugin>
+    </hardware>
+
+    <!-- SpotArm -->
+    <joint name="spotarm_1_joint">
+      <command_interface name="position">
+        </command_interface>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+    </joint>
+    <joint name="spotarm_2_joint">
+      <command_interface name="position">
+        </command_interface>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+    </joint>
+    <joint name="spotarm_3_joint">
+      <command_interface name="position">
+        </command_interface>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+    </joint>
+    <joint name="spotarm_4_joint">
+      <command_interface name="position">
+        </command_interface>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+    </joint>
+    <joint name="Slider11">
+      <command_interface name="position">
+        </command_interface>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+    </joint>
+    <joint name="spotarm_5_joint">
+      <command_interface name="position">
+        </command_interface>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+    </joint>
+    <joint name="spotarm_6_joint">
+      <command_interface name="position">
+        </command_interface>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+    </joint>
+
+    <!-- Tiago Gripper -->
+    <joint name="gripper_left_finger_joint">
+      <command_interface name="position">
+        </command_interface>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+    </joint>
+    <joint name="gripper_right_finger_joint">
+      <command_interface name="position">
+        </command_interface>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+    </joint>
+  </ros2_control>
+
+  <link name="base_link">
+    <visual>
+      <origin xyz="0.462094 0 -0.000232" rpy="1.127615 -0.00001 -1.570793"/>
+      <geometry>
+        <box size="0.192453 0.177358 0.043396"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.462094 0 -0.000232" rpy="1.127615 -0.00001 -1.570793"/>
+      <geometry>
+        <box size="0.192453 0.177358 0.043396"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.477932 0 0.084013" rpy="1.217205 -0.000011 -1.570792"/>
+      <geometry>
+        <box size="0.192453 0.018113 0.08717"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.477932 0 0.084013" rpy="1.217205 -0.000011 -1.570792"/>
+      <geometry>
+        <box size="0.192453 0.018113 0.08717"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.451833 0 0.041304" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.037736 0.192453 0.102642"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.451833 0 0.041304" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.037736 0.192453 0.102642"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.031113 0 0.018833" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.535849 0.222642 0.189434"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.031113 0 0.018833" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.535849 0.222642 0.189434"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.376019 0 0.107135" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.158491 0.166038 0.013208"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.376019 0 0.107135" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.158491 0.166038 0.013208"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.363578 0 -0.079657" rpy="1.537614 -0.00001 -1.570793"/>
+      <geometry>
+        <box size="0.166038 0.013208 0.124528"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.363578 0 -0.079657" rpy="1.537614 -0.00001 -1.570793"/>
+      <geometry>
+        <box size="0.166038 0.013208 0.124528"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.319453 0 0.107135" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.158491 0.166038 0.013208"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.319453 0 0.107135" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.158491 0.166038 0.013208"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.313025 0 -0.064563" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.14717 0.166038 0.013208"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.313025 0 -0.064563" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.14717 0.166038 0.013208"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.120195 0.111629 0.074886" rpy="1.5708 -0.423195 -1.570794"/>
+      <geometry>
+        <box size="0.030189 0.064528 0.182264"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.120195 0.111629 0.074886" rpy="1.5708 -0.423195 -1.570794"/>
+      <geometry>
+        <box size="0.030189 0.064528 0.182264"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.120195 -0.111629 0.074886" rpy="1.5708 0.423174 -1.570793"/>
+      <geometry>
+        <box size="0.030189 0.064528 0.182264"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.120195 -0.111629 0.074886" rpy="1.5708 0.423174 -1.570793"/>
+      <geometry>
+        <box size="0.030189 0.064528 0.182264"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.427377 0 0.059588" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.089057 0.192453 0.05283"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.427377 0 0.059588" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.089057 0.192453 0.05283"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.404 0 0.096611" rpy="1.730798 -0.000011 -1.570794"/>
+      <geometry>
+        <box size="0.193585 0.003774 0.135849"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.404 0 0.096611" rpy="1.730798 -0.000011 -1.570794"/>
+      <geometry>
+        <box size="0.193585 0.003774 0.135849"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.400585 0 -0.018903" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.04566 0.191698 0.103396"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.400585 0 -0.018903" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.04566 0.191698 0.103396"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.431528 0 -0.008337" rpy="2.0308 -0.00001 -1.570793"/>
+      <geometry>
+        <box size="0.193585 0.109434 0.037736"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.431528 0 -0.008337" rpy="2.0308 -0.00001 -1.570793"/>
+      <geometry>
+        <box size="0.193585 0.109434 0.037736"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.28 0 0.121" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.05 0.1 0.02"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.28 0 0.121" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.05 0.1 0.02"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.02 0 0.121" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.05 0.1 0.02"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.02 0 0.121" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.05 0.1 0.02"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="Velodyne_Puck">
+    <visual>
+      <origin xyz="0 -0.00165 0" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.052" length="0.0727"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0 -0.00165 0" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.052" length="0.0727"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="base_link_Velodyne_Puck_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="Velodyne_Puck"/>
+    <origin xyz="-0.063 0 0.1518" rpy="-0.000008 -0.000008 0.000001"/>
+  </joint>
+  <link name="solid">
+  </link>
+  <joint name="base_link_solid_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="solid"/>
+    <origin xyz="0.496831 0.000001 0.104801" rpy="1.2208 -0.00001 -1.570793"/>
+  </joint>
+  <link name="kinect range">
+  </link>
+  <joint name="solid_kinect range_joint" type="fixed">
+    <parent link="solid"/>
+    <child link="kinect range"/>
+    <origin xyz="0 0 -0.0127" rpy="-0.785397 1.570792 0.785397"/>
+  </joint>
+  <link name="kinect color">
+  </link>
+  <joint name="solid_kinect color_joint" type="fixed">
+    <parent link="solid"/>
+    <child link="kinect color"/>
+    <origin xyz="0 0 -0.0127" rpy="-2.356193 1.570792 -0.7854"/>
+  </joint>
+  <link name="SpotArm">
+    <visual>
+      <origin xyz="0 0 0.021" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.075" length="0.035"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0.021" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.075" length="0.035"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="base_link_SpotArm_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="SpotArm"/>
+    <origin xyz="0.35 0 0.1118" rpy="-0.000017 -0.000003 0.000005"/>
+  </joint>
+  <joint name="spotarm_1_joint" type="revolute">
+    <parent link="SpotArm"/>
+    <child link="spotarm_link1"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="500" lower="-2.61799" upper="3.14159" velocity="2.5"/>
+    <origin xyz="0 0 0.04" rpy="0 0 0"/>
+  </joint>
+  <link name="spotarm_link1">
+    <visual>
+      <origin xyz="0 -0.035 0.076" rpy="3.141593 1.570793 3.141593"/>
+      <geometry>
+        <box size="0.14 0.01 0.08"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0 -0.035 0.076" rpy="3.141593 1.570793 3.141593"/>
+      <geometry>
+        <box size="0.14 0.01 0.08"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0 0.035 0.076" rpy="3.141593 1.570793 3.141593"/>
+      <geometry>
+        <box size="0.14 0.01 0.08"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0 0.035 0.076" rpy="3.141593 1.570793 3.141593"/>
+      <geometry>
+        <box size="0.14 0.01 0.08"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0 -0.0555 0.1" rpy="1.5708 0 0"/>
+      <geometry>
+        <cylinder radius="0.032" length="0.031"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0 -0.0555 0.1" rpy="1.5708 0 0"/>
+      <geometry>
+        <cylinder radius="0.032" length="0.031"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0 0.0555 0.1" rpy="1.5708 0 0"/>
+      <geometry>
+        <cylinder radius="0.032" length="0.031"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0 0.0555 0.1" rpy="1.5708 0 0"/>
+      <geometry>
+        <cylinder radius="0.032" length="0.031"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="spotarm_2_joint" type="revolute">
+    <parent link="spotarm_link1"/>
+    <child link="spotarm_link2"/>
+    <axis xyz="0 -1 0"/>
+    <limit effort="500" lower="-0.523599" upper="3.14159" velocity="2.5"/>
+    <origin xyz="0 0.03 0.1" rpy="0 0 0"/>
+  </joint>
+  <link name="spotarm_link2">
+    <visual>
+      <origin xyz="0.017 -0.03 0" rpy="3.141593 1.570793 3.141593"/>
+      <geometry>
+        <box size="0.06 0.059 0.093"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.017 -0.03 0" rpy="3.141593 1.570793 3.141593"/>
+      <geometry>
+        <box size="0.06 0.059 0.093"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.185 -0.03 0" rpy="3.141593 1.570793 3.141593"/>
+      <geometry>
+        <cylinder radius="0.0456" length="0.24"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.185 -0.03 0" rpy="3.141593 1.570793 3.141593"/>
+      <geometry>
+        <cylinder radius="0.0456" length="0.24"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="spotarm_3_joint" type="revolute">
+    <parent link="spotarm_link2"/>
+    <child link="spotarm_link3"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="500" lower="0" upper="3.0" velocity="2.5"/>
+    <origin xyz="0.3385 -0.02 0.000137" rpy="0 0 0"/>
+  </joint>
+  <link name="spotarm_link3">
+    <visual>
+      <origin xyz="0.044 -0.01 0.0514" rpy="3.141593 0.851593 3.141593"/>
+      <geometry>
+        <box size="0.16 0.041 0.04"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.044 -0.01 0.0514" rpy="3.141593 0.851593 3.141593"/>
+      <geometry>
+        <box size="0.16 0.041 0.04"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.1065 -0.01 0.0973" rpy="3.141593 1.570793 3.141593"/>
+      <geometry>
+        <cylinder radius="0.0477" length="0.059"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.1065 -0.01 0.0973" rpy="3.141593 1.570793 3.141593"/>
+      <geometry>
+        <cylinder radius="0.0477" length="0.059"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="spotarm_4_joint" type="revolute">
+    <parent link="spotarm_link3"/>
+    <child link="spotarm_link4"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="500" lower="-4.3633" upper="1.22173" velocity="2.5"/>
+    <origin xyz="0.107 -0.01 0.09734" rpy="0 0 0"/>
+  </joint>
+  <link name="spotarm_link4">
+  </link>
+  <joint name="Slider11" type="prismatic">
+    <parent link="spotarm_link4"/>
+    <child link="spotarm_link5"/>
+    <axis xyz="1 0 0"/>
+    <origin xyz="0.035 0 0" rpy="0 0 0"/>
+    <limit effort="100" lower="0" upper="0.075" velocity="100"/>
+  </joint>
+  <link name="spotarm_link5">
+    <visual>
+      <origin xyz="0.232 0 0" rpy="3.141593 1.570793 3.141593"/>
+      <geometry>
+        <box size="0.04 0.044 0.042"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.232 0 0" rpy="3.141593 1.570793 3.141593"/>
+      <geometry>
+        <box size="0.04 0.044 0.042"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.106 0 0" rpy="3.141593 1.570793 3.141593"/>
+      <geometry>
+        <cylinder radius="0.0477" length="0.21"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.106 0 0" rpy="3.141593 1.570793 3.141593"/>
+      <geometry>
+        <cylinder radius="0.0477" length="0.21"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="spotarm_5_joint" type="revolute">
+    <parent link="spotarm_link5"/>
+    <child link="spotarm_link6"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="500" lower="-1.0472" upper="1.0472" velocity="2.5"/>
+    <origin xyz="0.2413 0.01 0" rpy="0 0 0"/>
+  </joint>
+  <link name="spotarm_link6">
+    <visual>
+      <origin xyz="0.0671 -0.0105 0" rpy="3.141593 1.570793 3.141593"/>
+      <geometry>
+        <cylinder radius="0.0477" length="0.065"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.0671 -0.0105 0" rpy="3.141593 1.570793 3.141593"/>
+      <geometry>
+        <cylinder radius="0.0477" length="0.065"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="spotarm_6_joint" type="revolute">
+    <parent link="spotarm_link6"/>
+    <child link="front"/>
+    <axis xyz="0.000004 0 1"/>
+    <limit effort="500" lower="-4.45059" upper="1.30899" velocity="2.5"/>
+    <origin xyz="0.1 -0.0105 0" rpy="3.141593 -1.570793 3.141593"/>
+  </joint>
+  <link name="front">
+    <visual>
+      <origin xyz="0 0.0005 -0.007" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.0227" length="0.028"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0 0.0005 -0.007" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.0227" length="0.028"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.029 -0.01784 -0.053" rpy="0 0 0.59"/>
+      <geometry>
+        <box size="0.04 0.0217 0.105"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.029 -0.01784 -0.053" rpy="0 0 0.59"/>
+      <geometry>
+        <box size="0.04 0.0217 0.105"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.029 0.01884 -0.053" rpy="0 0 0.59"/>
+      <geometry>
+        <box size="0.04 0.0217 0.105"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.029 0.01884 -0.053" rpy="0 0 0.59"/>
+      <geometry>
+        <box size="0.04 0.0217 0.105"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.0172 0.0114 -0.053" rpy="3.141593 -1.570793 3.141593"/>
+      <geometry>
+        <box size="0.106 0.056 0.071"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.0172 0.0114 -0.053" rpy="3.141593 -1.570793 3.141593"/>
+      <geometry>
+        <box size="0.106 0.056 0.071"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.0172 -0.0104 -0.053" rpy="3.141593 -1.570793 3.141593"/>
+      <geometry>
+        <box size="0.106 0.056 0.071"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.0172 -0.0104 -0.053" rpy="3.141593 -1.570793 3.141593"/>
+      <geometry>
+        <box size="0.106 0.056 0.071"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="gripper_centre">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.01"/>
+      </geometry>
+    </visual>
+  </link>
+  <joint name="gripper_centre_joint" type="fixed">
+    <parent link="front"/>
+    <child link="gripper_centre"/>
+    <origin xyz="0 0 -0.18" rpy="0 0 0"/>
+  </joint>
+  <joint name="gripper_right_finger_joint" type="prismatic">
+    <parent link="front"/>
+    <child link="gripper_right_finger_link"/>
+    <axis xyz="1 0 0"/>
+    <origin xyz="0 0.0005 -0.007" rpy="0 0 0"/>
+    <limit effort="16" lower="0" upper="0.045" velocity="0.05"/>
+  </joint>
+  <link name="gripper_right_finger_link">
+    <visual>
+      <origin xyz="0.004 0 -0.1741" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.0076 0.05091 0.08982"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.004 0 -0.1741" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.0076 0.05091 0.08982"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.0049 0.0153 -0.1208" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.0055 0.0295 0.018"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.0049 0.0153 -0.1208" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.0055 0.0295 0.018"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.0085 0.0156 -0.1048" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.03213 0.02959 0.01487"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.0085 0.0156 -0.1048" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.03213 0.02959 0.01487"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="gripper_left_finger_joint" type="prismatic">
+    <parent link="front"/>
+    <child link="gripper_left_finger_link"/>
+    <axis xyz="1 0.000093 0"/>
+    <origin xyz="0 0 0" rpy="0 0 3.1415"/>
+    <limit effort="16" lower="0" upper="0.045" velocity="0.05"/>
+  </joint>
+  <link name="gripper_left_finger_link">
+    <visual>
+      <origin xyz="0.004 0 -0.1741" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.0076 0.05091 0.08982"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.004 0 -0.1741" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.0076 0.05091 0.08982"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.0049 0.0153 -0.1208" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.0055 0.0295 0.018"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.0049 0.0153 -0.1208" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.0055 0.0295 0.018"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.0085 0.0156 -0.1048" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.03213 0.02959 0.01487"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.0085 0.0156 -0.1048" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.03213 0.02959 0.01487"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="cap screw(19)">
+  </link>
+  <joint name="front_cap screw(19)_joint" type="fixed">
+    <parent link="front"/>
+    <child link="cap screw(19)"/>
+    <origin xyz="0.0415 -0.0029 -0.0004" rpy="3.14159 0 0"/>
+  </joint>
+  <link name="cap screw(18)">
+  </link>
+  <joint name="front_cap screw(18)_joint" type="fixed">
+    <parent link="front"/>
+    <child link="cap screw(18)"/>
+    <origin xyz="-0.0415 0.0029 -0.0004" rpy="3.14159 0 0"/>
+  </joint>
+  <link name="cap screw(17)">
+  </link>
+  <joint name="front_cap screw(17)_joint" type="fixed">
+    <parent link="front"/>
+    <child link="cap screw(17)"/>
+    <origin xyz="0.0433 0.0315 0.0026" rpy="3.14159 0 0"/>
+  </joint>
+  <link name="cap screw(16)">
+  </link>
+  <joint name="front_cap screw(16)_joint" type="fixed">
+    <parent link="front"/>
+    <child link="cap screw(16)"/>
+    <origin xyz="-0.0433 -0.0315 0.0026" rpy="3.14159 0 0"/>
+  </joint>
+  <link name="cap screw(15)">
+  </link>
+  <joint name="front_cap screw(15)_joint" type="fixed">
+    <parent link="front"/>
+    <child link="cap screw(15)"/>
+    <origin xyz="0 0.033 0.0026" rpy="3.14159 0 0"/>
+  </joint>
+  <link name="cap screw(14)">
+  </link>
+  <joint name="front_cap screw(14)_joint" type="fixed">
+    <parent link="front"/>
+    <child link="cap screw(14)"/>
+    <origin xyz="0 -0.033 0.0026" rpy="3.14159 0 0"/>
+  </joint>
+  <link name="cap screw(13)">
+  </link>
+  <joint name="front_cap screw(13)_joint" type="fixed">
+    <parent link="front"/>
+    <child link="cap screw(13)"/>
+    <origin xyz="0.0426 -0.0094 -0.09448" rpy="0 0 0"/>
+  </joint>
+  <link name="cap screw(12)">
+  </link>
+  <joint name="front_cap screw(12)_joint" type="fixed">
+    <parent link="front"/>
+    <child link="cap screw(12)"/>
+    <origin xyz="-0.0425 0.0094 -0.0945" rpy="0 0 0"/>
+  </joint>
+  <link name="cap screw(11)">
+  </link>
+  <joint name="front_cap screw(11)_joint" type="fixed">
+    <parent link="front"/>
+    <child link="cap screw(11)"/>
+    <origin xyz="-0.0477 -0.008 -0.0108" rpy="3.141593 1.570793 3.141593"/>
+  </joint>
+  <link name="cap screw(10)">
+  </link>
+  <joint name="front_cap screw(10)_joint" type="fixed">
+    <parent link="front"/>
+    <child link="cap screw(10)"/>
+    <origin xyz="-0.0477 -0.008 -0.043" rpy="3.141593 1.570793 3.141593"/>
+  </joint>
+  <link name="cap screw(9)">
+  </link>
+  <joint name="front_cap screw(9)_joint" type="fixed">
+    <parent link="front"/>
+    <child link="cap screw(9)"/>
+    <origin xyz="-0.0477 -0.008 -0.075" rpy="3.141593 1.570793 3.141593"/>
+  </joint>
+  <link name="cap screw(8)">
+  </link>
+  <joint name="front_cap screw(8)_joint" type="fixed">
+    <parent link="front"/>
+    <child link="cap screw(8)"/>
+    <origin xyz="-0.0477 0.008 -0.075" rpy="3.141593 1.570793 3.141593"/>
+  </joint>
+  <link name="cap screw(7)">
+  </link>
+  <joint name="front_cap screw(7)_joint" type="fixed">
+    <parent link="front"/>
+    <child link="cap screw(7)"/>
+    <origin xyz="-0.0477 0.008 -0.043" rpy="3.141593 1.570793 3.141593"/>
+  </joint>
+  <link name="cap screw(6)">
+  </link>
+  <joint name="front_cap screw(6)_joint" type="fixed">
+    <parent link="front"/>
+    <child link="cap screw(6)"/>
+    <origin xyz="-0.0477 0.008 -0.0108" rpy="3.141593 1.570793 3.141593"/>
+  </joint>
+  <link name="cap screw(5)">
+  </link>
+  <joint name="front_cap screw(5)_joint" type="fixed">
+    <parent link="front"/>
+    <child link="cap screw(5)"/>
+    <origin xyz="0.0477 -0.008 -0.0108" rpy="3.141593 -1.570793 3.141593"/>
+  </joint>
+  <link name="cap screw(4)">
+  </link>
+  <joint name="front_cap screw(4)_joint" type="fixed">
+    <parent link="front"/>
+    <child link="cap screw(4)"/>
+    <origin xyz="0.0477 -0.008 -0.043" rpy="3.141593 -1.570793 3.141593"/>
+  </joint>
+  <link name="cap screw(3)">
+  </link>
+  <joint name="front_cap screw(3)_joint" type="fixed">
+    <parent link="front"/>
+    <child link="cap screw(3)"/>
+    <origin xyz="0.0477 -0.008 -0.075" rpy="3.141593 -1.570793 3.141593"/>
+  </joint>
+  <link name="cap screw(2)">
+  </link>
+  <joint name="front_cap screw(2)_joint" type="fixed">
+    <parent link="front"/>
+    <child link="cap screw(2)"/>
+    <origin xyz="0.0477 0.008 -0.075" rpy="3.141593 -1.570793 3.141593"/>
+  </joint>
+  <link name="cap screw(1)">
+  </link>
+  <joint name="front_cap screw(1)_joint" type="fixed">
+    <parent link="front"/>
+    <child link="cap screw(1)"/>
+    <origin xyz="0.0477 0.008 -0.043" rpy="3.141593 -1.570793 3.141593"/>
+  </joint>
+  <link name="cap screw">
+  </link>
+  <joint name="front_cap screw_joint" type="fixed">
+    <parent link="front"/>
+    <child link="cap screw"/>
+    <origin xyz="0.0477 0.008 -0.0108" rpy="3.141593 -1.570793 3.141593"/>
+  </joint>
+  <joint name="rear right shoulder abduction motor" type="revolute">
+    <parent link="base_link"/>
+    <child link="rear right shoulder"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="50" lower="-0.6" upper="0.5" velocity="10"/>
+    <origin xyz="-0.3084 -0.0528 0.012301" rpy="1.5708 -0.00001 -1.570793"/>
+  </joint>
+  <link name="rear right shoulder">
+    <visual>
+      <origin xyz="-0.004874 0.000873 -0.000643" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.086792 0.124528 0.120755"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.004874 0.000873 -0.000643" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.086792 0.124528 0.120755"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.006447 0.000873 -0.000643" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.098113 0.098113 0.098113"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.006447 0.000873 -0.000643" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.098113 0.098113 0.098113"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.058145 0.000873 -0.000643" rpy="-1.5708 0 1.5708"/>
+      <geometry>
+        <cylinder radius="0.047925" length="0.006038"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.058145 0.000873 -0.000643" rpy="-1.5708 0 1.5708"/>
+      <geometry>
+        <cylinder radius="0.047925" length="0.006038"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="rear right shoulder rotation motor" type="revolute">
+    <parent link="rear right shoulder"/>
+    <child link="rear right upperarm"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="50" lower="-1.7" upper="1.7" velocity="10"/>
+    <origin xyz="0 -0.00053 0" rpy="0 0 0"/>
+  </joint>
+  <link name="rear right upperarm">
+    <visual>
+      <origin xyz="0.117011 -0.32073 0.1829" rpy="-1.5708 0 1.5708"/>
+      <geometry>
+        <cylinder radius="0.02717" length="0.060377"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.117011 -0.32073 0.1829" rpy="-1.5708 0 1.5708"/>
+      <geometry>
+        <cylinder radius="0.02717" length="0.060377"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.117011 -0.25867 0.180358" rpy="0 0.05 1.570795"/>
+      <geometry>
+        <box size="0.09434 0.060377 0.053208"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.117011 -0.25867 0.180358" rpy="0 0.05 1.570795"/>
+      <geometry>
+        <box size="0.09434 0.060377 0.053208"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.149087 -0.023654 0.007772" rpy="-0.656375 0 0"/>
+      <geometry>
+        <box size="0.016226 0.177358 0.071698"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.149087 -0.023654 0.007772" rpy="-0.656375 0 0"/>
+      <geometry>
+        <box size="0.016226 0.177358 0.071698"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.14944 -0.001095 0.048195" rpy="-0.688653 0.282831 -0.225783"/>
+      <geometry>
+        <box size="0.004528 0.173585 0.015094"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.14944 -0.001095 0.048195" rpy="-0.688653 0.282831 -0.225783"/>
+      <geometry>
+        <box size="0.004528 0.173585 0.015094"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.143963 -0.142325 0.149419" rpy="-0.689784 0.390399 -0.237371"/>
+      <geometry>
+        <box size="0.004528 0.173585 0.015094"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.143963 -0.142325 0.149419" rpy="-0.689784 0.390399 -0.237371"/>
+      <geometry>
+        <box size="0.004528 0.173585 0.015094"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.143376 -0.16295 0.113632" rpy="-0.64577 0.030504 0.039623"/>
+      <geometry>
+        <box size="0.016226 0.173585 0.071698"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.143376 -0.16295 0.113632" rpy="-0.64577 0.030504 0.039623"/>
+      <geometry>
+        <box size="0.016226 0.173585 0.071698"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.113992 -0.21476 0.154045" rpy="-0.656375 0 0"/>
+      <geometry>
+        <box size="0.054717 0.098113 0.064151"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.113992 -0.21476 0.154045" rpy="-0.656375 0 0"/>
+      <geometry>
+        <box size="0.054717 0.098113 0.064151"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.113238 -0.119427 0.086296" rpy="-2.227175 0 0"/>
+      <geometry>
+        <cylinder radius="0.028679" length="0.150943"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.113238 -0.119427 0.086296" rpy="-2.227175 0 0"/>
+      <geometry>
+        <cylinder radius="0.028679" length="0.150943"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.113238 -0.04459 0.028636" rpy="-2.227175 0 0"/>
+      <geometry>
+        <cylinder radius="0.037736" length="0.041509"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.113238 -0.04459 0.028636" rpy="-2.227175 0 0"/>
+      <geometry>
+        <cylinder radius="0.037736" length="0.041509"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.113738 -0.02284 -0.046726" rpy="-0.656375 0 0"/>
+      <geometry>
+        <box size="0.061509 0.069811 0.011321"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.113738 -0.02284 -0.046726" rpy="-0.656375 0 0"/>
+      <geometry>
+        <box size="0.061509 0.069811 0.011321"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.073615 -0.00019 -0.000689" rpy="-1.5708 0 1.5708"/>
+      <geometry>
+        <cylinder radius="0.04" length="0.037736"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.073615 -0.00019 -0.000689" rpy="-1.5708 0 1.5708"/>
+      <geometry>
+        <cylinder radius="0.04" length="0.037736"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.115879 0.008331 -0.011344" rpy="-2.227175 0 0"/>
+      <geometry>
+        <cylinder radius="0.033962" length="0.086792"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.115879 0.008331 -0.011344" rpy="-2.227175 0 0"/>
+      <geometry>
+        <cylinder radius="0.033962" length="0.086792"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.12154 0.053961 -0.047932" rpy="-0.656375 0 0"/>
+      <geometry>
+        <box size="0.064151 0.003774 0.071698"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.12154 0.053961 -0.047932" rpy="-0.656375 0 0"/>
+      <geometry>
+        <box size="0.064151 0.003774 0.071698"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.12003 0.073308 -0.006499" rpy="0.013625 0 0"/>
+      <geometry>
+        <box size="0.060377 0.007547 0.022642"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.12003 0.073308 -0.006499" rpy="0.013625 0 0"/>
+      <geometry>
+        <box size="0.060377 0.007547 0.022642"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.117011 -0.13635 0.154336" rpy="0.99 0 0"/>
+      <geometry>
+        <box size="0.049057 0.005283 0.169811"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.117011 -0.13635 0.154336" rpy="0.99 0 0"/>
+      <geometry>
+        <box size="0.049057 0.005283 0.169811"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.083111 -0.31378 0.182934" rpy="0 -0.076371 1.570798"/>
+      <geometry>
+        <box size="0.044 0.008 0.0387"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.083111 -0.31378 0.182934" rpy="0 -0.076371 1.570798"/>
+      <geometry>
+        <box size="0.044 0.008 0.0387"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="rear right elbow motor" type="revolute">
+    <parent link="rear right upperarm"/>
+    <child link="rear right forearm"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="50" lower="-0.45" upper="1.6" velocity="10"/>
+    <origin xyz="0.1122 -0.319729 0.182338" rpy="0 0 0"/>
+  </joint>
+  <link name="rear right forearm">
+    <visual>
+      <origin xyz="0.001038 -0.271321 -0.225038" rpy="-1.5708 0 1.570801"/>
+      <geometry>
+        <cylinder radius="0.035094" length="0.039623"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.001038 -0.271321 -0.225038" rpy="-1.5708 0 1.570801"/>
+      <geometry>
+        <cylinder radius="0.035094" length="0.039623"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.001038 -0.227421 -0.230239" rpy="-0.947675 0 0"/>
+      <geometry>
+        <box size="0.037736 0.006038 0.041509"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.001038 -0.227421 -0.230239" rpy="-0.947675 0 0"/>
+      <geometry>
+        <box size="0.037736 0.006038 0.041509"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.001038 -0.232272 -0.198255" rpy="-0.093185 0 0"/>
+      <geometry>
+        <box size="0.037736 0.045283 0.006792"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.001038 -0.232272 -0.198255" rpy="-0.093185 0 0"/>
+      <geometry>
+        <box size="0.037736 0.045283 0.006792"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.001038 -0.205751 -0.206348" rpy="0.616815 0 0"/>
+      <geometry>
+        <box size="0.025283 0.022642 0.020755"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.001038 -0.205751 -0.206348" rpy="0.616815 0 0"/>
+      <geometry>
+        <box size="0.025283 0.022642 0.020755"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.001038 -0.168611 -0.18111" rpy="-2.513185 0 0"/>
+      <geometry>
+        <box size="0.019245 0.071698 0.017358"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.001038 -0.168611 -0.18111" rpy="-2.513185 0 0"/>
+      <geometry>
+        <box size="0.019245 0.071698 0.017358"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.001038 -0.125111 -0.148236" rpy="0.663625 0 0"/>
+      <geometry>
+        <box size="0.021887 0.037736 0.018491"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.001038 -0.125111 -0.148236" rpy="0.663625 0 0"/>
+      <geometry>
+        <box size="0.021887 0.037736 0.018491"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.001038 -0.100861 -0.126163" rpy="0.760415 0 0"/>
+      <geometry>
+        <box size="0.026415 0.030189 0.021132"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.001038 -0.100861 -0.126163" rpy="0.760415 0 0"/>
+      <geometry>
+        <box size="0.026415 0.030189 0.021132"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.001038 -0.084241 -0.106843" rpy="0.867225 0 0"/>
+      <geometry>
+        <box size="0.032453 0.022642 0.024528"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.001038 -0.084241 -0.106843" rpy="0.867225 0 0"/>
+      <geometry>
+        <box size="0.032453 0.022642 0.024528"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.001038 -0.068551 -0.08661" rpy="0.907225 0 0"/>
+      <geometry>
+        <box size="0.037736 0.033962 0.026415"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.001038 -0.068551 -0.08661" rpy="0.907225 0 0"/>
+      <geometry>
+        <box size="0.037736 0.033962 0.026415"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.001038 -0.049721 -0.059227" rpy="0.933245 0 0"/>
+      <geometry>
+        <cylinder radius="0.024528" length="0.030566"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.001038 -0.049721 -0.059227" rpy="0.933245 0 0"/>
+      <geometry>
+        <cylinder radius="0.024528" length="0.030566"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.001038 -0.034991 -0.038166" rpy="0.937225 0 0"/>
+      <geometry>
+        <box size="0.038113 0.022642 0.032075"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.001038 -0.034991 -0.038166" rpy="0.937225 0 0"/>
+      <geometry>
+        <box size="0.038113 0.022642 0.032075"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.001038 -0.022931 -0.021744" rpy="0.937225 0 0"/>
+      <geometry>
+        <box size="0.031321 0.018868 0.03283"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.001038 -0.022931 -0.021744" rpy="0.937225 0 0"/>
+      <geometry>
+        <box size="0.031321 0.018868 0.03283"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.001038 -0.010711 -0.006332" rpy="0.834035 0 0"/>
+      <geometry>
+        <box size="0.027925 0.018868 0.03283"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.001038 -0.010711 -0.006332" rpy="0.834035 0 0"/>
+      <geometry>
+        <box size="0.027925 0.018868 0.03283"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.001038 0.004299 0.006862" rpy="0.680845 0 0"/>
+      <geometry>
+        <box size="0.027547 0.018868 0.034717"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.001038 0.004299 0.006862" rpy="0.680845 0 0"/>
+      <geometry>
+        <box size="0.027547 0.018868 0.034717"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.001038 0.021259 0.019851" rpy="0.604465 0 0"/>
+      <geometry>
+        <box size="0.020377 0.026415 0.03434"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.001038 0.021259 0.019851" rpy="0.604465 0 0"/>
+      <geometry>
+        <box size="0.020377 0.026415 0.03434"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.001038 0.019179 -0.009531" rpy="0.494895 0 0"/>
+      <geometry>
+        <box size="0.010189 0.05283 0.00717"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.001038 0.019179 -0.009531" rpy="0.494895 0 0"/>
+      <geometry>
+        <box size="0.010189 0.05283 0.00717"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.001038 0.037459 0.024742" rpy="-1.5708 0 1.5708"/>
+      <geometry>
+        <cylinder radius="0.021887" length="0.019736"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.001038 0.037459 0.024742" rpy="-1.5708 0 1.5708"/>
+      <geometry>
+        <cylinder radius="0.021887" length="0.019736"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="rear right touch sensor">
+  </link>
+  <joint name="rear right forearm_rear right touch sensor_joint" type="fixed">
+    <parent link="rear right forearm"/>
+    <child link="rear right touch sensor"/>
+    <origin xyz="0.001038 -0.271321 -0.225038" rpy="-1.5708 0 1.570801"/>
+  </joint>
+  <joint name="rear right piston motor" type="continuous">
+    <parent link="rear right upperarm"/>
+    <child link="solid_0"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="10" velocity="10"/>
+    <origin xyz="0.1172 -0.29077 0.2083" rpy="-0.5236 0 0"/>
+  </joint>
+  <link name="solid_0">
+  </link>
+  <joint name="rear left shoulder abduction motor" type="revolute">
+    <parent link="base_link"/>
+    <child link="rear left shoulder"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="50" lower="-0.6" upper="0.5" velocity="10"/>
+    <origin xyz="-0.3084 0.0528 0.012299" rpy="1.5708 -0.00001 -1.570793"/>
+  </joint>
+  <link name="rear left shoulder">
+    <visual>
+      <origin xyz="0.004874 0.000873 -0.000643" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.086792 0.124528 0.120755"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.004874 0.000873 -0.000643" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.086792 0.124528 0.120755"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.006447 0.000873 -0.000643" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.098113 0.098113 0.098113"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.006447 0.000873 -0.000643" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.098113 0.098113 0.098113"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.058145 0.000873 -0.000643" rpy="-1.5708 0 1.5708"/>
+      <geometry>
+        <cylinder radius="0.047925" length="0.006038"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.058145 0.000873 -0.000643" rpy="-1.5708 0 1.5708"/>
+      <geometry>
+        <cylinder radius="0.047925" length="0.006038"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="rear left shoulder rotation motor" type="revolute">
+    <parent link="rear left shoulder"/>
+    <child link="rear left upperarm"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="50" lower="-1.7" upper="1.7" velocity="10"/>
+    <origin xyz="0 -0.00053 0" rpy="0 0 0"/>
+  </joint>
+  <link name="rear left upperarm">
+    <visual>
+      <origin xyz="-0.117011 -0.32073 0.1829" rpy="-1.5708 0 1.5708"/>
+      <geometry>
+        <cylinder radius="0.02717" length="0.060377"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.117011 -0.32073 0.1829" rpy="-1.5708 0 1.5708"/>
+      <geometry>
+        <cylinder radius="0.02717" length="0.060377"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.117011 -0.25867 0.180358" rpy="0 0.05 1.570795"/>
+      <geometry>
+        <box size="0.09434 0.060377 0.053208"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.117011 -0.25867 0.180358" rpy="0 0.05 1.570795"/>
+      <geometry>
+        <box size="0.09434 0.060377 0.053208"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.149087 -0.023654 0.007772" rpy="-0.656375 0 0"/>
+      <geometry>
+        <box size="0.016226 0.177358 0.071698"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.149087 -0.023654 0.007772" rpy="-0.656375 0 0"/>
+      <geometry>
+        <box size="0.016226 0.177358 0.071698"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.14944 -0.001095 0.048195" rpy="-0.688653 0.282831 -0.225783"/>
+      <geometry>
+        <box size="0.004528 0.173585 0.015094"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.14944 -0.001095 0.048195" rpy="-0.688653 0.282831 -0.225783"/>
+      <geometry>
+        <box size="0.004528 0.173585 0.015094"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.143963 -0.142325 0.149419" rpy="-0.689784 0.390399 -0.237371"/>
+      <geometry>
+        <box size="0.004528 0.173585 0.015094"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.143963 -0.142325 0.149419" rpy="-0.689784 0.390399 -0.237371"/>
+      <geometry>
+        <box size="0.004528 0.173585 0.015094"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.143376 -0.16295 0.113632" rpy="-0.64577 0.030504 0.039623"/>
+      <geometry>
+        <box size="0.016226 0.173585 0.071698"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.143376 -0.16295 0.113632" rpy="-0.64577 0.030504 0.039623"/>
+      <geometry>
+        <box size="0.016226 0.173585 0.071698"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.113992 -0.21476 0.154045" rpy="-0.656375 0 0"/>
+      <geometry>
+        <box size="0.054717 0.098113 0.064151"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.113992 -0.21476 0.154045" rpy="-0.656375 0 0"/>
+      <geometry>
+        <box size="0.054717 0.098113 0.064151"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.113238 -0.119427 0.086296" rpy="-2.227175 0 0"/>
+      <geometry>
+        <cylinder radius="0.028679" length="0.150943"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.113238 -0.119427 0.086296" rpy="-2.227175 0 0"/>
+      <geometry>
+        <cylinder radius="0.028679" length="0.150943"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.113238 -0.04459 0.028636" rpy="-2.227175 0 0"/>
+      <geometry>
+        <cylinder radius="0.037736" length="0.041509"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.113238 -0.04459 0.028636" rpy="-2.227175 0 0"/>
+      <geometry>
+        <cylinder radius="0.037736" length="0.041509"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.113738 -0.02284 -0.046726" rpy="-0.656375 0 0"/>
+      <geometry>
+        <box size="0.061509 0.069811 0.011321"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.113738 -0.02284 -0.046726" rpy="-0.656375 0 0"/>
+      <geometry>
+        <box size="0.061509 0.069811 0.011321"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.073615 -0.00019 -0.000689" rpy="-1.5708 0 1.5708"/>
+      <geometry>
+        <cylinder radius="0.04" length="0.037736"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.073615 -0.00019 -0.000689" rpy="-1.5708 0 1.5708"/>
+      <geometry>
+        <cylinder radius="0.04" length="0.037736"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.115879 0.008331 -0.011344" rpy="-2.227175 0 0"/>
+      <geometry>
+        <cylinder radius="0.033962" length="0.086792"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.115879 0.008331 -0.011344" rpy="-2.227175 0 0"/>
+      <geometry>
+        <cylinder radius="0.033962" length="0.086792"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.12154 0.053961 -0.047932" rpy="-0.656375 0 0"/>
+      <geometry>
+        <box size="0.064151 0.003774 0.071698"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.12154 0.053961 -0.047932" rpy="-0.656375 0 0"/>
+      <geometry>
+        <box size="0.064151 0.003774 0.071698"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.12003 0.073308 -0.006499" rpy="0.013625 0 0"/>
+      <geometry>
+        <box size="0.060377 0.007547 0.022642"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.12003 0.073308 -0.006499" rpy="0.013625 0 0"/>
+      <geometry>
+        <box size="0.060377 0.007547 0.022642"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.117011 -0.13635 0.154336" rpy="0.99 0 0"/>
+      <geometry>
+        <box size="0.049057 0.005283 0.169811"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.117011 -0.13635 0.154336" rpy="0.99 0 0"/>
+      <geometry>
+        <box size="0.049057 0.005283 0.169811"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.083111 -0.31378 0.182934" rpy="0 -0.076371 1.570798"/>
+      <geometry>
+        <box size="0.044 0.008 0.0387"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.083111 -0.31378 0.182934" rpy="0 -0.076371 1.570798"/>
+      <geometry>
+        <box size="0.044 0.008 0.0387"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="rear left elbow motor" type="revolute">
+    <parent link="rear left upperarm"/>
+    <child link="rear left forearm"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="50" lower="-0.45" upper="1.6" velocity="10"/>
+    <origin xyz="-0.1122 -0.319729 0.182338" rpy="0 0 0"/>
+  </joint>
+  <link name="rear left forearm">
+    <visual>
+      <origin xyz="-0.001038 -0.271321 -0.225038" rpy="-1.5708 0 1.570801"/>
+      <geometry>
+        <cylinder radius="0.035094" length="0.039623"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.001038 -0.271321 -0.225038" rpy="-1.5708 0 1.570801"/>
+      <geometry>
+        <cylinder radius="0.035094" length="0.039623"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.001038 -0.227421 -0.230239" rpy="-0.947675 0 0"/>
+      <geometry>
+        <box size="0.037736 0.006038 0.041509"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.001038 -0.227421 -0.230239" rpy="-0.947675 0 0"/>
+      <geometry>
+        <box size="0.037736 0.006038 0.041509"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.001038 -0.232272 -0.198255" rpy="-0.093185 0 0"/>
+      <geometry>
+        <box size="0.037736 0.045283 0.006792"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.001038 -0.232272 -0.198255" rpy="-0.093185 0 0"/>
+      <geometry>
+        <box size="0.037736 0.045283 0.006792"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.001038 -0.205751 -0.206348" rpy="0.616815 0 0"/>
+      <geometry>
+        <box size="0.025283 0.022642 0.020755"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.001038 -0.205751 -0.206348" rpy="0.616815 0 0"/>
+      <geometry>
+        <box size="0.025283 0.022642 0.020755"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.001038 -0.168611 -0.18111" rpy="-2.513185 0 0"/>
+      <geometry>
+        <box size="0.019245 0.071698 0.017358"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.001038 -0.168611 -0.18111" rpy="-2.513185 0 0"/>
+      <geometry>
+        <box size="0.019245 0.071698 0.017358"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.001038 -0.125111 -0.148236" rpy="0.663625 0 0"/>
+      <geometry>
+        <box size="0.021887 0.037736 0.018491"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.001038 -0.125111 -0.148236" rpy="0.663625 0 0"/>
+      <geometry>
+        <box size="0.021887 0.037736 0.018491"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.001038 -0.100861 -0.126163" rpy="0.760415 0 0"/>
+      <geometry>
+        <box size="0.026415 0.030189 0.021132"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.001038 -0.100861 -0.126163" rpy="0.760415 0 0"/>
+      <geometry>
+        <box size="0.026415 0.030189 0.021132"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.001038 -0.084241 -0.106843" rpy="0.867225 0 0"/>
+      <geometry>
+        <box size="0.032453 0.022642 0.024528"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.001038 -0.084241 -0.106843" rpy="0.867225 0 0"/>
+      <geometry>
+        <box size="0.032453 0.022642 0.024528"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.001038 -0.068551 -0.08661" rpy="0.907225 0 0"/>
+      <geometry>
+        <box size="0.037736 0.033962 0.026415"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.001038 -0.068551 -0.08661" rpy="0.907225 0 0"/>
+      <geometry>
+        <box size="0.037736 0.033962 0.026415"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.001038 -0.049721 -0.059227" rpy="0.933245 0 0"/>
+      <geometry>
+        <cylinder radius="0.024528" length="0.030566"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.001038 -0.049721 -0.059227" rpy="0.933245 0 0"/>
+      <geometry>
+        <cylinder radius="0.024528" length="0.030566"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.001038 -0.034991 -0.038166" rpy="0.937225 0 0"/>
+      <geometry>
+        <box size="0.038113 0.022642 0.032075"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.001038 -0.034991 -0.038166" rpy="0.937225 0 0"/>
+      <geometry>
+        <box size="0.038113 0.022642 0.032075"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.001038 -0.022931 -0.021744" rpy="0.937225 0 0"/>
+      <geometry>
+        <box size="0.031321 0.018868 0.03283"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.001038 -0.022931 -0.021744" rpy="0.937225 0 0"/>
+      <geometry>
+        <box size="0.031321 0.018868 0.03283"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.001038 -0.010711 -0.006332" rpy="0.834035 0 0"/>
+      <geometry>
+        <box size="0.027925 0.018868 0.03283"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.001038 -0.010711 -0.006332" rpy="0.834035 0 0"/>
+      <geometry>
+        <box size="0.027925 0.018868 0.03283"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.001038 0.004299 0.006862" rpy="0.680845 0 0"/>
+      <geometry>
+        <box size="0.027547 0.018868 0.034717"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.001038 0.004299 0.006862" rpy="0.680845 0 0"/>
+      <geometry>
+        <box size="0.027547 0.018868 0.034717"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.001038 0.021259 0.019851" rpy="0.604465 0 0"/>
+      <geometry>
+        <box size="0.020377 0.026415 0.03434"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.001038 0.021259 0.019851" rpy="0.604465 0 0"/>
+      <geometry>
+        <box size="0.020377 0.026415 0.03434"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.001038 0.019179 -0.009531" rpy="0.494895 0 0"/>
+      <geometry>
+        <box size="0.010189 0.05283 0.00717"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.001038 0.019179 -0.009531" rpy="0.494895 0 0"/>
+      <geometry>
+        <box size="0.010189 0.05283 0.00717"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.001038 0.037459 0.024742" rpy="-1.5708 0 1.5708"/>
+      <geometry>
+        <cylinder radius="0.021887" length="0.019736"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.001038 0.037459 0.024742" rpy="-1.5708 0 1.5708"/>
+      <geometry>
+        <cylinder radius="0.021887" length="0.019736"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="rear left touch sensor">
+  </link>
+  <joint name="rear left forearm_rear left touch sensor_joint" type="fixed">
+    <parent link="rear left forearm"/>
+    <child link="rear left touch sensor"/>
+    <origin xyz="0.001038 -0.271321 -0.225038" rpy="-1.5708 0 1.570801"/>
+  </joint>
+  <joint name="rear left piston motor" type="continuous">
+    <parent link="rear left upperarm"/>
+    <child link="solid_1"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="10" velocity="10"/>
+    <origin xyz="-0.1172 -0.29077 0.2083" rpy="-0.5236 0 0"/>
+  </joint>
+  <link name="solid_1">
+  </link>
+  <joint name="front right shoulder abduction motor" type="revolute">
+    <parent link="base_link"/>
+    <child link="front right shoulder"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="50" lower="-0.6" upper="0.5" velocity="10"/>
+    <origin xyz="0.3635 -0.0528 0.012401" rpy="1.5708 -0.00001 -1.570793"/>
+  </joint>
+  <link name="front right shoulder">
+    <visual>
+      <origin xyz="-0.004874 0.000873 -0.000643" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.086792 0.124528 0.120755"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.004874 0.000873 -0.000643" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.086792 0.124528 0.120755"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.006447 0.000873 -0.000643" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.098113 0.098113 0.098113"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.006447 0.000873 -0.000643" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.098113 0.098113 0.098113"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.058145 0.000873 -0.000643" rpy="-1.5708 0 1.5708"/>
+      <geometry>
+        <cylinder radius="0.047925" length="0.006038"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.058145 0.000873 -0.000643" rpy="-1.5708 0 1.5708"/>
+      <geometry>
+        <cylinder radius="0.047925" length="0.006038"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="front right shoulder rotation motor" type="revolute">
+    <parent link="front right shoulder"/>
+    <child link="front right upperarm"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="50" lower="-1.7" upper="1.7" velocity="10"/>
+    <origin xyz="0 -0.00053 0" rpy="0 0 0"/>
+  </joint>
+  <link name="front right upperarm">
+    <visual>
+      <origin xyz="0.117011 -0.32073 0.1829" rpy="-1.5708 0 1.5708"/>
+      <geometry>
+        <cylinder radius="0.02717" length="0.060377"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.117011 -0.32073 0.1829" rpy="-1.5708 0 1.5708"/>
+      <geometry>
+        <cylinder radius="0.02717" length="0.060377"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.117011 -0.25867 0.180358" rpy="0 0.05 1.570795"/>
+      <geometry>
+        <box size="0.09434 0.060377 0.053208"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.117011 -0.25867 0.180358" rpy="0 0.05 1.570795"/>
+      <geometry>
+        <box size="0.09434 0.060377 0.053208"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.149087 -0.023654 0.007772" rpy="-0.656375 0 0"/>
+      <geometry>
+        <box size="0.016226 0.177358 0.071698"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.149087 -0.023654 0.007772" rpy="-0.656375 0 0"/>
+      <geometry>
+        <box size="0.016226 0.177358 0.071698"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.14944 -0.001095 0.048195" rpy="-0.688653 0.282831 -0.225783"/>
+      <geometry>
+        <box size="0.004528 0.173585 0.015094"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.14944 -0.001095 0.048195" rpy="-0.688653 0.282831 -0.225783"/>
+      <geometry>
+        <box size="0.004528 0.173585 0.015094"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.143963 -0.142325 0.149419" rpy="-0.689784 0.390399 -0.237371"/>
+      <geometry>
+        <box size="0.004528 0.173585 0.015094"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.143963 -0.142325 0.149419" rpy="-0.689784 0.390399 -0.237371"/>
+      <geometry>
+        <box size="0.004528 0.173585 0.015094"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.143376 -0.16295 0.113632" rpy="-0.64577 0.030504 0.039623"/>
+      <geometry>
+        <box size="0.016226 0.173585 0.071698"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.143376 -0.16295 0.113632" rpy="-0.64577 0.030504 0.039623"/>
+      <geometry>
+        <box size="0.016226 0.173585 0.071698"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.113992 -0.21476 0.154045" rpy="-0.656375 0 0"/>
+      <geometry>
+        <box size="0.054717 0.098113 0.064151"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.113992 -0.21476 0.154045" rpy="-0.656375 0 0"/>
+      <geometry>
+        <box size="0.054717 0.098113 0.064151"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.113238 -0.119427 0.086296" rpy="-2.227175 0 0"/>
+      <geometry>
+        <cylinder radius="0.028679" length="0.150943"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.113238 -0.119427 0.086296" rpy="-2.227175 0 0"/>
+      <geometry>
+        <cylinder radius="0.028679" length="0.150943"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.113238 -0.04459 0.028636" rpy="-2.227175 0 0"/>
+      <geometry>
+        <cylinder radius="0.037736" length="0.041509"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.113238 -0.04459 0.028636" rpy="-2.227175 0 0"/>
+      <geometry>
+        <cylinder radius="0.037736" length="0.041509"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.113738 -0.02284 -0.046726" rpy="-0.656375 0 0"/>
+      <geometry>
+        <box size="0.061509 0.069811 0.011321"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.113738 -0.02284 -0.046726" rpy="-0.656375 0 0"/>
+      <geometry>
+        <box size="0.061509 0.069811 0.011321"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.073615 -0.00019 -0.000689" rpy="-1.5708 0 1.5708"/>
+      <geometry>
+        <cylinder radius="0.04" length="0.037736"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.073615 -0.00019 -0.000689" rpy="-1.5708 0 1.5708"/>
+      <geometry>
+        <cylinder radius="0.04" length="0.037736"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.115879 0.008331 -0.011344" rpy="-2.227175 0 0"/>
+      <geometry>
+        <cylinder radius="0.033962" length="0.086792"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.115879 0.008331 -0.011344" rpy="-2.227175 0 0"/>
+      <geometry>
+        <cylinder radius="0.033962" length="0.086792"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.12154 0.053961 -0.047932" rpy="-0.656375 0 0"/>
+      <geometry>
+        <box size="0.064151 0.003774 0.071698"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.12154 0.053961 -0.047932" rpy="-0.656375 0 0"/>
+      <geometry>
+        <box size="0.064151 0.003774 0.071698"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.12003 0.073308 -0.006499" rpy="0.013625 0 0"/>
+      <geometry>
+        <box size="0.060377 0.007547 0.022642"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.12003 0.073308 -0.006499" rpy="0.013625 0 0"/>
+      <geometry>
+        <box size="0.060377 0.007547 0.022642"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.117011 -0.13635 0.154336" rpy="0.99 0 0"/>
+      <geometry>
+        <box size="0.049057 0.005283 0.169811"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.117011 -0.13635 0.154336" rpy="0.99 0 0"/>
+      <geometry>
+        <box size="0.049057 0.005283 0.169811"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.083111 -0.31378 0.182934" rpy="0 -0.076371 1.570798"/>
+      <geometry>
+        <box size="0.044 0.008 0.0387"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.083111 -0.31378 0.182934" rpy="0 -0.076371 1.570798"/>
+      <geometry>
+        <box size="0.044 0.008 0.0387"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="front right elbow motor" type="revolute">
+    <parent link="front right upperarm"/>
+    <child link="front right forearm"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="50" lower="-0.45" upper="1.6" velocity="10"/>
+    <origin xyz="0.1122 -0.319729 0.182338" rpy="0 0 0"/>
+  </joint>
+  <link name="front right forearm">
+    <visual>
+      <origin xyz="0.001038 -0.271321 -0.225038" rpy="-1.5708 0 1.570801"/>
+      <geometry>
+        <cylinder radius="0.035094" length="0.039623"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.001038 -0.271321 -0.225038" rpy="-1.5708 0 1.570801"/>
+      <geometry>
+        <cylinder radius="0.035094" length="0.039623"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.001038 -0.227421 -0.230239" rpy="-0.947675 0 0"/>
+      <geometry>
+        <box size="0.037736 0.006038 0.041509"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.001038 -0.227421 -0.230239" rpy="-0.947675 0 0"/>
+      <geometry>
+        <box size="0.037736 0.006038 0.041509"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.001038 -0.232272 -0.198255" rpy="-0.093185 0 0"/>
+      <geometry>
+        <box size="0.037736 0.045283 0.006792"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.001038 -0.232272 -0.198255" rpy="-0.093185 0 0"/>
+      <geometry>
+        <box size="0.037736 0.045283 0.006792"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.001038 -0.205751 -0.206348" rpy="0.616815 0 0"/>
+      <geometry>
+        <box size="0.025283 0.022642 0.020755"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.001038 -0.205751 -0.206348" rpy="0.616815 0 0"/>
+      <geometry>
+        <box size="0.025283 0.022642 0.020755"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.001038 -0.168611 -0.18111" rpy="-2.513185 0 0"/>
+      <geometry>
+        <box size="0.019245 0.071698 0.017358"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.001038 -0.168611 -0.18111" rpy="-2.513185 0 0"/>
+      <geometry>
+        <box size="0.019245 0.071698 0.017358"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.001038 -0.125111 -0.148236" rpy="0.663625 0 0"/>
+      <geometry>
+        <box size="0.021887 0.037736 0.018491"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.001038 -0.125111 -0.148236" rpy="0.663625 0 0"/>
+      <geometry>
+        <box size="0.021887 0.037736 0.018491"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.001038 -0.100861 -0.126163" rpy="0.760415 0 0"/>
+      <geometry>
+        <box size="0.026415 0.030189 0.021132"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.001038 -0.100861 -0.126163" rpy="0.760415 0 0"/>
+      <geometry>
+        <box size="0.026415 0.030189 0.021132"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.001038 -0.084241 -0.106843" rpy="0.867225 0 0"/>
+      <geometry>
+        <box size="0.032453 0.022642 0.024528"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.001038 -0.084241 -0.106843" rpy="0.867225 0 0"/>
+      <geometry>
+        <box size="0.032453 0.022642 0.024528"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.001038 -0.068551 -0.08661" rpy="0.907225 0 0"/>
+      <geometry>
+        <box size="0.037736 0.033962 0.026415"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.001038 -0.068551 -0.08661" rpy="0.907225 0 0"/>
+      <geometry>
+        <box size="0.037736 0.033962 0.026415"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.001038 -0.049721 -0.059227" rpy="0.933245 0 0"/>
+      <geometry>
+        <cylinder radius="0.024528" length="0.030566"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.001038 -0.049721 -0.059227" rpy="0.933245 0 0"/>
+      <geometry>
+        <cylinder radius="0.024528" length="0.030566"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.001038 -0.034991 -0.038166" rpy="0.937225 0 0"/>
+      <geometry>
+        <box size="0.038113 0.022642 0.032075"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.001038 -0.034991 -0.038166" rpy="0.937225 0 0"/>
+      <geometry>
+        <box size="0.038113 0.022642 0.032075"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.001038 -0.022931 -0.021744" rpy="0.937225 0 0"/>
+      <geometry>
+        <box size="0.031321 0.018868 0.03283"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.001038 -0.022931 -0.021744" rpy="0.937225 0 0"/>
+      <geometry>
+        <box size="0.031321 0.018868 0.03283"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.001038 -0.010711 -0.006332" rpy="0.834035 0 0"/>
+      <geometry>
+        <box size="0.027925 0.018868 0.03283"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.001038 -0.010711 -0.006332" rpy="0.834035 0 0"/>
+      <geometry>
+        <box size="0.027925 0.018868 0.03283"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.001038 0.004299 0.006862" rpy="0.680845 0 0"/>
+      <geometry>
+        <box size="0.027547 0.018868 0.034717"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.001038 0.004299 0.006862" rpy="0.680845 0 0"/>
+      <geometry>
+        <box size="0.027547 0.018868 0.034717"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.001038 0.021259 0.019851" rpy="0.604465 0 0"/>
+      <geometry>
+        <box size="0.020377 0.026415 0.03434"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.001038 0.021259 0.019851" rpy="0.604465 0 0"/>
+      <geometry>
+        <box size="0.020377 0.026415 0.03434"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.001038 0.019179 -0.009531" rpy="0.494895 0 0"/>
+      <geometry>
+        <box size="0.010189 0.05283 0.00717"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.001038 0.019179 -0.009531" rpy="0.494895 0 0"/>
+      <geometry>
+        <box size="0.010189 0.05283 0.00717"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.001038 0.037459 0.024742" rpy="-1.5708 0 1.5708"/>
+      <geometry>
+        <cylinder radius="0.021887" length="0.019736"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.001038 0.037459 0.024742" rpy="-1.5708 0 1.5708"/>
+      <geometry>
+        <cylinder radius="0.021887" length="0.019736"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="front right touch sensor">
+  </link>
+  <joint name="front right forearm_front right touch sensor_joint" type="fixed">
+    <parent link="front right forearm"/>
+    <child link="front right touch sensor"/>
+    <origin xyz="0.001038 -0.271321 -0.225038" rpy="-1.5708 0 1.570801"/>
+  </joint>
+  <joint name="front right piston motor" type="continuous">
+    <parent link="front right upperarm"/>
+    <child link="solid_2"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="10" velocity="10"/>
+    <origin xyz="0.1172 -0.29077 0.2083" rpy="-0.5236 0 0"/>
+  </joint>
+  <link name="solid_2">
+  </link>
+  <joint name="front left shoulder abduction motor" type="revolute">
+    <parent link="base_link"/>
+    <child link="front left shoulder"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="50" lower="-0.6" upper="0.5" velocity="10"/>
+    <origin xyz="0.3635 0.0528 0.012399" rpy="1.5708 -0.00001 -1.570793"/>
+  </joint>
+  <link name="front left shoulder">
+    <visual>
+      <origin xyz="0.004874 0.000873 -0.000643" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.086792 0.124528 0.120755"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.004874 0.000873 -0.000643" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.086792 0.124528 0.120755"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.006447 0.000873 -0.000643" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.098113 0.098113 0.098113"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.006447 0.000873 -0.000643" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.098113 0.098113 0.098113"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.058145 0.000873 -0.000643" rpy="-1.5708 0 1.5708"/>
+      <geometry>
+        <cylinder radius="0.047925" length="0.006038"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.058145 0.000873 -0.000643" rpy="-1.5708 0 1.5708"/>
+      <geometry>
+        <cylinder radius="0.047925" length="0.006038"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="front left shoulder rotation motor" type="revolute">
+    <parent link="front left shoulder"/>
+    <child link="front left upperarm"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="50" lower="-1.7" upper="1.7" velocity="10"/>
+    <origin xyz="0 -0.00053 0" rpy="0 0 0"/>
+  </joint>
+  <link name="front left upperarm">
+    <visual>
+      <origin xyz="-0.117011 -0.32073 0.1829" rpy="-1.5708 0 1.5708"/>
+      <geometry>
+        <cylinder radius="0.02717" length="0.060377"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.117011 -0.32073 0.1829" rpy="-1.5708 0 1.5708"/>
+      <geometry>
+        <cylinder radius="0.02717" length="0.060377"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.117011 -0.25867 0.180358" rpy="0 0.05 1.570795"/>
+      <geometry>
+        <box size="0.09434 0.060377 0.053208"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.117011 -0.25867 0.180358" rpy="0 0.05 1.570795"/>
+      <geometry>
+        <box size="0.09434 0.060377 0.053208"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.149087 -0.023654 0.007772" rpy="-0.656375 0 0"/>
+      <geometry>
+        <box size="0.016226 0.177358 0.071698"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.149087 -0.023654 0.007772" rpy="-0.656375 0 0"/>
+      <geometry>
+        <box size="0.016226 0.177358 0.071698"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.14944 -0.001095 0.048195" rpy="-0.688653 0.282831 -0.225783"/>
+      <geometry>
+        <box size="0.004528 0.173585 0.015094"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.14944 -0.001095 0.048195" rpy="-0.688653 0.282831 -0.225783"/>
+      <geometry>
+        <box size="0.004528 0.173585 0.015094"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.143963 -0.142325 0.149419" rpy="-0.689784 0.390399 -0.237371"/>
+      <geometry>
+        <box size="0.004528 0.173585 0.015094"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.143963 -0.142325 0.149419" rpy="-0.689784 0.390399 -0.237371"/>
+      <geometry>
+        <box size="0.004528 0.173585 0.015094"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.143376 -0.16295 0.113632" rpy="-0.64577 0.030504 0.039623"/>
+      <geometry>
+        <box size="0.016226 0.173585 0.071698"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.143376 -0.16295 0.113632" rpy="-0.64577 0.030504 0.039623"/>
+      <geometry>
+        <box size="0.016226 0.173585 0.071698"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.113992 -0.21476 0.154045" rpy="-0.656375 0 0"/>
+      <geometry>
+        <box size="0.054717 0.098113 0.064151"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.113992 -0.21476 0.154045" rpy="-0.656375 0 0"/>
+      <geometry>
+        <box size="0.054717 0.098113 0.064151"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.113238 -0.119427 0.086296" rpy="-2.227175 0 0"/>
+      <geometry>
+        <cylinder radius="0.028679" length="0.150943"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.113238 -0.119427 0.086296" rpy="-2.227175 0 0"/>
+      <geometry>
+        <cylinder radius="0.028679" length="0.150943"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.113238 -0.04459 0.028636" rpy="-2.227175 0 0"/>
+      <geometry>
+        <cylinder radius="0.037736" length="0.041509"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.113238 -0.04459 0.028636" rpy="-2.227175 0 0"/>
+      <geometry>
+        <cylinder radius="0.037736" length="0.041509"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.113738 -0.02284 -0.046726" rpy="-0.656375 0 0"/>
+      <geometry>
+        <box size="0.061509 0.069811 0.011321"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.113738 -0.02284 -0.046726" rpy="-0.656375 0 0"/>
+      <geometry>
+        <box size="0.061509 0.069811 0.011321"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.073615 -0.00019 -0.000689" rpy="-1.5708 0 1.5708"/>
+      <geometry>
+        <cylinder radius="0.04" length="0.037736"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.073615 -0.00019 -0.000689" rpy="-1.5708 0 1.5708"/>
+      <geometry>
+        <cylinder radius="0.04" length="0.037736"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.115879 0.008331 -0.011344" rpy="-2.227175 0 0"/>
+      <geometry>
+        <cylinder radius="0.033962" length="0.086792"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.115879 0.008331 -0.011344" rpy="-2.227175 0 0"/>
+      <geometry>
+        <cylinder radius="0.033962" length="0.086792"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.12154 0.053961 -0.047932" rpy="-0.656375 0 0"/>
+      <geometry>
+        <box size="0.064151 0.003774 0.071698"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.12154 0.053961 -0.047932" rpy="-0.656375 0 0"/>
+      <geometry>
+        <box size="0.064151 0.003774 0.071698"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.12003 0.073308 -0.006499" rpy="0.013625 0 0"/>
+      <geometry>
+        <box size="0.060377 0.007547 0.022642"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.12003 0.073308 -0.006499" rpy="0.013625 0 0"/>
+      <geometry>
+        <box size="0.060377 0.007547 0.022642"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.117011 -0.13635 0.154336" rpy="0.99 0 0"/>
+      <geometry>
+        <box size="0.049057 0.005283 0.169811"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.117011 -0.13635 0.154336" rpy="0.99 0 0"/>
+      <geometry>
+        <box size="0.049057 0.005283 0.169811"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.083111 -0.31378 0.182934" rpy="0 -0.076371 1.570798"/>
+      <geometry>
+        <box size="0.044 0.008 0.0387"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.083111 -0.31378 0.182934" rpy="0 -0.076371 1.570798"/>
+      <geometry>
+        <box size="0.044 0.008 0.0387"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="front left elbow motor" type="revolute">
+    <parent link="front left upperarm"/>
+    <child link="front left forearm"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="50" lower="-0.45" upper="1.6" velocity="10"/>
+    <origin xyz="-0.1122 -0.319729 0.182338" rpy="0 0 0"/>
+  </joint>
+  <link name="front left forearm">
+    <visual>
+      <origin xyz="-0.001038 -0.271321 -0.225038" rpy="-1.5708 0 1.570801"/>
+      <geometry>
+        <cylinder radius="0.035094" length="0.039623"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.001038 -0.271321 -0.225038" rpy="-1.5708 0 1.570801"/>
+      <geometry>
+        <cylinder radius="0.035094" length="0.039623"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.001038 -0.227421 -0.230239" rpy="-0.947675 0 0"/>
+      <geometry>
+        <box size="0.037736 0.006038 0.041509"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.001038 -0.227421 -0.230239" rpy="-0.947675 0 0"/>
+      <geometry>
+        <box size="0.037736 0.006038 0.041509"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.001038 -0.232272 -0.198255" rpy="-0.093185 0 0"/>
+      <geometry>
+        <box size="0.037736 0.045283 0.006792"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.001038 -0.232272 -0.198255" rpy="-0.093185 0 0"/>
+      <geometry>
+        <box size="0.037736 0.045283 0.006792"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.001038 -0.205751 -0.206348" rpy="0.616815 0 0"/>
+      <geometry>
+        <box size="0.025283 0.022642 0.020755"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.001038 -0.205751 -0.206348" rpy="0.616815 0 0"/>
+      <geometry>
+        <box size="0.025283 0.022642 0.020755"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.001038 -0.168611 -0.18111" rpy="-2.513185 0 0"/>
+      <geometry>
+        <box size="0.019245 0.071698 0.017358"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.001038 -0.168611 -0.18111" rpy="-2.513185 0 0"/>
+      <geometry>
+        <box size="0.019245 0.071698 0.017358"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.001038 -0.125111 -0.148236" rpy="0.663625 0 0"/>
+      <geometry>
+        <box size="0.021887 0.037736 0.018491"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.001038 -0.125111 -0.148236" rpy="0.663625 0 0"/>
+      <geometry>
+        <box size="0.021887 0.037736 0.018491"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.001038 -0.100861 -0.126163" rpy="0.760415 0 0"/>
+      <geometry>
+        <box size="0.026415 0.030189 0.021132"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.001038 -0.100861 -0.126163" rpy="0.760415 0 0"/>
+      <geometry>
+        <box size="0.026415 0.030189 0.021132"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.001038 -0.084241 -0.106843" rpy="0.867225 0 0"/>
+      <geometry>
+        <box size="0.032453 0.022642 0.024528"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.001038 -0.084241 -0.106843" rpy="0.867225 0 0"/>
+      <geometry>
+        <box size="0.032453 0.022642 0.024528"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.001038 -0.068551 -0.08661" rpy="0.907225 0 0"/>
+      <geometry>
+        <box size="0.037736 0.033962 0.026415"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.001038 -0.068551 -0.08661" rpy="0.907225 0 0"/>
+      <geometry>
+        <box size="0.037736 0.033962 0.026415"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.001038 -0.049721 -0.059227" rpy="0.933245 0 0"/>
+      <geometry>
+        <cylinder radius="0.024528" length="0.030566"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.001038 -0.049721 -0.059227" rpy="0.933245 0 0"/>
+      <geometry>
+        <cylinder radius="0.024528" length="0.030566"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.001038 -0.034991 -0.038166" rpy="0.937225 0 0"/>
+      <geometry>
+        <box size="0.038113 0.022642 0.032075"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.001038 -0.034991 -0.038166" rpy="0.937225 0 0"/>
+      <geometry>
+        <box size="0.038113 0.022642 0.032075"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.001038 -0.022931 -0.021744" rpy="0.937225 0 0"/>
+      <geometry>
+        <box size="0.031321 0.018868 0.03283"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.001038 -0.022931 -0.021744" rpy="0.937225 0 0"/>
+      <geometry>
+        <box size="0.031321 0.018868 0.03283"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.001038 -0.010711 -0.006332" rpy="0.834035 0 0"/>
+      <geometry>
+        <box size="0.027925 0.018868 0.03283"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.001038 -0.010711 -0.006332" rpy="0.834035 0 0"/>
+      <geometry>
+        <box size="0.027925 0.018868 0.03283"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.001038 0.004299 0.006862" rpy="0.680845 0 0"/>
+      <geometry>
+        <box size="0.027547 0.018868 0.034717"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.001038 0.004299 0.006862" rpy="0.680845 0 0"/>
+      <geometry>
+        <box size="0.027547 0.018868 0.034717"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.001038 0.021259 0.019851" rpy="0.604465 0 0"/>
+      <geometry>
+        <box size="0.020377 0.026415 0.03434"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.001038 0.021259 0.019851" rpy="0.604465 0 0"/>
+      <geometry>
+        <box size="0.020377 0.026415 0.03434"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.001038 0.019179 -0.009531" rpy="0.494895 0 0"/>
+      <geometry>
+        <box size="0.010189 0.05283 0.00717"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.001038 0.019179 -0.009531" rpy="0.494895 0 0"/>
+      <geometry>
+        <box size="0.010189 0.05283 0.00717"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="-0.001038 0.037459 0.024742" rpy="-1.5708 0 1.5708"/>
+      <geometry>
+        <cylinder radius="0.021887" length="0.019736"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.001038 0.037459 0.024742" rpy="-1.5708 0 1.5708"/>
+      <geometry>
+        <cylinder radius="0.021887" length="0.019736"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="front left touch sensor">
+  </link>
+  <joint name="front left forearm_front left touch sensor_joint" type="fixed">
+    <parent link="front left forearm"/>
+    <child link="front left touch sensor"/>
+    <origin xyz="0.001038 -0.271321 -0.225038" rpy="-1.5708 0 1.570801"/>
+  </joint>
+  <joint name="front left piston motor" type="continuous">
+    <parent link="front left upperarm"/>
+    <child link="solid_3"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="10" velocity="10"/>
+    <origin xyz="-0.1172 -0.29077 0.2083" rpy="-0.5236 0 0"/>
+  </joint>
+  <link name="solid_3">
+  </link>
+  <link name="rear depth">
+  </link>
+  <joint name="base_link_rear depth_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="rear depth"/>
+    <origin xyz="-0.448136 -0.018115 0.012558" rpy="0.000015 0.474984 -3.141579"/>
+  </joint>
+  <link name="rear camera">
+  </link>
+  <joint name="base_link_rear camera_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="rear camera"/>
+    <origin xyz="-0.448136 -0.018115 0.012558" rpy="-1.570781 0.474984 -4.712375"/>
+  </joint>
+  <link name="left flank depth">
+  </link>
+  <joint name="base_link_left flank depth_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="left flank depth"/>
+    <origin xyz="-0.13568 0.110127 0.065528" rpy="-0.000003 0.403191 1.570802"/>
+  </joint>
+  <link name="left flank camera">
+  </link>
+  <joint name="base_link_left flank camera_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="left flank camera"/>
+    <origin xyz="-0.13568 0.110127 0.065528" rpy="-1.570799 0.403191 0.000005"/>
+  </joint>
+  <link name="right flank depth">
+  </link>
+  <joint name="base_link_right flank depth_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="right flank depth"/>
+    <origin xyz="-0.135678 -0.110163 0.06561" rpy="0.000005 0.413171 -1.570794"/>
+  </joint>
+  <link name="right flank camera">
+  </link>
+  <joint name="base_link_right flank camera_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="right flank camera"/>
+    <origin xyz="-0.135678 -0.110163 0.06561" rpy="-1.570791 0.413171 -3.141591"/>
+  </joint>
+  <link name="left head depth">
+  </link>
+  <joint name="base_link_left head depth_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="left head depth"/>
+    <origin xyz="0.46325 0.045111 0.029551" rpy="-0.158862 0.433786 -0.408004"/>
+  </joint>
+  <link name="left head camera">
+  </link>
+  <joint name="base_link_left head camera_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="left head camera"/>
+    <origin xyz="0.46325 0.045111 0.029551" rpy="-1.729658 0.433786 -1.9788"/>
+  </joint>
+  <link name="right head depth">
+  </link>
+  <joint name="base_link_right head depth_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="right head depth"/>
+    <origin xyz="0.462603 -0.044896 0.027972" rpy="0.193959 0.3931 0.409605"/>
+  </joint>
+  <link name="right head camera">
+  </link>
+  <joint name="base_link_right head camera_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="right head camera"/>
+    <origin xyz="0.462603 -0.044896 0.027972" rpy="-1.376837 0.3931 -1.161192"/>
+  </joint>
+  <link name="left bottom led">
+  </link>
+  <joint name="base_link_left bottom led_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="left bottom led"/>
+    <origin xyz="0.481728 0.081967 0.001191" rpy="1.873112 0.329178 0.767486"/>
+  </joint>
+  <link name="right bottom led">
+  </link>
+  <joint name="base_link_right bottom led_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="right bottom led"/>
+    <origin xyz="0.481728 -0.081963 0.001193" rpy="1.279539 0.338923 -0.733718"/>
+  </joint>
+  <link name="right middle down led">
+  </link>
+  <joint name="base_link_right middle down led_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="right middle down led"/>
+    <origin xyz="0.489879 -0.081963 0.018293" rpy="1.279539 0.338923 -0.733718"/>
+  </joint>
+  <link name="right middle up led">
+  </link>
+  <joint name="base_link_right middle up led_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="right middle up led"/>
+    <origin xyz="0.497601 -0.081963 0.034493" rpy="1.279539 0.338923 -0.733718"/>
+  </joint>
+  <link name="right top led">
+  </link>
+  <joint name="base_link_right top led_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="right top led"/>
+    <origin xyz="0.505323 -0.081963 0.050723" rpy="1.279539 0.338923 -0.733718"/>
+  </joint>
+  <link name="left middle down led">
+  </link>
+  <joint name="base_link_left middle down led_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="left middle down led"/>
+    <origin xyz="0.489879 0.081966 0.018291" rpy="1.873112 0.329178 0.767486"/>
+  </joint>
+  <link name="left middle up led">
+  </link>
+  <joint name="base_link_left middle up led_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="left middle up led"/>
+    <origin xyz="0.497601 0.081967 0.034491" rpy="1.873112 0.329178 0.767486"/>
+  </joint>
+  <link name="left top led">
+  </link>
+  <joint name="base_link_left top led_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="left top led"/>
+    <origin xyz="0.505323 0.081967 0.050721" rpy="1.873112 0.329178 0.767486"/>
+  </joint>
+  <link name="inertial unit">
+  </link>
+  <joint name="base_link_inertial unit_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="inertial unit"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+  </joint>
+  <link name="gyro">
+  </link>
+  <joint name="base_link_gyro_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="gyro"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+  </joint>
+  <link name="gps">
+  </link>
+  <joint name="base_link_gps_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="gps"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+  </joint>
+  <link name="accelerometer">
+  </link>
+  <joint name="base_link_accelerometer_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="accelerometer"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+  </joint>
+</robot>
diff --git a/resource/spotarm_control.urdf b/resource/spotarm_control.urdf
deleted file mode 100644
index f8343bc..0000000
--- a/resource/spotarm_control.urdf
+++ /dev/null
@@ -1,69 +0,0 @@
-<?xml version="1.0"?>
-<robot name="SpotArm" xmlns:xacro="http://ros.org/wiki/xacro">
-  <webots>
-    <plugin type="webots_ros2_control::Ros2Control"/>
-  </webots>
-  <ros2_control name="WebotsControl" type="system">
-    <hardware>
-      <plugin>webots_ros2_control::Ros2ControlSystem</plugin>
-    </hardware>
-
-    <!-- SpotArm -->
-    <joint name="spotarm_1_joint">
-      <command_interface name="position">
-        </command_interface>
-      <state_interface name="position"/>
-      <state_interface name="velocity"/>
-    </joint>
-    <joint name="spotarm_2_joint">
-      <command_interface name="position">
-        </command_interface>
-      <state_interface name="position"/>
-      <state_interface name="velocity"/>
-    </joint>
-    <joint name="spotarm_3_joint">
-      <command_interface name="position">
-        </command_interface>
-      <state_interface name="position"/>
-      <state_interface name="velocity"/>
-    </joint>
-    <joint name="spotarm_4_joint">
-      <command_interface name="position">
-        </command_interface>
-      <state_interface name="position"/>
-      <state_interface name="velocity"/>
-    </joint>
-    <joint name="Slider11">
-      <command_interface name="position">
-        </command_interface>
-      <state_interface name="position"/>
-      <state_interface name="velocity"/>
-    </joint>
-    <joint name="spotarm_5_joint">
-      <command_interface name="position">
-        </command_interface>
-      <state_interface name="position"/>
-      <state_interface name="velocity"/>
-    </joint>
-    <joint name="spotarm_6_joint">
-      <command_interface name="position">
-        </command_interface>
-      <state_interface name="position"/>
-      <state_interface name="velocity"/>
-    </joint>
-
-    <!-- Tiago Gripper -->
-    <joint name="gripper_left_finger_joint">
-      <command_interface name="position">
-        </command_interface>
-      <state_interface name="position"/>
-      <state_interface name="velocity"/>
-    </joint>
-    <joint name="gripper_right_finger_joint">
-      <command_interface name="position">
-        </command_interface>
-      <state_interface name="position"/>
-      <state_interface name="velocity"/>
-    </joint>
-  </ros2_control>
-</robot>