diff --git a/urdf/sciurus17.gazebo_ros2_control.xacro b/urdf/sciurus17.gazebo_ros2_control.xacro
index d5c19e2..42ff818 100644
--- a/urdf/sciurus17.gazebo_ros2_control.xacro
+++ b/urdf/sciurus17.gazebo_ros2_control.xacro
@@ -8,7 +8,7 @@
         <plugin>gz_ros2_control/GazeboSimSystem</plugin>
       </hardware>
 
-      <joint name="${NAME_JOINT_BODY}">
+      <joint name="waist_yaw_joint">
         <command_interface name="position">
           <param name="min">${JOINT_BODY_LOWER_LIMIT}</param>
           <param name="max">${JOINT_BODY_UPPER_LIMIT}</param>
@@ -20,7 +20,7 @@
         <state_interface name="effort"/>
       </joint>
 
-      <joint name="${NAME_JOINT_NECK_1}">
+      <joint name="neck_yaw_joint">
         <command_interface name="position">
           <param name="min">${JOINT_NECK_1_LOWER_LIMIT}</param>
           <param name="max">${JOINT_NECK_1_UPPER_LIMIT}</param>
@@ -32,7 +32,7 @@
         <state_interface name="effort"/>
       </joint>
 
-      <joint name="${NAME_JOINT_NECK_2}">
+      <joint name="neck_pitch_joint">
         <command_interface name="position">
           <param name="min">${JOINT_NECK_2_LOWER_LIMIT}</param>
           <param name="max">${JOINT_NECK_2_UPPER_LIMIT}</param>
@@ -44,7 +44,7 @@
         <state_interface name="effort"/>
       </joint>
 
-      <joint name="${NAME_JOINT_ARM_R_1}">
+      <joint name="r_arm_joint1">
         <command_interface name="position">
           <param name="min">${JOINT_ARM_R_1_LOWER_LIMIT}</param>
           <param name="max">${JOINT_ARM_R_1_UPPER_LIMIT}</param>
@@ -56,7 +56,7 @@
         <state_interface name="effort"/>
       </joint>
 
-      <joint name="${NAME_JOINT_ARM_R_2}">
+      <joint name="r_arm_joint2">
         <command_interface name="position">
           <param name="min">${JOINT_ARM_R_2_LOWER_LIMIT}</param>
           <param name="max">${JOINT_ARM_R_2_UPPER_LIMIT}</param>
@@ -68,7 +68,7 @@
         <state_interface name="effort"/>
       </joint>
 
-      <joint name="${NAME_JOINT_ARM_R_3}">
+      <joint name="r_arm_joint3">
         <command_interface name="position">
           <param name="min">${JOINT_ARM_R_3_LOWER_LIMIT}</param>
           <param name="max">${JOINT_ARM_R_3_UPPER_LIMIT}</param>
@@ -80,7 +80,7 @@
         <state_interface name="effort"/>
       </joint>
 
-      <joint name="${NAME_JOINT_ARM_R_4}">
+      <joint name="r_arm_joint4">
         <command_interface name="position">
           <param name="min">${JOINT_ARM_R_4_LOWER_LIMIT}</param>
           <param name="max">${JOINT_ARM_R_4_UPPER_LIMIT}</param>
@@ -92,7 +92,7 @@
         <state_interface name="effort"/>
       </joint>
 
-      <joint name="${NAME_JOINT_ARM_R_5}">
+      <joint name="r_arm_joint5">
         <command_interface name="position">
           <param name="min">${JOINT_ARM_R_5_LOWER_LIMIT}</param>
           <param name="max">${JOINT_ARM_R_5_UPPER_LIMIT}</param>
@@ -104,7 +104,7 @@
         <state_interface name="effort"/>
       </joint>
 
-      <joint name="${NAME_JOINT_ARM_R_6}">
+      <joint name="r_arm_joint6">
         <command_interface name="position">
           <param name="min">${JOINT_ARM_R_6_LOWER_LIMIT}</param>
           <param name="max">${JOINT_ARM_R_6_UPPER_LIMIT}</param>
@@ -116,7 +116,7 @@
         <state_interface name="effort"/>
       </joint>
 
-      <joint name="${NAME_JOINT_ARM_R_7}">
+      <joint name="r_arm_joint7">
         <command_interface name="position">
           <param name="min">${JOINT_ARM_R_7_LOWER_LIMIT}</param>
           <param name="max">${JOINT_ARM_R_7_UPPER_LIMIT}</param>
@@ -128,7 +128,7 @@
         <state_interface name="effort"/>
       </joint>
 
-      <joint name="${NAME_JOINT_GRIPPER_R_A}">
+      <joint name="r_gripper_joint">
         <command_interface name="position">
           <param name="min">${JOINT_GRIPPER_R_LOWER_LIMIT}</param>
           <param name="max">${JOINT_GRIPPER_R_UPPER_LIMIT}</param>
@@ -140,15 +140,15 @@
         <state_interface name="effort"/>
       </joint>
 
-      <joint name="${NAME_JOINT_GRIPPER_R_B}">
-        <param name="mimic">${NAME_JOINT_GRIPPER_R_A}</param>
+      <joint name="r_gripper_mimic_joint">
+        <param name="mimic">r_gripper_joint</param>
         <param name="multiplier">1</param>
         <state_interface name="position"/>
         <state_interface name="velocity"/>
         <state_interface name="effort"/>
       </joint>
 
-      <joint name="${NAME_JOINT_ARM_L_1}">
+      <joint name="l_arm_joint1">
         <command_interface name="position">
           <param name="min">${JOINT_ARM_L_1_LOWER_LIMIT}</param>
           <param name="max">${JOINT_ARM_L_1_UPPER_LIMIT}</param>
@@ -160,7 +160,7 @@
         <state_interface name="effort"/>
       </joint>
 
-      <joint name="${NAME_JOINT_ARM_L_2}">
+      <joint name="l_arm_joint2">
         <command_interface name="position">
           <param name="min">${JOINT_ARM_L_2_LOWER_LIMIT}</param>
           <param name="max">${JOINT_ARM_L_2_UPPER_LIMIT}</param>
@@ -172,7 +172,7 @@
         <state_interface name="effort"/>
       </joint>
 
-      <joint name="${NAME_JOINT_ARM_L_3}">
+      <joint name="l_arm_joint3">
         <command_interface name="position">
           <param name="min">${JOINT_ARM_L_3_LOWER_LIMIT}</param>
           <param name="max">${JOINT_ARM_L_3_UPPER_LIMIT}</param>
@@ -184,7 +184,7 @@
         <state_interface name="effort"/>
       </joint>
 
-      <joint name="${NAME_JOINT_ARM_L_4}">
+      <joint name="l_arm_joint4">
         <command_interface name="position">
           <param name="min">${JOINT_ARM_L_4_LOWER_LIMIT}</param>
           <param name="max">${JOINT_ARM_L_4_UPPER_LIMIT}</param>
@@ -196,7 +196,7 @@
         <state_interface name="effort"/>
       </joint>
 
-      <joint name="${NAME_JOINT_ARM_L_5}">
+      <joint name="l_arm_joint5">
         <command_interface name="position">
           <param name="min">${JOINT_ARM_L_5_LOWER_LIMIT}</param>
           <param name="max">${JOINT_ARM_L_5_UPPER_LIMIT}</param>
@@ -208,7 +208,7 @@
         <state_interface name="effort"/>
       </joint>
 
-      <joint name="${NAME_JOINT_ARM_L_6}">
+      <joint name="l_arm_joint6">
         <command_interface name="position">
           <param name="min">${JOINT_ARM_L_6_LOWER_LIMIT}</param>
           <param name="max">${JOINT_ARM_L_6_UPPER_LIMIT}</param>
@@ -220,7 +220,7 @@
         <state_interface name="effort"/>
       </joint>
 
-      <joint name="${NAME_JOINT_ARM_L_7}">
+      <joint name="l_arm_joint7">
         <command_interface name="position">
           <param name="min">${JOINT_ARM_L_7_LOWER_LIMIT}</param>
           <param name="max">${JOINT_ARM_L_7_UPPER_LIMIT}</param>
@@ -232,7 +232,7 @@
         <state_interface name="effort"/>
       </joint>
 
-      <joint name="${NAME_JOINT_GRIPPER_L_A}">
+      <joint name="l_gripper_joint">
         <command_interface name="position">
           <param name="min">${JOINT_GRIPPER_L_LOWER_LIMIT}</param>
           <param name="max">${JOINT_GRIPPER_L_UPPER_LIMIT}</param>
@@ -244,8 +244,8 @@
         <state_interface name="effort"/>
       </joint>
 
