diff --git a/res/config/example_controller.yaml b/res/config/example_controller.yaml
index 3ea1c09..d367ec4 100644
--- a/res/config/example_controller.yaml
+++ b/res/config/example_controller.yaml
@@ -7,6 +7,7 @@ CartesianImpedance_trajectory_controller:
     - joint4
     - joint5
     - joint6
+    - joint7
   end_effector: tool0                   # Link to control arm in
   update_frequency: 500                 # Controller update frequency in Hz
   dynamic_reconfigure: true             # Starts dynamic reconfigure server
@@ -38,3 +39,4 @@ hardware_interface:
     - joint4
     - joint5
     - joint6
+    - joint7
diff --git a/res/launch/examples.launch b/res/launch/examples.launch
index 5bae475..7f85fbb 100644
--- a/res/launch/examples.launch
+++ b/res/launch/examples.launch
@@ -38,4 +38,4 @@
         <!-- Run a python script to send a service call to gazebo_ros to spawn a URDF robot -->
         <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
                 args="-urdf -model robot -param robot_description"/>
-</launch>
\ No newline at end of file
+</launch>
diff --git a/res/urdf/robot.urdf.xacro b/res/urdf/robot.urdf.xacro
index b4b1610..dcf04ec 100644
--- a/res/urdf/robot.urdf.xacro
+++ b/res/urdf/robot.urdf.xacro
@@ -1,372 +1,115 @@
 <?xml version="1.0"?>
 <robot name="robot" xmlns:xacro="http://ros.org/wiki/xacro">
 