-      <joint name="${NAME_JOINT_GRIPPER_L_B}">
-        <param name="mimic">${NAME_JOINT_GRIPPER_L_A}</param>
+      <joint name="l_gripper_mimic_joint">
+        <param name="mimic">l_gripper_joint</param>
         <param name="multiplier">1</param>
         <state_interface name="position"/>
         <state_interface name="velocity"/>
diff --git a/urdf/sciurus17.ros2_control.xacro b/urdf/sciurus17.ros2_control.xacro
index bf68e23..3a584ab 100644
--- a/urdf/sciurus17.ros2_control.xacro
+++ b/urdf/sciurus17.ros2_control.xacro
@@ -1,23 +1,28 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-  <xacro:macro name="sciurus17_ros2_control_settings">
+  <xacro:macro name="sciurus17_ros2_control_settings"
+               params="use_mock_components:=false
+                       port_name:=/dev/sciurus17spine
+                       baudrate:=3000000
+                       timeout_seconds:=1.0
+                       manipulator_config_file_path:=''">
 
     <ros2_control name="sciurus17" type="system">
       <hardware>
-        <xacro:if value="${USE_MOCK_COMPONENTS}">
+        <xacro:if value="${use_mock_components}">
           <plugin>mock_components/GenericSystem</plugin>
         </xacro:if>
-        <xacro:unless value="${USE_MOCK_COMPONENTS}">
+        <xacro:unless value="${use_mock_components}">
           <plugin>sciurus17_hardware/Sciurus17Hardware</plugin>
-          <param name="port_name">${PORT_NAME}</param>
-          <param name="baudrate">${BAUDRATE}</param>
-          <param name="timeout_seconds">${TIMEOUT_SECONDS}</param>
-          <param name="manipulator_config_file_path">${MANIPULATOR_CONFIG_FILE_PATH}</param>
+          <param name="port_name">${port_name}</param>
+          <param name="baudrate">${baudrate}</param>
+          <param name="timeout_seconds">${timeout_seconds}</param>
+          <param name="manipulator_config_file_path">${manipulator_config_file_path}</param>
         </xacro:unless>
       </hardware>
 
-      <joint name="${NAME_JOINT_BODY}">
+      <joint name="waist_yaw_joint">
         <command_interface name="position">
           <param name="min">${JOINT_BODY_LOWER_LIMIT}</param>
           <param name="max">${JOINT_BODY_UPPER_LIMIT}</param>
@@ -28,7 +33,7 @@
         <param name="current_to_effort">3.60</param>
       </joint>
 
-      <joint name="${NAME_JOINT_NECK_1}">
+      <joint name="neck_yaw_joint">
         <command_interface name="position">
           <param name="min">${JOINT_NECK_1_LOWER_LIMIT}</param>
           <param name="max">${JOINT_NECK_1_UPPER_LIMIT}</param>
@@ -39,7 +44,7 @@
         <param name="current_to_effort">2.20</param>
       </joint>
 
-      <joint name="${NAME_JOINT_NECK_2}">
+      <joint name="neck_pitch_joint">
         <command_interface name="position">
           <param name="min">${JOINT_NECK_2_LOWER_LIMIT}</param>
           <param name="max">${JOINT_NECK_2_UPPER_LIMIT}</param>
@@ -50,7 +55,7 @@
         <param name="current_to_effort">2.20</param>
       </joint>
 
-      <joint name="${NAME_JOINT_ARM_R_1}">
+      <joint name="r_arm_joint1">
         <command_interface name="position">
           <param name="min">${JOINT_ARM_R_1_LOWER_LIMIT}</param>
           <param name="max">${JOINT_ARM_R_1_UPPER_LIMIT}</param>
@@ -61,7 +66,7 @@
         <param name="current_to_effort">3.60</param>
       </joint>
 
-      <joint name="${NAME_JOINT_ARM_R_2}">
+      <joint name="r_arm_joint2">
         <command_interface name="position">
           <param name="min">${JOINT_ARM_R_2_LOWER_LIMIT}</param>
           <param name="max">${JOINT_ARM_R_2_UPPER_LIMIT}</param>
@@ -72,7 +77,7 @@
         <param name="current_to_effort">3.60</param>
       </joint>
 
-      <joint name="${NAME_JOINT_ARM_R_3}">
+      <joint name="r_arm_joint3">
         <command_interface name="position">
           <param name="min">${JOINT_ARM_R_3_LOWER_LIMIT}</param>
           <param name="max">${JOINT_ARM_R_3_UPPER_LIMIT}</param>
@@ -83,7 +88,7 @@
         <param name="current_to_effort">2.20</param>
       </joint>
 
-      <joint name="${NAME_JOINT_ARM_R_4}">
+      <joint name="r_arm_joint4">
         <command_interface name="position">
           <param name="min">${JOINT_ARM_R_4_LOWER_LIMIT}</param>
           <param name="max">${JOINT_ARM_R_4_UPPER_LIMIT}</param>
@@ -94,7 +99,7 @@
         <param name="current_to_effort">2.00</param>
       </joint>
 
-      <joint name="${NAME_JOINT_ARM_R_5}">
+      <joint name="r_arm_joint5">
         <command_interface name="position">
           <param name="min">${JOINT_ARM_R_5_LOWER_LIMIT}</param>
           <param name="max">${JOINT_ARM_R_5_UPPER_LIMIT}</param>
@@ -105,7 +110,7 @@
         <param name="current_to_effort">2.20</param>
       </joint>
 
-      <joint name="${NAME_JOINT_ARM_R_6}">
+      <joint name="r_arm_joint6">
         <command_interface name="position">
           <param name="min">${JOINT_ARM_R_6_LOWER_LIMIT}</param>
           <param name="max">${JOINT_ARM_R_6_UPPER_LIMIT}</param>
@@ -116,7 +121,7 @@
         <param name="current_to_effort">2.20</param>
       </joint>
 
-      <joint name="${NAME_JOINT_ARM_R_7}">
+      <joint name="r_arm_joint7">
         <command_interface name="position">
           <param name="min">${JOINT_ARM_R_7_LOWER_LIMIT}</param>
           <param name="max">${JOINT_ARM_R_7_UPPER_LIMIT}</param>
@@ -127,7 +132,7 @@
         <param name="current_to_effort">2.20</param>
       </joint>
 
-      <joint name="${NAME_JOINT_GRIPPER_R_A}">
+      <joint name="r_gripper_joint">
         <command_interface name="position">
           <param name="min">${JOINT_GRIPPER_R_LOWER_LIMIT}</param>
           <param name="max">${JOINT_GRIPPER_R_UPPER_LIMIT}</param>
@@ -138,7 +143,7 @@
         <param name="current_to_effort">2.20</param>
       </joint>
 
-      <joint name="${NAME_JOINT_ARM_L_1}">
+      <joint name="l_arm_joint1">
         <command_interface name="position">
           <param name="min">${JOINT_ARM_L_1_LOWER_LIMIT}</param>
           <param name="max">${JOINT_ARM_L_1_UPPER_LIMIT}</param>
@@ -149,7 +154,7 @@
         <param name="current_to_effort">3.60</param>
       </joint>
 
-      <joint name="${NAME_JOINT_ARM_L_2}">
+      <joint name="l_arm_joint2">
         <command_interface name="position">
           <param name="min">${JOINT_ARM_L_2_LOWER_LIMIT}</param>
           <param name="max">${JOINT_ARM_L_2_UPPER_LIMIT}</param>
@@ -160,7 +165,7 @@
         <param name="current_to_effort">3.60</param>
       </joint>
 
-      <joint name="${NAME_JOINT_ARM_L_3}">
+      <joint name="l_arm_joint3">
         <command_interface name="position">
           <param name="min">${JOINT_ARM_L_3_LOWER_LIMIT}</param>
           <param name="max">${JOINT_ARM_L_3_UPPER_LIMIT}</param>
@@ -171,7 +176,7 @@
         <param name="current_to_effort">2.20</param>
       </joint>
 
-      <joint name="${NAME_JOINT_ARM_L_4}">
+      <joint name="l_arm_joint4">
         <command_interface name="position">
           <param name="min">${JOINT_ARM_L_4_LOWER_LIMIT}</param>
           <param name="max">${JOINT_ARM_L_4_UPPER_LIMIT}</param>
@@ -182,7 +187,7 @@
         <param name="current_to_effort">2.00</param>
       </joint>
 
-      <joint name="${NAME_JOINT_ARM_L_5}">
+      <joint name="l_arm_joint5">
         <command_interface name="position">
           <param name="min">${JOINT_ARM_L_5_LOWER_LIMIT}</param>
           <param name="max">${JOINT_ARM_L_5_UPPER_LIMIT}</param>