-        <!--
-        A fictitious robot model for testing purposes.
-        The kinematics is roughly inspired by the universal robots ur10.
-        The idea for an example robot and the URDF is from Stefan Scherzinger's repo:
-        https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers
-
-        The robot has 6 revolute joints and a spherical end-effector.
-        Some rough inertia values and Gazebo references are added.
-        -->
-
-        <!-- joint properties -->
-        <xacro:property name="joint_radius" value="0.02" />
-        <xacro:property name="joint_length" value="0.03" />
-
-        <!-- link properties -->
-        <xacro:property name="link_radius" value="0.01" />
-        <xacro:property name="link1_length" value="0.15" />
-        <xacro:property name="link2_length" value="0.5" />
-        <xacro:property name="link3_length" value="0.5" />
-        <xacro:property name="link4_length" value="0.15" />
-        <xacro:property name="link5_length" value="0.10" />
-        <xacro:property name="link6_length" value="0.05" />
-
-        <xacro:property name="inertia_radius" value="0.05" />
-        <xacro:property name="inertia_length" value="0.3" />
-        <xacro:property name="link_mass" value="1.0" />
-
-        <!-- colors -->
-        <material name="Orange">
-                <color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
-        </material>
-
-        <!--    Macro Source: https://github.com/ros-industrial/universal_robot/
-                Author: Ludovic Delval
-                Contributor: Felix Messmer      -->
-        <xacro:macro name="cylinder_inertial" params="radius length mass *origin">
-                <inertial>
-                        <mass value="${mass}" />
-                        <xacro:insert_block name="origin" />
-                        <inertia ixx="${0.0833333 * mass * (3 * radius * radius + length * length)}" ixy="0.0" ixz="0.0"
-                                iyy="${0.0833333 * mass * (3 * radius * radius + length * length)}" iyz="0.0"
-                                izz="${0.5 * mass * radius * radius}" />
-                </inertial>
-        </xacro:macro>
-
-        <!-- Connect to world -->
-        <link name="world" />
-        <joint name="world_joint" type="fixed">
-                <parent link="world" />
-                <child link = "base_link" />
-                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
-        </joint>
-
-        <link name="base_link" >
-                <visual name="joint">
-                        <origin xyz="0 0 ${joint_length / 2.0}" rpy="0 0 0" />
-                        <geometry>
-                                <cylinder length="${joint_length}" radius="${joint_radius}"/>
-                        </geometry>
-                        <material name="Orange"/>
-                </visual>
-                <xacro:cylinder_inertial radius="${inertia_radius}" length="${inertia_length}" mass="${link_mass}">
-                        <origin xyz="0 0 0" rpy="0 0 0" />
-                </xacro:cylinder_inertial>
-        </link>
-
-        <joint name="joint1" type="revolute">
-                <parent link="base_link" />
-                <child link = "link1" />
-                <origin xyz="0.0 0.0 ${3.0 / 2.0 * joint_length}" rpy="0.0 0.0 0.0" />
-                <axis xyz="0 0 1" />
-                <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="2.0" velocity="10.0"/>
-        </joint>
-
-        <link name="link1">
-                <visual name="joint1">
-                        <origin xyz="0 0 0" rpy="0 0 0" />
-                        <geometry>
-                                <cylinder length="${joint_length}" radius="${joint_radius}"/>
-                        </geometry>
-                        <material name="Orange"/>
-                </visual>
-                <visual name="link">
-                        <origin xyz="0 0 ${link1_length / 2.0}" rpy="0 0 0" />
-                        <geometry>
-                                <cylinder length="${link1_length}" radius="${link_radius}"/>
-                        </geometry>
-                        <material name="Orange"/>
-                </visual>
-                <visual name="joint2">
-                        <origin xyz="0 0 ${link1_length}" rpy="${pi / 2.0} 0 0" />
-                        <geometry>
-                                <cylinder length="${joint_length}" radius="${joint_radius}"/>
-                        </geometry>
-                        <material name="Orange"/>
-                </visual>
-                <xacro:cylinder_inertial radius="${inertia_radius}" length="${inertia_length}" mass="${link_mass}">
-                        <origin xyz="0 0 ${link1_length / 2.0}" rpy="0 0 0" />
-                </xacro:cylinder_inertial>
-        </link>
-
-        <joint name="joint2" type="revolute">
-                <parent link="link1" />
-                <child link = "link2" />
-                <origin xyz="0.0 ${joint_length} ${link1_length}" rpy="0.0 ${pi / 2.0} 0.0" />
-                <axis xyz="0 1 0" />
-                <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="2.0" velocity="10.0"/>
-        </joint>
-
-        <link name="link2">
-                <visual name="joint2">
-                        <origin xyz="0 0 0" rpy="${pi / 2.0} 0 0" />
-                        <geometry>
-                                <cylinder length="${joint_length}" radius="${joint_radius}"/>
-                        </geometry>
-                        <material name="Orange"/>
-                </visual>
-                <visual name="link">
-                        <origin xyz="0 0 ${link2_length / 2.0}" rpy="0 0 0" />
-                        <geometry>
-                                <cylinder length="${link2_length}" radius="${link_radius}"/>
-                        </geometry>
-                        <material name="Orange"/>
-                </visual>
-                <visual name="joint3">
-                        <origin xyz="0 0 ${link2_length}" rpy="${pi / 2.0} 0 0" />
-                        <geometry>
-                                <cylinder length="${joint_length}" radius="${joint_radius}"/>
-                        </geometry>
-                        <material name="Orange"/>
-                </visual>
-                <xacro:cylinder_inertial radius="${inertia_radius}" length="${inertia_length}" mass="${link_mass}">
-                        <origin xyz="0 0 ${link2_length / 2.0}" rpy="0 0 0" />
-                </xacro:cylinder_inertial>
-        </link>
-
-        <joint name="joint3" type="revolute">
-                <parent link="link2" />
-                <child link = "link3" />
-                <origin xyz="0.0 -${joint_length} ${link2_length}" rpy="0.0 0.0 0.0" />
-                <axis xyz="0 1 0" />
-                <limit lower="${-pi}" upper="${pi}" effort="2.0" velocity="10.0"/>
-        </joint>
-
-        <link name="link3">
-                <visual name="joint3">
-                        <origin xyz="0 0 0" rpy="${pi / 2.0} 0 0" />
-                        <geometry>
-                                <cylinder length="${joint_length}" radius="${joint_radius}"/>
-                        </geometry>
-                        <material name="Orange"/>
-                </visual>
-                <visual name="link">
-                        <origin xyz="0 0 ${link3_length / 2.0}" rpy="0 0 0" />
-                        <geometry>
-                                <cylinder length="${link3_length}" radius="${link_radius}"/>
-                        </geometry>
-                        <material name="Orange"/>
-                </visual>
-                <visual name="joint4">
-                        <origin xyz="0 0 ${link3_length}" rpy="${pi / 2.0} 0 0" />
-                        <geometry>
-                                <cylinder length="${joint_length}" radius="${joint_radius}"/>
-                        </geometry>
-                        <material name="Orange"/>
-                </visual>
-                <xacro:cylinder_inertial radius="${inertia_radius}" length="${inertia_length}" mass="${link_mass}">
-                        <origin xyz="0 0 ${link3_length / 2.0}" rpy="0 0 0" />
-                </xacro:cylinder_inertial>
-        </link>
-
-        <joint name="joint4" type="revolute">
-                <parent link="link3" />
-                <child link = "link4" />
-                <origin xyz="0.0 ${joint_length} ${link3_length}" rpy="0.0 ${pi / 2.0} 0.0" />
-                <axis xyz="0 1 0" />
-                <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="2.0" velocity="10.0"/>
-        </joint>
-
-        <link name="link4">
-                <visual name="joint4">
-                        <origin xyz="0 0 0" rpy="${pi / 2.0} 0 0" />
-                        <geometry>
-                                <cylinder length="${joint_length}" radius="${joint_radius}"/>
-                        </geometry>
-                        <material name="Orange"/>
-                </visual>
-                <visual name="link">
-                        <origin xyz="0 ${link4_length / 2.0} 0" rpy="${pi / 2.0} 0 0" />
-                        <geometry>
-                                <cylinder length="${link4_length}" radius="${link_radius}"/>
-                        </geometry>
-                        <material name="Orange"/>
-                </visual>
-                <visual name="joint5">
-                        <origin xyz="0 ${link4_length} 0" rpy="0 0 0" />
-                        <geometry>
-                                <cylinder length="${joint_length}" radius="${joint_radius}"/>
-                        </geometry>
-                        <material name="Orange"/>
-                </visual>
-                <xacro:cylinder_inertial radius="${inertia_radius}" length="${0.5 * inertia_length}" mass="${link_mass}">
-                        <origin xyz="0 ${link4_length / 2.0} 0" rpy="${pi / 2.0} 0 0" />
-                </xacro:cylinder_inertial>
-        </link>
-
-        <joint name="joint5" type="revolute">
-                <parent link="link4" />
-                <child link = "link5" />
-                <origin xyz="0.0 ${link4_length} ${joint_length}" rpy="0.0 0.0 0.0" />
-                <axis xyz="0 0 1" />
-                <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="2.0" velocity="10.0"/>
-        </joint>
-
-        <link name="link5">
-                <visual name="joint5">
-                        <origin xyz="0 0 0" rpy="0 0 0" />
-                        <geometry>
-                                <cylinder length="${joint_length}" radius="${joint_radius}"/>
-                        </geometry>
-                        <material name="Orange"/>
-                </visual>
-                <visual name="link">
-                        <origin xyz="0 0 ${link5_length / 2.0}" rpy="0 0 0" />
-                        <geometry>
-                                <cylinder length="${link5_length}" radius="${link_radius}"/>
-                        </geometry>
-                        <material name="Orange"/>
-                </visual>
-                <visual name="joint6">
-                        <origin xyz="0 0 ${link5_length}" rpy="${pi / 2.0} 0 0" />
-                        <geometry>
-                                <cylinder length="${joint_length}" radius="${joint_radius}"/>
-                        </geometry>
-                        <material name="Orange"/>
-                </visual>
-                <xacro:cylinder_inertial radius="${inertia_radius}" length="${link5_length / 2.0}" mass="${link_mass}">
-                        <origin xyz="0 0 ${link5_length / 2.0}" rpy="${pi / 2.0} 0 0" />
-                </xacro:cylinder_inertial>
-        </link>
-
-        <joint name="joint6" type="revolute">
-                <parent link="link5" />
-                <child link = "link6" />
-                <origin xyz="0.0 ${joint_length} ${link5_length}" rpy="0.0 0.0 0.0" />
-                <axis xyz="0 1 0" />
-                <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="2.0" velocity="10.0"/>
-        </joint>
-
-        <link name="link6">
-                <visual name="joint6">
-                        <origin xyz="0 0 0" rpy="${pi / 2.0} 0 0" />
-                        <geometry>
-                                <cylinder length="${joint_length}" radius="${joint_radius}"/>
-                        </geometry>
-                        <material name="Orange"/>
-                </visual>
-                <visual name="link">
-                        <origin xyz="0 ${link6_length / 2.0} 0" rpy="${-pi / 2.0} 0 0" />
-                        <geometry>
-                                <cylinder length="${link6_length}" radius="${link_radius}"/>
-                        </geometry>
-                        <material name="Orange"/>
-                </visual>
-                <xacro:cylinder_inertial radius="${inertia_radius}" length="${link6_length / 2.0}" mass="${link_mass}">
-                        <origin xyz="0 ${link6_length / 2.0} 0" rpy="${pi / 2.0} 0 0" />
-                </xacro:cylinder_inertial>
-        </link>
-
-        <!-- The default end-effector -->
-        <joint name="tool0_joint" type="fixed">
-                <origin xyz="0 0 ${link6_length / 2.0}" rpy="0 0 0"/>
-                <parent link="link6"/>
-                <child link="tool0"/>
-        </joint>
-        <link name="tool0">
-                <visual>
-                        <origin xyz="0 0 0" rpy="0 0 0" />
-                        <geometry>
-                                <sphere radius="${link_radius}"/>
-                        </geometry>
-                        <material name="Orange"/>
-                </visual>
-        </link>
-
-        <!-- Transmission and motors -->
-        <transmission name="joint1_trans">
-                <type>transmission_interface/SimpleTransmission</type>
-                <joint name="joint1">
-                        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-                </joint>
-                <actuator name="joint1_motor">
-                        <mechanicalReduction>1</mechanicalReduction>
-                </actuator>
-        </transmission>
-
-        <transmission name="joint2_trans">
-                <type>transmission_interface/SimpleTransmission</type>
-                <joint name="joint2">
-                        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-                </joint>
-                <actuator name="joint2_motor">
-                        <mechanicalReduction>1</mechanicalReduction>
-                </actuator>
-        </transmission>
-
-        <transmission name="joint3_trans">
-                <type>transmission_interface/SimpleTransmission</type>
-                <joint name="joint3">
-                        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-                </joint>
-                <actuator name="joint3_motor">
-                        <mechanicalReduction>1</mechanicalReduction>
-                </actuator>
-        </transmission>
-
-        <transmission name="joint4_trans">
-                <type>transmission_interface/SimpleTransmission</type>
-                <joint name="joint4">
-                        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-                </joint>
-                <actuator name="joint4_motor">
-                        <mechanicalReduction>1</mechanicalReduction>
-                </actuator>
-        </transmission>
-
-        <transmission name="joint5_trans">
-                <type>transmission_interface/SimpleTransmission</type>
-                <joint name="joint5">
-                        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-                </joint>
-                <actuator name="joint5_motor">
-                        <mechanicalReduction>1</mechanicalReduction>
-                </actuator>
-        </transmission>
-
-        <transmission name="joint6_trans">
-                <type>transmission_interface/SimpleTransmission</type>
-                <joint name="joint6">
-                        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-                </joint>
-                <actuator name="joint6_motor">
-                        <mechanicalReduction>1</mechanicalReduction>
-                </actuator>
-        </transmission>
-
-        <!-- Load Gazebo ROS Control, & Disable gravity -->
+  <!-- Properties for materials -->
+  <material name="blue">
+    <color rgba="0 0 0.8 1"/>
+  </material>
+
+<!-- Connect to world -->
+<link name="world" />
+<joint name="world_joint" type="fixed">
+        <parent link="world" />
+        <child link = "base_link" />
+        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+</joint>
+
+  <!-- Base link -->
+  <link name="base_link">
+    <visual>
+      <geometry>
+        <cylinder length="0.1" radius="0.05"/>
+      </geometry>
+      <material name="blue"/>
+    </visual>
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="1"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
+    </inertial>
+  </link>
+
+  <!-- Joint and link definitions -->
+  <xacro:macro name="rotational_joint" params="prefix parent child xyz rpy">
+    <joint name="${prefix}" type="revolute">
+      <origin xyz="${xyz}" rpy="${rpy}"/>
+      <parent link="${parent}"/>
+      <child link="${child}"/>
+      <axis xyz="0 0 1"/>
+      <limit lower="-2.967" upper="2.967" effort="300" velocity="2.5"/>
+    </joint>
+    
+    <link name="${child}">
+      <visual>
+        <geometry>
+          <cylinder length="0.05" radius="0.05"/>
+        </geometry>
+        <material name="blue"/>
+      </visual>   
+      <inertial>
+        <!-- Generic inertial properties, adjust for accuracy -->
+        <origin xyz="${xyz}" rpy="${rpy}"/>
+        <mass value="1"/>
+        <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
+      </inertial>
+    </link>
+
+    <!-- Transmission -->
+    <transmission name="${prefix}_trans">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${prefix}">
+        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      </joint>
+      <actuator name="${prefix}_motor">
+        <mechanicalReduction>1</mechanicalReduction>
+      </actuator>
+    </transmission>
+    
+    
+     <!-- Gazebo configuration for the joint -->
+    <gazebo reference="${prefix}">
+      <dynamics damping="0.01" friction="0.02"/>
+    </gazebo>
+    
+    <gazebo reference="${child}">
+        <turnGravityOff>true</turnGravityOff>
+    </gazebo>
+  </xacro:macro>
+
+  <!-- Chain of joints and links -->
+  <xacro:rotational_joint prefix="joint1" parent="base_link" child="link1" xyz="0 0 0.333" rpy="0 0 0"/>
+  <xacro:rotational_joint prefix="joint2" parent="link1" child="link2" xyz="0 0 0" rpy="0 -1.5708 0"/>
+  <xacro:rotational_joint prefix="joint3" parent="link2" child="link3" xyz="0 0.316 0" rpy="0 1.5708 0"/>
+  <xacro:rotational_joint prefix="joint4" parent="link3" child="link4" xyz="0.0825 0 0" rpy="0 1.5708 0"/>
+  <xacro:rotational_joint prefix="joint5" parent="link4" child="link5" xyz="-0.0825 0 0.384" rpy="0 -1.5708 0"/>
+  <xacro:rotational_joint prefix="joint6" parent="link5" child="link6" xyz="0 0 0" rpy="0 1.5708 0"/>
+  <xacro:rotational_joint prefix="joint7" parent="link6" child="link7" xyz="0.088 0 0" rpy="0 1.5708 0"/>
+
+  <!-- Joint connecting the last link and the end-effector -->
+  <joint name="tool0_joint" type="fixed">
+    <parent link="link7"/>
+    <child link="tool0"/>
+    <origin xyz="0 0 0.107" rpy="0 0 0"/>
+  </joint>
+
+  <!-- End-effector link -->
+  <link name="tool0">
+    <visual>
+      <geometry>
+        <sphere radius="0.05"/>
+      </geometry>
+      <material name="blue"/>
+    </visual>
+  </link>
+  
+
+
+  <!-- Gazebo ROS Control plugin -->
         <gazebo>
                 <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so" />
         </gazebo>