@@ -193,7 +198,7 @@
         <param name="current_to_effort">2.20</param>
       </joint>
 
-      <joint name="${NAME_JOINT_ARM_L_6}">
+      <joint name="l_arm_joint6">
         <command_interface name="position">
           <param name="min">${JOINT_ARM_L_6_LOWER_LIMIT}</param>
           <param name="max">${JOINT_ARM_L_6_UPPER_LIMIT}</param>
@@ -204,7 +209,7 @@
         <param name="current_to_effort">2.20</param>
       </joint>
 
-      <joint name="${NAME_JOINT_ARM_L_7}">
+      <joint name="l_arm_joint7">
         <command_interface name="position">
           <param name="min">${JOINT_ARM_L_7_LOWER_LIMIT}</param>
           <param name="max">${JOINT_ARM_L_7_UPPER_LIMIT}</param>
@@ -215,7 +220,7 @@
         <param name="current_to_effort">2.20</param>
       </joint>
 
-      <joint name="${NAME_JOINT_GRIPPER_L_A}">
+      <joint name="l_gripper_joint">
         <command_interface name="position">
           <param name="min">${JOINT_GRIPPER_L_LOWER_LIMIT}</param>
           <param name="max">${JOINT_GRIPPER_L_UPPER_LIMIT}</param>
diff --git a/urdf/sciurus17.urdf.xacro b/urdf/sciurus17.urdf.xacro
index 73945ba..5f0fe19 100644
--- a/urdf/sciurus17.urdf.xacro
+++ b/urdf/sciurus17.urdf.xacro
@@ -27,18 +27,6 @@
   <xacro:arg name="manipulator_config_file_path" default="" />
   <xacro:arg name="gz_control_config_package" default="" />
   <xacro:arg name="gz_control_config_file_path" default="" />
-  <xacro:property name="USE_GAZEBO_HEAD_CAMERA" value="$(arg use_gazebo_head_camera)"/>
-  <xacro:property name="USE_GAZEBO_CHEST_CAMERA" value="$(arg use_gazebo_chest_camera)"/>
-  <xacro:property name="USE_MOCK_COMPONENTS" value="$(arg use_mock_components)"/>
-  <xacro:property name="PORT_NAME" value="$(arg port_name)"/>
-  <xacro:property name="BAUDRATE" value="$(arg baudrate)"/>
-  <xacro:property name="TIMEOUT_SECONDS" value="$(arg timeout_seconds)"/>
-  <xacro:property name="MANIPULATOR_CONFIG_FILE_PATH" value="$(arg manipulator_config_file_path)"/>
-  <xacro:property name="GZ_CONTROL_CONFIG_PACKAGE" value="$(arg gz_control_config_package)"/>
-  <xacro:property name="GZ_CONTROL_CONFIG_FILE_PATH" value="$(arg gz_control_config_file_path)"/>
-
-  <xacro:property name="NAME_HEAD_CAMERA_PREFIX" value="head_camera"/>
-  <xacro:property name="NAME_CHEST_CAMERA_PREFIX" value="chest_camera"/>
 
   <material name="white">
     <color rgba="0.9 0.9 0.9 1.0"/>
@@ -48,68 +36,6 @@
     <color rgba="0.8984375 0.0 0.0703125 1"/>
   </material>
 
-  <xacro:property name="NAME_LINK_BASE" value="base_link"/>
-  <xacro:property name="NAME_LINK_BODY" value="body_link"/>
-
-  <xacro:property name="NAME_LINK_NECK_1" value="neck_yaw_link"/>
-  <xacro:property name="NAME_LINK_NECK_2" value="neck_pitch_link"/>
-
-  <xacro:property name="NAME_LINK_HEAD_CAMERA" value="${NAME_HEAD_CAMERA_PREFIX}_link"/>
-  <xacro:property name="NAME_LINK_CHEST_CAMERA" value="${NAME_CHEST_CAMERA_PREFIX}_link"/>
-
-  <xacro:property name="NAME_LINK_ARM_R_1" value="r_link1"/>
-  <xacro:property name="NAME_LINK_ARM_R_2" value="r_link2"/>
-  <xacro:property name="NAME_LINK_ARM_R_3" value="r_link3"/>
-  <xacro:property name="NAME_LINK_ARM_R_4" value="r_link4"/>
-  <xacro:property name="NAME_LINK_ARM_R_5" value="r_link5"/>
-  <xacro:property name="NAME_LINK_ARM_R_6" value="r_link6"/>
-  <xacro:property name="NAME_LINK_ARM_R_7" value="r_link7"/>
-  <xacro:property name="NAME_LINK_GRIPPER_R_A" value="r_gripperA_link"/>
-  <xacro:property name="NAME_LINK_GRIPPER_R_B" value="r_gripperB_link"/>
-  <xacro:property name="NAME_LINK_ARMARKER_R" value="r_link5_armarker"/>
-
-  <xacro:property name="NAME_LINK_ARM_L_1" value="l_link1"/>
-  <xacro:property name="NAME_LINK_ARM_L_2" value="l_link2"/>
-  <xacro:property name="NAME_LINK_ARM_L_3" value="l_link3"/>
-  <xacro:property name="NAME_LINK_ARM_L_4" value="l_link4"/>
-  <xacro:property name="NAME_LINK_ARM_L_5" value="l_link5"/>
-  <xacro:property name="NAME_LINK_ARM_L_6" value="l_link6"/>
-  <xacro:property name="NAME_LINK_ARM_L_7" value="l_link7"/>
-  <xacro:property name="NAME_LINK_GRIPPER_L_A" value="l_gripperA_link"/>
-  <xacro:property name="NAME_LINK_GRIPPER_L_B" value="l_gripperB_link"/>
-  <xacro:property name="NAME_LINK_ARMARKER_L" value="l_link5_armarker"/>
-
-  <xacro:property name="NAME_JOINT_BASE" value="base_link_to_world_joint"/>
-  <xacro:property name="NAME_JOINT_BODY" value="waist_yaw_joint"/>
-
-  <xacro:property name="NAME_JOINT_NECK_1" value="neck_yaw_joint"/>
-  <xacro:property name="NAME_JOINT_NECK_2" value="neck_pitch_joint"/>
-
-  <xacro:property name="NAME_JOINT_HEAD_CAMERA" value="head_camera_joint"/>
-  <xacro:property name="NAME_JOINT_CHEST_CAMERA" value="chest_camera_joint"/>
-
-  <xacro:property name="NAME_JOINT_ARM_R_1" value="r_arm_joint1"/>
-  <xacro:property name="NAME_JOINT_ARM_R_2" value="r_arm_joint2"/>
-  <xacro:property name="NAME_JOINT_ARM_R_3" value="r_arm_joint3"/>
-  <xacro:property name="NAME_JOINT_ARM_R_4" value="r_arm_joint4"/>
-  <xacro:property name="NAME_JOINT_ARM_R_5" value="r_arm_joint5"/>
-  <xacro:property name="NAME_JOINT_ARM_R_6" value="r_arm_joint6"/>
-  <xacro:property name="NAME_JOINT_ARM_R_7" value="r_arm_joint7"/>
-  <xacro:property name="NAME_JOINT_GRIPPER_R_A" value="r_gripper_joint"/>
-  <xacro:property name="NAME_JOINT_GRIPPER_R_B" value="r_gripper_mimic_joint"/>
-  <xacro:property name="NAME_JOINT_ARMARKER_R" value="r_arm_armarker"/>
-
-  <xacro:property name="NAME_JOINT_ARM_L_1" value="l_arm_joint1"/>
-  <xacro:property name="NAME_JOINT_ARM_L_2" value="l_arm_joint2"/>
-  <xacro:property name="NAME_JOINT_ARM_L_3" value="l_arm_joint3"/>
-  <xacro:property name="NAME_JOINT_ARM_L_4" value="l_arm_joint4"/>
-  <xacro:property name="NAME_JOINT_ARM_L_5" value="l_arm_joint5"/>
-  <xacro:property name="NAME_JOINT_ARM_L_6" value="l_arm_joint6"/>
-  <xacro:property name="NAME_JOINT_ARM_L_7" value="l_arm_joint7"/>
-  <xacro:property name="NAME_JOINT_GRIPPER_L_A" value="l_gripper_joint"/>
-  <xacro:property name="NAME_JOINT_GRIPPER_L_B" value="l_gripper_mimic_joint"/>
-  <xacro:property name="NAME_JOINT_ARMARKER_L" value="l_arm_armarker"/>
-
   <xacro:property name="EFFORT_LIMIT" value="4.0"/>
   <xacro:property name="VELOCITY_LIMIT" value="5.969211435"/>
 
@@ -167,18 +93,13 @@
   <!-- 参照: https://github.com/rt-net/sciurus17_description/pull/6 -->
   <xacro:property name="JOINT_GRIPPER_L_UPPER_LIMIT" value="${radians(0.05)}"/>
 
-  <xacro:property name="COLOR_LINK_BASE" value="white"/>
-  <xacro:property name="COLOR_LINK_BODY" value="white"/>
-  <xacro:property name="COLOR_LINK_HEAD" value="white"/>
-  <xacro:property name="COLOR_LINK_ARM" value="white"/>
-  <xacro:property name="COLOR_LINK_GRIPPER" value="red"/>
 
   <!-- Used for fixing robot 'base_link'  to Gazebo 'world' -->
   <link name="world"/>
 
-  <joint name="${NAME_JOINT_BASE}" type="fixed">
+  <joint name="base_link_to_world_joint" type="fixed">
     <parent link="world"/>
-    <child link="${NAME_LINK_BASE}"/>
+    <child link="base_link"/>
   </joint>
 
   <xacro:sciurus17_body/>
@@ -195,31 +116,40 @@
 
   <xacro:unless value="$(arg use_gazebo)">
 
-    <xacro:sciurus17_ros2_control_settings/>
+    <xacro:sciurus17_ros2_control_settings
+      use_mock_components="$(arg use_mock_components)"
+      port_name="$(arg port_name)"
+      baudrate="$(arg baudrate)"
+      timeout_seconds="$(arg timeout_seconds)"
+      manipulator_config_file_path="$(arg manipulator_config_file_path)"/>
 
   </xacro:unless>
 
   <xacro:if value="$(arg use_gazebo)">
 
-    <xacro:gazebo_robot_settings/>
+    <xacro:gazebo_robot_settings
+      use_gazebo_head_camera="$(arg use_gazebo_head_camera)"
+      use_gazebo_chest_camera="$(arg use_gazebo_chest_camera)"
+      gz_control_config_package="$(arg gz_control_config_package)"
+      gz_control_config_file_path="$(arg gz_control_config_file_path)"/>
 
     <xacro:sciurus17_gazebo_ros2_control_settings/>
 
-    <joint name="${NAME_HEAD_CAMERA_PREFIX}_color_joint" type="fixed">
+    <joint name="head_camera_color_joint" type="fixed">
       <origin xyz="0 0.015 0" rpy="0 0 0" />
-      <parent link="${NAME_LINK_HEAD_CAMERA}" />
-      <child link="${NAME_HEAD_CAMERA_PREFIX}_color_frame" />
+      <parent link="head_camera_link" />
+      <child link="head_camera_color_frame" />
     </joint>
 
-    <link name="${NAME_HEAD_CAMERA_PREFIX}_color_frame"/>
+    <link name="head_camera_color_frame"/>
 
-    <joint name="${NAME_HEAD_CAMERA_PREFIX}_color_optical_joint" type="fixed">
+    <joint name="head_camera_color_optical_joint" type="fixed">
       <origin xyz="0 0 0" rpy="${radians(-90)} 0 ${radians(-90)}" />
-      <parent link="${NAME_HEAD_CAMERA_PREFIX}_color_frame" />
-      <child link="${NAME_HEAD_CAMERA_PREFIX}_color_optical_frame" />
+      <parent link="head_camera_color_frame" />
+      <child link="head_camera_color_optical_frame" />
     </joint>
 
-    <link name="${NAME_HEAD_CAMERA_PREFIX}_color_optical_frame"/>
+    <link name="head_camera_color_optical_frame"/>
 
   </xacro:if>
 </robot>
diff --git a/urdf/sciurus17_body.xacro b/urdf/sciurus17_body.xacro
index 49011a2..6cc7577 100644
--- a/urdf/sciurus17_body.xacro
+++ b/urdf/sciurus17_body.xacro
@@ -4,12 +4,12 @@
 
   <xacro:macro name="sciurus17_body">
 
-    <link name="${NAME_LINK_BASE}">
+    <link name="base_link">
       <visual>
         <geometry>
           <mesh filename="package://sciurus17_description/meshes/visual/Base.stl" scale="1.0 1.0 1.0"/>
         </geometry>
-        <material name="${COLOR_LINK_BASE}"/>
+        <material name="white"/>
       </visual>
       <collision>
         <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
@@ -24,9 +24,9 @@
       </inertial>
     </link>
 
-    <joint name="${NAME_JOINT_BODY}" type="revolute">
-      <parent link="${NAME_LINK_BASE}" />
-      <child link="${NAME_LINK_BODY}" />
+    <joint name="waist_yaw_joint" type="revolute">
+      <parent link="base_link" />
+      <child link="body_link" />
       <origin xyz="0.0 0.0 0.1315" rpy="0 0 0"/>
       <limit 
         lower="${JOINT_BODY_LOWER_LIMIT}" 
@@ -37,12 +37,12 @@
       <dynamics damping="1.0e-6" friction="0.8"/>
     </joint>
 
-    <link name="${NAME_LINK_BODY}">
+    <link name="body_link">
       <visual>
         <geometry>
           <mesh filename="package://sciurus17_description/meshes/visual/Body.stl" scale="1.0 1.0 1.0"/>
         </geometry>
-        <material name="${COLOR_LINK_BODY}"/>
+        <material name="white"/>
       </visual>
       <collision>
         <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
@@ -57,13 +57,13 @@
       </inertial>
     </link>
 
-    <joint name="${NAME_JOINT_CHEST_CAMERA}" type="fixed">
-      <parent link="${NAME_LINK_BODY}" />
-      <child link="${NAME_LINK_CHEST_CAMERA}" />
+    <joint name="chest_camera_joint" type="fixed">
+      <parent link="body_link" />
+      <child link="chest_camera_link" />
       <origin xyz="0.114576 0.0 0.217659" rpy="-2.7925268032 0 -1.5707963268"/>
     </joint>
 
-    <link name="${NAME_LINK_CHEST_CAMERA}"/>
+    <link name="chest_camera_link"/>
 
   </xacro:macro>
 
diff --git a/urdf/sciurus17_gazebo.xacro b/urdf/sciurus17_gazebo.xacro
index 5bbecfe..4acdca6 100644
--- a/urdf/sciurus17_gazebo.xacro
+++ b/urdf/sciurus17_gazebo.xacro
@@ -29,26 +29,30 @@
     </visual>
   </xacro:macro>
 
-  <xacro:macro name="gazebo_robot_settings">
+  <xacro:macro name="gazebo_robot_settings"
+               params="use_gazebo_head_camera:=false
+               use_gazebo_chest_camera:=false
+               gz_control_config_package:=''
+               gz_control_config_file_path:=''">
     <gazebo>
       <plugin filename="libgz_ros2_control-system.so" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
-        <parameters>$(find ${GZ_CONTROL_CONFIG_PACKAGE})/${GZ_CONTROL_CONFIG_FILE_PATH}</parameters>
+        <parameters>$(find ${gz_control_config_package})/${gz_control_config_file_path}</parameters>
       </plugin>
     </gazebo>
 
-    <gazebo reference="${NAME_LINK_BASE}">
+    <gazebo reference="base_link">
       <xacro:material_gazebo_white/>
     </gazebo>
 
-    <gazebo reference="${NAME_LINK_BODY}">
+    <gazebo reference="body_link">
       <xacro:material_gazebo_white/>
     </gazebo>
 
-    <gazebo reference="${NAME_LINK_NECK_1}">
+    <gazebo reference="neck_yaw_link">
       <xacro:material_gazebo_white/>
     </gazebo>
 
-    <gazebo reference="${NAME_LINK_NECK_2}">
+    <gazebo reference="neck_pitch_link">
       <xacro:material_gazebo_white/>
     </gazebo>
 
@@ -58,13 +62,13 @@
       </plugin>
     </gazebo>
 
-    <xacro:if value="${USE_GAZEBO_HEAD_CAMERA}">
-      <gazebo reference="${NAME_HEAD_CAMERA_PREFIX}_color_frame">
+    <xacro:if value="${use_gazebo_head_camera}">
+      <gazebo reference="head_camera_color_frame">
         <sensor name="sim_realsense_d415" type="rgbd_camera">
           <update_rate>10</update_rate>
           <always_on>true</always_on>
-          <topic>${NAME_HEAD_CAMERA_PREFIX}</topic>
-          <frame_id>${NAME_HEAD_CAMERA_PREFIX}_color_frame</frame_id>
+          <topic>head_camera</topic>
+          <frame_id>head_camera_color_frame</frame_id>
           <camera name="realsense_d415">
             <xacro:property name="head_camera_width" value="640"/>
             <xacro:property name="head_camera_height" value="360"/>