-        <gazebo reference="link1">
-                <turnGravityOff>true</turnGravityOff>
-        </gazebo>
-        <gazebo reference="link2">
-                <turnGravityOff>true</turnGravityOff>
-        </gazebo>
-        <gazebo reference="link3">
-                <turnGravityOff>true</turnGravityOff>
-        </gazebo>
-        <gazebo reference="link4">
-                <turnGravityOff>true</turnGravityOff>
-        </gazebo>
-        <gazebo reference="link5">
-                <turnGravityOff>true</turnGravityOff>
-        </gazebo>
-        <gazebo reference="link6">
-                <turnGravityOff>true</turnGravityOff>
-        </gazebo>
+
+   
+
 </robot>
+
diff --git a/res/urdf/robot_old.urdf.xacro b/res/urdf/robot_old.urdf.xacro
new file mode 100644
index 0000000..b4b1610
--- /dev/null
+++ b/res/urdf/robot_old.urdf.xacro
@@ -0,0 +1,372 @@
+<?xml version="1.0"?>
+<robot name="robot" xmlns:xacro="http://ros.org/wiki/xacro">
+
+        <!--
+        A fictitious robot model for testing purposes.
+        The kinematics is roughly inspired by the universal robots ur10.
+        The idea for an example robot and the URDF is from Stefan Scherzinger's repo:
+        https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers
+
+        The robot has 6 revolute joints and a spherical end-effector.
+        Some rough inertia values and Gazebo references are added.
+        -->
+
+        <!-- joint properties -->
+        <xacro:property name="joint_radius" value="0.02" />
+        <xacro:property name="joint_length" value="0.03" />
+
+        <!-- link properties -->
+        <xacro:property name="link_radius" value="0.01" />
+        <xacro:property name="link1_length" value="0.15" />
+        <xacro:property name="link2_length" value="0.5" />
+        <xacro:property name="link3_length" value="0.5" />
+        <xacro:property name="link4_length" value="0.15" />
+        <xacro:property name="link5_length" value="0.10" />
+        <xacro:property name="link6_length" value="0.05" />
+
+        <xacro:property name="inertia_radius" value="0.05" />
+        <xacro:property name="inertia_length" value="0.3" />
+        <xacro:property name="link_mass" value="1.0" />
+
+        <!-- colors -->
+        <material name="Orange">
+                <color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
+        </material>
+
+        <!--    Macro Source: https://github.com/ros-industrial/universal_robot/
+                Author: Ludovic Delval
+                Contributor: Felix Messmer      -->
+        <xacro:macro name="cylinder_inertial" params="radius length mass *origin">
+                <inertial>
+                        <mass value="${mass}" />
+                        <xacro:insert_block name="origin" />
+                        <inertia ixx="${0.0833333 * mass * (3 * radius * radius + length * length)}" ixy="0.0" ixz="0.0"
+                                iyy="${0.0833333 * mass * (3 * radius * radius + length * length)}" iyz="0.0"
+                                izz="${0.5 * mass * radius * radius}" />
+                </inertial>
+        </xacro:macro>
+
+        <!-- Connect to world -->
+        <link name="world" />
+        <joint name="world_joint" type="fixed">
+                <parent link="world" />
+                <child link = "base_link" />
+                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+        </joint>
+
+        <link name="base_link" >
+                <visual name="joint">
+                        <origin xyz="0 0 ${joint_length / 2.0}" rpy="0 0 0" />
+                        <geometry>
+                                <cylinder length="${joint_length}" radius="${joint_radius}"/>
+                        </geometry>
+                        <material name="Orange"/>
+                </visual>
+                <xacro:cylinder_inertial radius="${inertia_radius}" length="${inertia_length}" mass="${link_mass}">
+                        <origin xyz="0 0 0" rpy="0 0 0" />
+                </xacro:cylinder_inertial>
+        </link>
+
+        <joint name="joint1" type="revolute">
+                <parent link="base_link" />
+                <child link = "link1" />
+                <origin xyz="0.0 0.0 ${3.0 / 2.0 * joint_length}" rpy="0.0 0.0 0.0" />
+                <axis xyz="0 0 1" />
+                <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="2.0" velocity="10.0"/>
+        </joint>
+
+        <link name="link1">
+                <visual name="joint1">
+                        <origin xyz="0 0 0" rpy="0 0 0" />
+                        <geometry>
+                                <cylinder length="${joint_length}" radius="${joint_radius}"/>
+                        </geometry>
+                        <material name="Orange"/>
+                </visual>
+                <visual name="link">
+                        <origin xyz="0 0 ${link1_length / 2.0}" rpy="0 0 0" />
+                        <geometry>
+                                <cylinder length="${link1_length}" radius="${link_radius}"/>
+                        </geometry>
+                        <material name="Orange"/>
+                </visual>
+                <visual name="joint2">
+                        <origin xyz="0 0 ${link1_length}" rpy="${pi / 2.0} 0 0" />
+                        <geometry>
+                                <cylinder length="${joint_length}" radius="${joint_radius}"/>
+                        </geometry>
+                        <material name="Orange"/>
+                </visual>
+                <xacro:cylinder_inertial radius="${inertia_radius}" length="${inertia_length}" mass="${link_mass}">
+                        <origin xyz="0 0 ${link1_length / 2.0}" rpy="0 0 0" />
+                </xacro:cylinder_inertial>
+        </link>
+
+        <joint name="joint2" type="revolute">
+                <parent link="link1" />
+                <child link = "link2" />
+                <origin xyz="0.0 ${joint_length} ${link1_length}" rpy="0.0 ${pi / 2.0} 0.0" />
+                <axis xyz="0 1 0" />
+                <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="2.0" velocity="10.0"/>
+        </joint>
+
+        <link name="link2">
+                <visual name="joint2">
+                        <origin xyz="0 0 0" rpy="${pi / 2.0} 0 0" />
+                        <geometry>
+                                <cylinder length="${joint_length}" radius="${joint_radius}"/>
+                        </geometry>
+                        <material name="Orange"/>
+                </visual>
+                <visual name="link">
+                        <origin xyz="0 0 ${link2_length / 2.0}" rpy="0 0 0" />
+                        <geometry>
+                                <cylinder length="${link2_length}" radius="${link_radius}"/>
+                        </geometry>
+                        <material name="Orange"/>
+                </visual>
+                <visual name="joint3">
+                        <origin xyz="0 0 ${link2_length}" rpy="${pi / 2.0} 0 0" />
+                        <geometry>
+                                <cylinder length="${joint_length}" radius="${joint_radius}"/>
+                        </geometry>
+                        <material name="Orange"/>
+                </visual>
+                <xacro:cylinder_inertial radius="${inertia_radius}" length="${inertia_length}" mass="${link_mass}">
+                        <origin xyz="0 0 ${link2_length / 2.0}" rpy="0 0 0" />
+                </xacro:cylinder_inertial>
+        </link>
+
+        <joint name="joint3" type="revolute">
+                <parent link="link2" />
+                <child link = "link3" />
+                <origin xyz="0.0 -${joint_length} ${link2_length}" rpy="0.0 0.0 0.0" />
+                <axis xyz="0 1 0" />
+                <limit lower="${-pi}" upper="${pi}" effort="2.0" velocity="10.0"/>
+        </joint>
+
+        <link name="link3">
+                <visual name="joint3">
+                        <origin xyz="0 0 0" rpy="${pi / 2.0} 0 0" />
+                        <geometry>
+                                <cylinder length="${joint_length}" radius="${joint_radius}"/>
+                        </geometry>
+                        <material name="Orange"/>
+                </visual>
+                <visual name="link">
+                        <origin xyz="0 0 ${link3_length / 2.0}" rpy="0 0 0" />
+                        <geometry>
+                                <cylinder length="${link3_length}" radius="${link_radius}"/>
+                        </geometry>
+                        <material name="Orange"/>
+                </visual>
+                <visual name="joint4">
+                        <origin xyz="0 0 ${link3_length}" rpy="${pi / 2.0} 0 0" />
+                        <geometry>
+                                <cylinder length="${joint_length}" radius="${joint_radius}"/>
+                        </geometry>
+                        <material name="Orange"/>
+                </visual>
+                <xacro:cylinder_inertial radius="${inertia_radius}" length="${inertia_length}" mass="${link_mass}">
+                        <origin xyz="0 0 ${link3_length / 2.0}" rpy="0 0 0" />
+                </xacro:cylinder_inertial>
+        </link>
+
+        <joint name="joint4" type="revolute">
+                <parent link="link3" />
+                <child link = "link4" />
+                <origin xyz="0.0 ${joint_length} ${link3_length}" rpy="0.0 ${pi / 2.0} 0.0" />
+                <axis xyz="0 1 0" />
+                <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="2.0" velocity="10.0"/>
+        </joint>
+
+        <link name="link4">
+                <visual name="joint4">
+                        <origin xyz="0 0 0" rpy="${pi / 2.0} 0 0" />
+                        <geometry>
+                                <cylinder length="${joint_length}" radius="${joint_radius}"/>
+                        </geometry>
+                        <material name="Orange"/>
+                </visual>
+                <visual name="link">
+                        <origin xyz="0 ${link4_length / 2.0} 0" rpy="${pi / 2.0} 0 0" />
+                        <geometry>
+                                <cylinder length="${link4_length}" radius="${link_radius}"/>
+                        </geometry>
+                        <material name="Orange"/>
+                </visual>
+                <visual name="joint5">
+                        <origin xyz="0 ${link4_length} 0" rpy="0 0 0" />
+                        <geometry>
+                                <cylinder length="${joint_length}" radius="${joint_radius}"/>
+                        </geometry>
+                        <material name="Orange"/>
+                </visual>
+                <xacro:cylinder_inertial radius="${inertia_radius}" length="${0.5 * inertia_length}" mass="${link_mass}">
+                        <origin xyz="0 ${link4_length / 2.0} 0" rpy="${pi / 2.0} 0 0" />
+                </xacro:cylinder_inertial>
+        </link>
+
+        <joint name="joint5" type="revolute">
+                <parent link="link4" />
+                <child link = "link5" />
+                <origin xyz="0.0 ${link4_length} ${joint_length}" rpy="0.0 0.0 0.0" />
+                <axis xyz="0 0 1" />
+                <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="2.0" velocity="10.0"/>
+        </joint>
+
+        <link name="link5">
+                <visual name="joint5">
+                        <origin xyz="0 0 0" rpy="0 0 0" />
+                        <geometry>
+                                <cylinder length="${joint_length}" radius="${joint_radius}"/>
+                        </geometry>
+                        <material name="Orange"/>
+                </visual>
+                <visual name="link">
+                        <origin xyz="0 0 ${link5_length / 2.0}" rpy="0 0 0" />
+                        <geometry>
+                                <cylinder length="${link5_length}" radius="${link_radius}"/>
+                        </geometry>
+                        <material name="Orange"/>
+                </visual>
+                <visual name="joint6">
+                        <origin xyz="0 0 ${link5_length}" rpy="${pi / 2.0} 0 0" />
+                        <geometry>
+                                <cylinder length="${joint_length}" radius="${joint_radius}"/>
+                        </geometry>
+                        <material name="Orange"/>
+                </visual>
+                <xacro:cylinder_inertial radius="${inertia_radius}" length="${link5_length / 2.0}" mass="${link_mass}">
+                        <origin xyz="0 0 ${link5_length / 2.0}" rpy="${pi / 2.0} 0 0" />
+                </xacro:cylinder_inertial>
+        </link>
+
+        <joint name="joint6" type="revolute">
+                <parent link="link5" />
+                <child link = "link6" />
+                <origin xyz="0.0 ${joint_length} ${link5_length}" rpy="0.0 0.0 0.0" />
+                <axis xyz="0 1 0" />
+                <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="2.0" velocity="10.0"/>
+        </joint>
+
+        <link name="link6">
+                <visual name="joint6">
+                        <origin xyz="0 0 0" rpy="${pi / 2.0} 0 0" />
+                        <geometry>
+                                <cylinder length="${joint_length}" radius="${joint_radius}"/>
+                        </geometry>
+                        <material name="Orange"/>
+                </visual>
+                <visual name="link">
+                        <origin xyz="0 ${link6_length / 2.0} 0" rpy="${-pi / 2.0} 0 0" />
+                        <geometry>
+                                <cylinder length="${link6_length}" radius="${link_radius}"/>
+                        </geometry>
+                        <material name="Orange"/>
+                </visual>
+                <xacro:cylinder_inertial radius="${inertia_radius}" length="${link6_length / 2.0}" mass="${link_mass}">
+                        <origin xyz="0 ${link6_length / 2.0} 0" rpy="${pi / 2.0} 0 0" />
+                </xacro:cylinder_inertial>
+        </link>
+
+        <!-- The default end-effector -->
+        <joint name="tool0_joint" type="fixed">
+                <origin xyz="0 0 ${link6_length / 2.0}" rpy="0 0 0"/>
+                <parent link="link6"/>
+                <child link="tool0"/>
+        </joint>
+        <link name="tool0">
+                <visual>
+                        <origin xyz="0 0 0" rpy="0 0 0" />
+                        <geometry>
+                                <sphere radius="${link_radius}"/>
+                        </geometry>
+                        <material name="Orange"/>
+                </visual>
+        </link>
+
+        <!-- Transmission and motors -->
+        <transmission name="joint1_trans">
+                <type>transmission_interface/SimpleTransmission</type>
+                <joint name="joint1">
+                        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+                </joint>
+                <actuator name="joint1_motor">
+                        <mechanicalReduction>1</mechanicalReduction>
+                </actuator>
+        </transmission>
+
+        <transmission name="joint2_trans">
+                <type>transmission_interface/SimpleTransmission</type>
+                <joint name="joint2">
+                        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+                </joint>
+                <actuator name="joint2_motor">
+                        <mechanicalReduction>1</mechanicalReduction>
+                </actuator>
+        </transmission>
+
+        <transmission name="joint3_trans">
+                <type>transmission_interface/SimpleTransmission</type>
+                <joint name="joint3">
+                        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+                </joint>
+                <actuator name="joint3_motor">
+                        <mechanicalReduction>1</mechanicalReduction>
+                </actuator>
+        </transmission>
+
+        <transmission name="joint4_trans">
+                <type>transmission_interface/SimpleTransmission</type>
+                <joint name="joint4">
+                        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+                </joint>
+                <actuator name="joint4_motor">
+                        <mechanicalReduction>1</mechanicalReduction>
+                </actuator>
+        </transmission>
+
+        <transmission name="joint5_trans">
+                <type>transmission_interface/SimpleTransmission</type>
+                <joint name="joint5">
+                        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+                </joint>
+                <actuator name="joint5_motor">
+                        <mechanicalReduction>1</mechanicalReduction>
+                </actuator>
+        </transmission>
+
+        <transmission name="joint6_trans">
+                <type>transmission_interface/SimpleTransmission</type>
+                <joint name="joint6">
+                        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+                </joint>
+                <actuator name="joint6_motor">
+                        <mechanicalReduction>1</mechanicalReduction>
+                </actuator>
+        </transmission>
+
+        <!-- Load Gazebo ROS Control, & Disable gravity -->
+        <gazebo>
+                <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so" />
+        </gazebo>
+        <gazebo reference="link1">
+                <turnGravityOff>true</turnGravityOff>
+        </gazebo>
+        <gazebo reference="link2">
+                <turnGravityOff>true</turnGravityOff>
+        </gazebo>
+        <gazebo reference="link3">
+                <turnGravityOff>true</turnGravityOff>
+        </gazebo>
+        <gazebo reference="link4">
+                <turnGravityOff>true</turnGravityOff>
+        </gazebo>
+        <gazebo reference="link5">
+                <turnGravityOff>true</turnGravityOff>
+        </gazebo>
+        <gazebo reference="link6">
+                <turnGravityOff>true</turnGravityOff>
+        </gazebo>
+</robot>