@@ -85,7 +89,7 @@
                 <far>3.0</far>
               </clip>
             </depth_camera>
-            <optical_frame_id>${NAME_HEAD_CAMERA_PREFIX}_color_optical_frame</optical_frame_id>
+            <optical_frame_id>head_camera_color_optical_frame</optical_frame_id>
             <lens>
               <intrinsics>
                 <fx>${head_camera_focal_length}</fx>
@@ -108,15 +112,15 @@
       </gazebo>
     </xacro:if>
 
-    <xacro:if value="${USE_GAZEBO_CHEST_CAMERA}">
-      <gazebo reference="${NAME_LINK_CHEST_CAMERA}">
-        <sensor name="${NAME_CHEST_CAMERA_PREFIX}" type="camera">
-          <pose relative_to="${NAME_LINK_CHEST_CAMERA}">0 0 0 ${radians(90)} ${radians(-90)} 0</pose>
+    <xacro:if value="${use_gazebo_chest_camera}">
+      <gazebo reference="chest_camera_link">
+        <sensor name="chest_camera" type="camera">
+          <pose relative_to="chest_camera_link">0 0 0 ${radians(90)} ${radians(-90)} 0</pose>
           <update_rate>10</update_rate>
           <always_on>true</always_on>
-          <topic>${NAME_CHEST_CAMERA_PREFIX}/image_raw</topic>
-          <frame_id>${NAME_LINK_CHEST_CAMERA}</frame_id>
-          <camera name="${NAME_CHEST_CAMERA_PREFIX}">
+          <topic>chest_camera/image_raw</topic>
+          <frame_id>chest_camera_link</frame_id>
+          <camera name="chest_camera">
             <xacro:property name="chest_camera_width" value="640"/>
             <xacro:property name="chest_camera_height" value="360"/>
             <xacro:property name="chest_camera_hfov" value="${radians(120)}"/>
@@ -152,81 +156,81 @@
       </gazebo>
     </xacro:if>
 
-    <gazebo reference="${NAME_LINK_ARM_R_1}">
+    <gazebo reference="r_link1">
       <xacro:material_gazebo_white/>
     </gazebo>
 
-    <gazebo reference="${NAME_LINK_ARM_R_2}">
+    <gazebo reference="r_link2">
       <xacro:material_gazebo_white/>
     </gazebo>
 
-    <gazebo reference="${NAME_LINK_ARM_R_3}">
+    <gazebo reference="r_link3">
       <xacro:material_gazebo_white/>
     </gazebo>
 
-    <gazebo reference="${NAME_LINK_ARM_R_4}">
+    <gazebo reference="r_link4">
       <xacro:material_gazebo_white/>
     </gazebo>
 
-    <gazebo reference="${NAME_LINK_ARM_R_5}">
+    <gazebo reference="r_link5">
       <xacro:material_gazebo_white/>
     </gazebo>
 
-    <gazebo reference="${NAME_LINK_ARM_R_6}">
+    <gazebo reference="r_link6">
       <xacro:material_gazebo_white/>
     </gazebo>
 
-    <gazebo reference="${NAME_LINK_ARM_R_7}">
+    <gazebo reference="r_link7">
       <xacro:material_gazebo_red/>
     </gazebo>
 
-    <gazebo reference="${NAME_LINK_GRIPPER_R_A}">
+    <gazebo reference="r_gripperA_link">
       <mu1>0.8</mu1>
       <mu2>0.8</mu2>
       <xacro:material_gazebo_red/>
     </gazebo>
 
-    <gazebo reference="${NAME_LINK_GRIPPER_R_B}">
+    <gazebo reference="r_gripperB_link">
       <mu1>0.8</mu1>
       <mu2>0.8</mu2>
       <xacro:material_gazebo_red/>
     </gazebo>
 
-    <gazebo reference="${NAME_LINK_ARM_L_1}">
+    <gazebo reference="l_link1">
       <xacro:material_gazebo_white/>
     </gazebo>
 
-    <gazebo reference="${NAME_LINK_ARM_L_2}">
+    <gazebo reference="l_link2">
       <xacro:material_gazebo_white/>
     </gazebo>
 
-    <gazebo reference="${NAME_LINK_ARM_L_3}">
+    <gazebo reference="l_link3">
       <xacro:material_gazebo_white/>
     </gazebo>
 
-    <gazebo reference="${NAME_LINK_ARM_L_4}">
+    <gazebo reference="l_link4">
       <xacro:material_gazebo_white/>
     </gazebo>
 
-    <gazebo reference="${NAME_LINK_ARM_L_5}">
+    <gazebo reference="l_link5">
       <xacro:material_gazebo_white/>
     </gazebo>
 
-    <gazebo reference="${NAME_LINK_ARM_L_6}">
+    <gazebo reference="l_link6">
       <xacro:material_gazebo_white/>
     </gazebo>
 
-    <gazebo reference="${NAME_LINK_ARM_L_7}">
+    <gazebo reference="l_link7">
       <xacro:material_gazebo_red/>
     </gazebo>
 
-    <gazebo reference="${NAME_LINK_GRIPPER_L_A}">
+    <gazebo reference="l_gripperA_link">
       <mu1>0.8</mu1>
       <mu2>0.8</mu2>
       <xacro:material_gazebo_red/>
     </gazebo>
 
-    <gazebo reference="${NAME_LINK_GRIPPER_L_B}">
+    <gazebo reference="l_gripperB_link">
       <mu1>0.8</mu1>
       <mu2>0.8</mu2>
       <xacro:material_gazebo_red/>
diff --git a/urdf/sciurus17_head.xacro b/urdf/sciurus17_head.xacro
index 228689d..c6c50d8 100644
--- a/urdf/sciurus17_head.xacro
+++ b/urdf/sciurus17_head.xacro
@@ -4,9 +4,9 @@
 
   <xacro:macro name="sciurus17_head">
 
-    <joint name="${NAME_JOINT_NECK_1}" type="revolute">
-      <parent link="${NAME_LINK_BODY}" />
-      <child link="${NAME_LINK_NECK_1}" />
+    <joint name="neck_yaw_joint" type="revolute">
+      <parent link="body_link" />
+      <child link="neck_yaw_link" />
       <origin xyz="0.08083 0.0 0.3395" rpy="0 0 0"/>
       <limit 
         lower="${JOINT_NECK_1_LOWER_LIMIT}" 
@@ -17,12 +17,12 @@
       <dynamics damping="1.0e-6" friction="0.8"/>
     </joint>
 
-    <link name="${NAME_LINK_NECK_1}">
+    <link name="neck_yaw_link">
       <visual>
         <geometry>
           <mesh filename="package://sciurus17_description/meshes/visual/Neck1.stl" scale="1.0 1.0 1.0"/>
         </geometry>
-        <material name="${COLOR_LINK_HEAD}"/>
+        <material name="white"/>
       </visual>
       <collision>
         <geometry>
@@ -36,9 +36,9 @@
       </inertial>
     </link>
 
-    <joint name="${NAME_JOINT_NECK_2}" type="revolute">
-      <parent link="${NAME_LINK_NECK_1}" />
-      <child link="${NAME_LINK_NECK_2}" />
+    <joint name="neck_pitch_joint" type="revolute">
+      <parent link="neck_yaw_link" />
+      <child link="neck_pitch_link" />
       <origin xyz="0.0 0.0 0.054" rpy="0 0 0"/>
       <limit 
         lower="${JOINT_NECK_2_LOWER_LIMIT}" 
@@ -49,13 +49,13 @@
       <dynamics damping="1.0e-6" friction="0.8"/>
     </joint>
 
-    <link name="${NAME_LINK_NECK_2}">
+    <link name="neck_pitch_link">
       <visual>
         <origin rpy="1.5708 0 0" xyz="0.0 0.0 0.0"/>
         <geometry>
           <mesh filename="package://sciurus17_description/meshes/visual/Neck2.stl" scale="1.0 1.0 1.0"/>
         </geometry>
-        <material name="${COLOR_LINK_HEAD}"/>
+        <material name="white"/>
       </visual>
       <collision>
         <origin rpy="1.5708 0 0" xyz="0.0 0.0 0.0"/>
@@ -70,13 +70,13 @@
       </inertial>
     </link>
 
-    <joint name="${NAME_JOINT_HEAD_CAMERA}" type="fixed">
-      <parent link="${NAME_LINK_NECK_2}" />
-      <child link="${NAME_LINK_HEAD_CAMERA}" />
+    <joint name="head_camera_joint" type="fixed">
+      <parent link="neck_pitch_link" />
+      <child link="head_camera_link" />
       <origin xyz="0.081785 0.020 0.091" rpy="0 0 0"/>
     </joint>
 
-    <link name="${NAME_LINK_HEAD_CAMERA}"/>
+    <link name="head_camera_link"/>
 
   </xacro:macro>
 
diff --git a/urdf/sciurus17_left_arm.xacro b/urdf/sciurus17_left_arm.xacro
index 11979d3..23fb78b 100644
--- a/urdf/sciurus17_left_arm.xacro
+++ b/urdf/sciurus17_left_arm.xacro
@@ -4,9 +4,9 @@
 
   <xacro:macro name="sciurus17_left_arm">
 
-    <joint name="${NAME_JOINT_ARM_L_1}" type="revolute">
-      <parent link="${NAME_LINK_BODY}" />
-      <child link="${NAME_LINK_ARM_L_1}" />
+    <joint name="l_arm_joint1" type="revolute">
+      <parent link="body_link" />
+      <child link="l_link1" />
       <origin xyz="0.08083 0.0965 0.2885" rpy="0 0 0"/>
       <limit 
         lower="${JOINT_ARM_L_1_LOWER_LIMIT}" 
@@ -17,13 +17,13 @@
       <dynamics damping="1.0e-6" friction="0.8"/>
     </joint>
 
-    <link name="${NAME_LINK_ARM_L_1}">
+    <link name="l_link1">
       <visual>
         <origin rpy="-1.5708 0 0" xyz="0.0 0.0 0.0"/>
         <geometry>
           <mesh filename="package://sciurus17_description/meshes/visual/L_Link1.stl" scale="1.0 1.0 1.0"/>
         </geometry>
-        <material name="${COLOR_LINK_ARM}"/>
+        <material name="white"/>
       </visual>
       <collision>
         <origin rpy="-1.5708 0 0" xyz="0.0 0.0 0.0"/>
@@ -38,9 +38,9 @@
       </inertial>
     </link>
 
-    <joint name="${NAME_JOINT_ARM_L_2}" type="revolute">
-      <parent link="${NAME_LINK_ARM_L_1}" />
-      <child link="${NAME_LINK_ARM_L_2}" />
+    <joint name="l_arm_joint2" type="revolute">
+      <parent link="l_link1" />
+      <child link="l_link2" />
       <origin xyz="0.0 0.064 0.0" rpy="0 0 0"/>
       <limit 
         lower="${JOINT_ARM_L_2_LOWER_LIMIT}" 
@@ -51,13 +51,13 @@
       <dynamics damping="1.0e-6" friction="0.8"/>
     </joint>
 
-    <link name="${NAME_LINK_ARM_L_2}">
+    <link name="l_link2">
       <visual>
         <origin rpy="0 -1.5708 0" xyz="0.0 0.0 0.0"/>
         <geometry>
           <mesh filename="package://sciurus17_description/meshes/visual/L_Link2.stl" scale="1.0 1.0 1.0"/>
         </geometry>
-        <material name="${COLOR_LINK_ARM}"/>
+        <material name="white"/>
       </visual>
       <collision>
         <origin rpy="0 -1.5708 0" xyz="0.0 0.0 0.0"/>
@@ -72,9 +72,9 @@
       </inertial>
     </link>
 
-    <joint name="${NAME_JOINT_ARM_L_3}" type="revolute">
-      <parent link="${NAME_LINK_ARM_L_2}" />
-      <child link="${NAME_LINK_ARM_L_3}" />
+    <joint name="l_arm_joint3" type="revolute">
+      <parent link="l_link2" />
+      <child link="l_link3" />
       <origin xyz="0.0 0.065 0.0" rpy="0 0 0"/>
       <limit 
         lower="${JOINT_ARM_L_3_LOWER_LIMIT}"
@@ -85,13 +85,13 @@
       <dynamics damping="1.0e-6" friction="0.8"/>
     </joint>
 
-    <link name="${NAME_LINK_ARM_L_3}">
+    <link name="l_link3">
       <visual>
         <origin rpy="-1.5708 0 0" xyz="0.0 0.0 0.0"/>
         <geometry>
           <mesh filename="package://sciurus17_description/meshes/visual/L_Link3.stl" scale="1.0 1.0 1.0"/>
         </geometry>
-        <material name="${COLOR_LINK_ARM}"/>
+        <material name="white"/>
       </visual>
       <collision>
         <origin rpy="-1.5708 0 0" xyz="0.0 0.0 0.0"/>
@@ -106,9 +106,9 @@
       </inertial>
     </link>
 
-    <joint name="${NAME_JOINT_ARM_L_4}" type="revolute">
-      <parent link="${NAME_LINK_ARM_L_3}" />
-      <child link="${NAME_LINK_ARM_L_4}" />
+    <joint name="l_arm_joint4" type="revolute">
+      <parent link="l_link3" />
+      <child link="l_link4" />
       <origin xyz="0.0 0.185 0.0" rpy="0 0 0"/>
       <limit 
         lower="${JOINT_ARM_L_4_LOWER_LIMIT}" 
@@ -119,13 +119,13 @@
       <dynamics damping="1.0e-6" friction="0.8"/>
     </joint>
 
-    <link name="${NAME_LINK_ARM_L_4}">
+    <link name="l_link4">
       <visual>
         <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
         <geometry>
           <mesh filename="package://sciurus17_description/meshes/visual/L_Link4.stl" scale="1.0 1.0 1.0"/>
         </geometry>
-        <material name="${COLOR_LINK_ARM}"/>
+        <material name="white"/>
       </visual>
       <collision>
         <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
@@ -140,9 +140,9 @@
       </inertial>
     </link>
 
-    <joint name="${NAME_JOINT_ARM_L_5}" type="revolute">
-      <parent link="${NAME_LINK_ARM_L_4}" />
-      <child link="${NAME_LINK_ARM_L_5}" />
+    <joint name="l_arm_joint5" type="revolute">
+      <parent link="l_link4" />
+      <child link="l_link5" />
       <origin xyz="0.0 0.121 0.0" rpy="0 0 0"/>
       <limit 
         lower="${JOINT_ARM_L_5_LOWER_LIMIT}" 
@@ -153,13 +153,13 @@
       <dynamics damping="1.0e-6" friction="0.8"/>
     </joint>
 
-    <link name="${NAME_LINK_ARM_L_5}">
+    <link name="l_link5">
       <visual>
         <origin rpy="-1.5708 0 0" xyz="0.0 0.0 0.0"/>
         <geometry>
           <mesh filename="package://sciurus17_description/meshes/visual/L_Link5.stl" scale="1.0 1.0 1.0"/>
         </geometry>
-        <material name="${COLOR_LINK_ARM}"/>
+        <material name="white"/>
       </visual>
       <visual>
         <origin xyz="0.032 0.074 -0.0346" rpy="3.1415926 0 2.0944"/>
@@ -192,19 +192,19 @@
       </inertial>
     </link>
 
-    <joint name="${NAME_JOINT_ARMARKER_L}" type="fixed">
-      <parent link="${NAME_LINK_ARM_L_5}" />
-      <child link="${NAME_LINK_ARMARKER_L}" />
+    <joint name="l_arm_armarker" type="fixed">
+      <parent link="l_link5" />
+      <child link="l_link5_armarker" />
       <origin xyz="0.0467 0.093 0.0" rpy="0 0 0"/>
     </joint>
 
-    <link name="${NAME_LINK_ARMARKER_L}">
+    <link name="l_link5_armarker">
       <visual>
         <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
         <geometry>
           <cylinder length="0.001" radius="0.001"/>
         </geometry>
-        <material name="${COLOR_LINK_ARM}"/>
+        <material name="white"/>
       </visual>
       <collision>
         <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
@@ -214,9 +214,9 @@
       </collision>
     </link>
 
-    <joint name="${NAME_JOINT_ARM_L_6}" type="revolute">
-      <parent link="${NAME_LINK_ARM_L_5}" />
-      <child link="${NAME_LINK_ARM_L_6}" />
+    <joint name="l_arm_joint6" type="revolute">
+      <parent link="l_link5" />
+      <child link="l_link6" />
       <origin xyz="0.0 0.129 0.0" rpy="0 0 0"/>
       <limit 
         lower="${JOINT_ARM_L_6_LOWER_LIMIT}" 
@@ -227,13 +227,13 @@
       <dynamics damping="1.0e-6" friction="0.8"/>
     </joint>
 
-    <link name="${NAME_LINK_ARM_L_6}">
+    <link name="l_link6">
       <visual>
         <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
         <geometry>
           <mesh filename="package://sciurus17_description/meshes/visual/L_Link6.stl" scale="1.0 1.0 1.0"/>
         </geometry>
-        <material name="${COLOR_LINK_ARM}"/>
+        <material name="white"/>
       </visual>
       <collision>
         <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
@@ -248,9 +248,9 @@
       </inertial>
     </link>
 
-    <joint name="${NAME_JOINT_ARM_L_7}" type="revolute">
-      <parent link="${NAME_LINK_ARM_L_6}" />
-      <child link="${NAME_LINK_ARM_L_7}" />
+    <joint name="l_arm_joint7" type="revolute">
+      <parent link="l_link6" />
+      <child link="l_link7" />
       <origin xyz="0.0 0.0191 0.0" rpy="0 0 0"/>
       <limit 
         lower="${JOINT_ARM_L_7_LOWER_LIMIT}" 
diff --git a/urdf/sciurus17_left_gripper.xacro b/urdf/sciurus17_left_gripper.xacro
index bfc5b06..0738f81 100644
--- a/urdf/sciurus17_left_gripper.xacro
+++ b/urdf/sciurus17_left_gripper.xacro
@@ -4,13 +4,13 @@
 
   <xacro:macro name="sciurus17_left_gripper">
 
-    <link name="${NAME_LINK_ARM_L_7}">
+    <link name="l_link7">
       <visual>
         <origin rpy="1.5708 0 0" xyz="0.0 0.0 0.0"/>
         <geometry>
           <mesh filename="package://sciurus17_description/meshes/visual/L_Link7.stl" scale="1.0 1.0 1.0"/>
         </geometry>
-        <material name="${COLOR_LINK_GRIPPER}"/>
+        <material name="red"/>
       </visual>
       <collision>
         <origin rpy="1.5708 0 0" xyz="0.0 0.0 0.0"/>
@@ -25,9 +25,9 @@
       </inertial>
     </link>
 
-    <joint name="${NAME_JOINT_GRIPPER_L_A}" type="revolute">
-      <parent link="${NAME_LINK_ARM_L_7}" />
-      <child link="${NAME_LINK_GRIPPER_L_A}" />
+    <joint name="l_gripper_joint" type="revolute">
+      <parent link="l_link7" />
+      <child link="l_gripperA_link" />
       <origin xyz="0.012 0.024 0.0" rpy="0 0 0"/>
       <limit 
         lower="${JOINT_GRIPPER_L_LOWER_LIMIT}" 
@@ -38,13 +38,13 @@
       <dynamics damping="1.0e-6" friction="0.8"/>
     </joint>
 
-    <link name="${NAME_LINK_GRIPPER_L_A}">
+    <link name="l_gripperA_link">
       <visual>
         <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
         <geometry>
           <mesh filename="package://sciurus17_description/meshes/visual/L_GripperA.stl" scale="1.0 1.0 1.0"/>
         </geometry>
-        <material name="${COLOR_LINK_GRIPPER}"/>
+        <material name="red"/>
       </visual>
       <collision>
         <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
@@ -59,9 +59,9 @@
       </inertial>
     </link>
 
-    <joint name="${NAME_JOINT_GRIPPER_L_B}" type="revolute">
-      <parent link="${NAME_LINK_ARM_L_7}" />
-      <child link="${NAME_LINK_GRIPPER_L_B}" />
+    <joint name="l_gripper_mimic_joint" type="revolute">
+      <parent link="l_link7" />
+      <child link="l_gripperB_link" />
       <origin xyz="-0.012 0.024 0.0" rpy="0 0 0"/>
       <limit 
         lower="${JOINT_GRIPPER_L_LOWER_LIMIT}" 
@@ -69,17 +69,17 @@
         effort="${EFFORT_LIMIT}" 
         velocity="${VELOCITY_LIMIT}"/>
       <axis xyz="0 0 -1"/>
-      <mimic joint="${NAME_JOINT_GRIPPER_L_A}" multiplier="1" offset="0"/>
+      <mimic joint="l_gripper_joint" multiplier="1" offset="0"/>
       <dynamics damping="1.0e-6" friction="0.8"/>
     </joint>
 
-    <link name="${NAME_LINK_GRIPPER_L_B}">
+    <link name="l_gripperB_link">
       <visual>
         <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
         <geometry>
           <mesh filename="package://sciurus17_description/meshes/visual/L_GripperB.stl" scale="1.0 1.0 1.0"/>
         </geometry>
-        <material name="${COLOR_LINK_GRIPPER}"/>
+        <material name="red"/>
       </visual>
       <collision>
         <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
diff --git a/urdf/sciurus17_right_arm.xacro b/urdf/sciurus17_right_arm.xacro
index 8ceef8e..a897bae 100644
--- a/urdf/sciurus17_right_arm.xacro
+++ b/urdf/sciurus17_right_arm.xacro
@@ -4,9 +4,9 @@
 
   <xacro:macro name="sciurus17_right_arm">
 
-    <joint name="${NAME_JOINT_ARM_R_1}" type="revolute">
-      <parent link="${NAME_LINK_BODY}" />
-      <child link="${NAME_LINK_ARM_R_1}" />
+    <joint name="r_arm_joint1" type="revolute">
+      <parent link="body_link" />
+      <child link="r_link1" />
       <origin xyz="0.08083 -0.0965 0.2885" rpy="0 0 0"/>
       <limit 
         lower="${JOINT_ARM_R_1_LOWER_LIMIT}" 
@@ -17,13 +17,13 @@
       <dynamics damping="1.0e-6" friction="0.8"/>
     </joint>
 
-    <link name="${NAME_LINK_ARM_R_1}">
+    <link name="r_link1">
       <visual>
         <origin rpy="1.5708 0 0" xyz="0.0 0.0 0.0"/>
         <geometry>
           <mesh filename="package://sciurus17_description/meshes/visual/R_Link1.stl" scale="1.0 1.0 1.0"/>
         </geometry>
-        <material name="${COLOR_LINK_ARM}"/>
+        <material name="white"/>
       </visual>
       <collision>
         <origin rpy="1.5708 0 0" xyz="0.0 0.0 0.0"/>
@@ -38,9 +38,9 @@
       </inertial>
     </link>
 
-    <joint name="${NAME_JOINT_ARM_R_2}" type="revolute">
-      <parent link="${NAME_LINK_ARM_R_1}" />
-      <child link="${NAME_LINK_ARM_R_2}" />
+    <joint name="r_arm_joint2" type="revolute">
+      <parent link="r_link1" />
+      <child link="r_link2" />
       <origin xyz="0.0 -0.064 0.0" rpy="0 0 0"/>
       <limit 
         lower="${JOINT_ARM_R_2_LOWER_LIMIT}" 
@@ -51,13 +51,13 @@
       <dynamics damping="1.0e-6" friction="0.8"/>
     </joint>
 
-    <link name="${NAME_LINK_ARM_R_2}">
+    <link name="r_link2">
       <visual>
         <origin rpy="0 1.5708 0" xyz="0.0 0.0 0.0"/>
         <geometry>
           <mesh filename="package://sciurus17_description/meshes/visual/R_Link2.stl" scale="1.0 1.0 1.0"/>
         </geometry>
-        <material name="${COLOR_LINK_ARM}"/>
+        <material name="white"/>
       </visual>
       <collision>
         <origin rpy="0 1.5708 0" xyz="0.0 0.0 0.0"/>
@@ -72,9 +72,9 @@
       </inertial>
     </link>
 
-    <joint name="${NAME_JOINT_ARM_R_3}" type="revolute">
-      <parent link="${NAME_LINK_ARM_R_2}" />
-      <child link="${NAME_LINK_ARM_R_3}" />
+    <joint name="r_arm_joint3" type="revolute">
+      <parent link="r_link2" />
+      <child link="r_link3" />
       <origin xyz="0.0 -0.065 0.0" rpy="0 0 0"/>
       <limit 
         lower="${JOINT_ARM_R_3_LOWER_LIMIT}"
@@ -85,13 +85,13 @@
       <dynamics damping="1.0e-6" friction="0.8"/>
     </joint>
 
-    <link name="${NAME_LINK_ARM_R_3}">
+    <link name="r_link3">
       <visual>
         <origin rpy="1.5708 0 0" xyz="0.0 0.0 0.0"/>
         <geometry>
           <mesh filename="package://sciurus17_description/meshes/visual/L_Link3.stl" scale="1.0 1.0 1.0"/>
         </geometry>
-        <material name="${COLOR_LINK_ARM}"/>
+        <material name="white"/>
       </visual>
       <collision>
         <origin rpy="1.5708 0 0" xyz="0.0 0.0 0.0"/>
@@ -106,9 +106,9 @@
       </inertial>
     </link>
 
-    <joint name="${NAME_JOINT_ARM_R_4}" type="revolute">
-      <parent link="${NAME_LINK_ARM_R_3}" />
-      <child link="${NAME_LINK_ARM_R_4}" />
+    <joint name="r_arm_joint4" type="revolute">
+      <parent link="r_link3" />
+      <child link="r_link4" />
       <origin xyz="0.0 -0.185 0.0" rpy="0 0 0"/>
       <limit 
         lower="${JOINT_ARM_R_4_LOWER_LIMIT}" 
@@ -119,13 +119,13 @@
       <dynamics damping="1.0e-6" friction="0.8"/>
     </joint>
 
-    <link name="${NAME_LINK_ARM_R_4}">
+    <link name="r_link4">
       <visual>
         <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
         <geometry>
           <mesh filename="package://sciurus17_description/meshes/visual/R_Link4.stl" scale="1.0 1.0 1.0"/>
         </geometry>
-        <material name="${COLOR_LINK_ARM}"/>
+        <material name="white"/>
       </visual>
       <collision>
         <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
@@ -140,9 +140,9 @@
       </inertial>
     </link>
 
-    <joint name="${NAME_JOINT_ARM_R_5}" type="revolute">
-      <parent link="${NAME_LINK_ARM_R_4}" />
-      <child link="${NAME_LINK_ARM_R_5}" />
+    <joint name="r_arm_joint5" type="revolute">
+      <parent link="r_link4" />
+      <child link="r_link5" />
       <origin xyz="0.0 -0.121 0.0" rpy="0 0 0"/>
       <limit 
         lower="${JOINT_ARM_R_5_LOWER_LIMIT}" 
@@ -153,13 +153,13 @@
       <dynamics damping="1.0e-6" friction="0.8"/>
     </joint>
 
-    <link name="${NAME_LINK_ARM_R_5}">
+    <link name="r_link5">
       <visual>
         <origin rpy="1.5708 0 0" xyz="0.0 0.0 0.0"/>
         <geometry>
           <mesh filename="package://sciurus17_description/meshes/visual/R_Link5.stl" scale="1.0 1.0 1.0"/>
         </geometry>
-        <material name="${COLOR_LINK_ARM}"/>
+        <material name="white"/>
       </visual>
       <visual>
         <origin xyz="0.032 -0.074 0.0345" rpy="0 0 -2.0944"/>
@@ -192,19 +192,19 @@
       </inertial>
     </link>
 
-    <joint name="${NAME_JOINT_ARMARKER_R}" type="fixed">
-      <parent link="${NAME_LINK_ARM_R_5}" />
-      <child link="${NAME_LINK_ARMARKER_R}" />
+    <joint name="r_arm_armarker" type="fixed">
+      <parent link="r_link5" />
+      <child link="r_link5_armarker" />
       <origin xyz="0.0467 -0.093 0.0" rpy="0 0 0"/>
     </joint>
 
-    <link name="${NAME_LINK_ARMARKER_R}">
+    <link name="r_link5_armarker">
       <visual>
         <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
         <geometry>
           <cylinder length="0.001" radius="0.001"/>
         </geometry>
-        <material name="${COLOR_LINK_ARM}"/>
+        <material name="white"/>
       </visual>
       <collision>
         <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
@@ -214,9 +214,9 @@
       </collision>
     </link>
 
-    <joint name="${NAME_JOINT_ARM_R_6}" type="revolute">
-      <parent link="${NAME_LINK_ARM_R_5}" />
-      <child link="${NAME_LINK_ARM_R_6}" />
+    <joint name="r_arm_joint6" type="revolute">
+      <parent link="r_link5" />
+      <child link="r_link6" />
       <origin xyz="0.0 -0.129 0.0" rpy="0 0 0"/>
       <limit 
         lower="${JOINT_ARM_R_6_LOWER_LIMIT}" 
@@ -227,13 +227,13 @@
       <dynamics damping="1.0e-6" friction="0.8"/>
     </joint>
 
-    <link name="${NAME_LINK_ARM_R_6}">
+    <link name="r_link6">
       <visual>
         <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
         <geometry>
           <mesh filename="package://sciurus17_description/meshes/visual/R_Link6.stl" scale="1.0 1.0 1.0"/>
         </geometry>
-        <material name="${COLOR_LINK_ARM}"/>
+        <material name="white"/>
       </visual>
       <collision>
         <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
@@ -248,9 +248,9 @@
       </inertial>
     </link>
 
-    <joint name="${NAME_JOINT_ARM_R_7}" type="revolute">
-      <parent link="${NAME_LINK_ARM_R_6}" />
-      <child link="${NAME_LINK_ARM_R_7}" />
+    <joint name="r_arm_joint7" type="revolute">
+      <parent link="r_link6" />
+      <child link="r_link7" />
       <origin xyz="0.0 -0.0191 0.0" rpy="0 0 0"/>
       <limit 
         lower="${JOINT_ARM_R_7_LOWER_LIMIT}" 
diff --git a/urdf/sciurus17_right_gripper.xacro b/urdf/sciurus17_right_gripper.xacro
index cb2df2f..e7e07e9 100644
--- a/urdf/sciurus17_right_gripper.xacro
+++ b/urdf/sciurus17_right_gripper.xacro
@@ -4,13 +4,13 @@
 
   <xacro:macro name="sciurus17_right_gripper">
 
-    <link name="${NAME_LINK_ARM_R_7}">
+    <link name="r_link7">
       <visual>
         <origin rpy="1.5708 0 0" xyz="0.0 0.0 0.0"/>
         <geometry>
           <mesh filename="package://sciurus17_description/meshes/visual/R_Link7.stl" scale="1.0 1.0 1.0"/>
         </geometry>
-        <material name="${COLOR_LINK_GRIPPER}"/>
+        <material name="red"/>
       </visual>
       <collision>
         <origin rpy="1.5708 0 0" xyz="0.0 0.0 0.0"/>
@@ -25,9 +25,9 @@
       </inertial>
     </link>
 
-    <joint name="${NAME_JOINT_GRIPPER_R_A}" type="revolute">
-      <parent link="${NAME_LINK_ARM_R_7}" />
-      <child link="${NAME_LINK_GRIPPER_R_A}" />
+    <joint name="r_gripper_joint" type="revolute">
+      <parent link="r_link7" />
+      <child link="r_gripperA_link" />
       <origin xyz="0.012 -0.024 0.0" rpy="0 0 0"/>
       <limit 
         lower="${JOINT_GRIPPER_R_LOWER_LIMIT}" 
@@ -38,14 +38,14 @@
       <dynamics damping="1.0e-6" friction="0.8"/>
     </joint>
 
-    <link name="${NAME_LINK_GRIPPER_R_A}">
+    <link name="r_gripperA_link">
       <visual>
         <!--origin rpy="3.14159 0 0" xyz="0.0 0.0 0.0"/-->
         <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
         <geometry>
           <mesh filename="package://sciurus17_description/meshes/visual/R_GripperA.stl" scale="1.0 1.0 1.0"/>
         </geometry>
-        <material name="${COLOR_LINK_GRIPPER}"/>
+        <material name="red"/>
       </visual>
       <collision>
         <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
@@ -60,9 +60,9 @@
       </inertial>
     </link>
 
-    <joint name="${NAME_JOINT_GRIPPER_R_B}" type="revolute">
-      <parent link="${NAME_LINK_ARM_R_7}" />
-      <child link="${NAME_LINK_GRIPPER_R_B}" />
+    <joint name="r_gripper_mimic_joint" type="revolute">
+      <parent link="r_link7" />
+      <child link="r_gripperB_link" />
       <origin xyz="-0.012 -0.024 0.0" rpy="0 0 0"/>
       <limit 
         lower="${JOINT_GRIPPER_R_LOWER_LIMIT}" 
@@ -70,18 +70,18 @@
         effort="${EFFORT_LIMIT}" 
         velocity="${VELOCITY_LIMIT}"/>
       <axis xyz="0 0 -1"/>
-      <mimic joint="${NAME_JOINT_GRIPPER_R_A}" multiplier="1" offset="0"/>
+      <mimic joint="r_gripper_joint" multiplier="1" offset="0"/>
       <dynamics damping="1.0e-6" friction="0.8"/>
     </joint>
 
-    <link name="${NAME_LINK_GRIPPER_R_B}">
+    <link name="r_gripperB_link">
       <visual>
         <!-- origin rpy="3.14159 0 0" xyz="0.0 0.0 0.0"/ -->
         <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
         <geometry>
           <mesh filename="package://sciurus17_description/meshes/visual/R_GripperB.stl" scale="1.0 1.0 1.0"/>
         </geometry>
-        <material name="${COLOR_LINK_GRIPPER}"/>
+        <material name="red"/>
       </visual>
       <collision>
         <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>