diff --git a/launch/display.rviz b/launch/display.rviz
index d11165d..5dd80cc 100644
--- a/launch/display.rviz
+++ b/launch/display.rviz
@@ -9,7 +9,7 @@ Panels:
         - /RobotModel1
         - /TF1
       Splitter Ratio: 0.5
-    Tree Height: 802
+    Tree Height: 793
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -161,10 +161,17 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        dummy_mimic_fix:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
         world:
           Alpha: 1
           Show Axes: false
           Show Trail: false
+      Mass Properties:
+        Inertia: false
+        Mass: false
       Name: RobotModel
       TF Prefix: ""
       Update Interval: 0
@@ -172,6 +179,8 @@ Visualization Manager:
       Visual Enabled: true
     - Class: rviz_default_plugins/TF
       Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
       Frame Timeout: 15
       Frames:
         All Enabled: true
@@ -215,6 +224,8 @@ Visualization Manager:
           Value: true
         crane_x7_wrist_link:
           Value: true
+        dummy_mimic_fix:
+          Value: true
         world:
           Value: true
       Marker Scale: 0.30000001192092896
@@ -254,6 +265,8 @@ Visualization Manager:
                                 {}
                               crane_x7_gripper_finger_b_link:
                                 {}
+          dummy_mimic_fix:
+            {}
       Update Interval: 0
       Value: true
   Enabled: true
@@ -271,6 +284,9 @@ Visualization Manager:
     - Class: rviz_default_plugins/Measure
       Line color: 128; 128; 0
     - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
       Topic:
         Depth: 5
         Durability Policy: Volatile
@@ -299,39 +315,39 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 1.1836897134780884
+      Distance: 1
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -0.0668451264500618
-        Y: 0.009886191226541996
-        Z: 0.30861979722976685
+        X: 0
+        Y: 0
+        Z: 0.4000000059604645
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.23039844632148743
+      Pitch: 0.5
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 0.635398805141449
+      Yaw: 0.5
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1031
+  Height: 1016
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000003adfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003ad000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003adfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003ad000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000050f000003ad00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000003a2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000003a2000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003a2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000003a2000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004c9000003a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Tool Properties:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1920
-  X: 0
-  Y: 25
+  Width: 1850
+  X: 70
+  Y: 27
diff --git a/urdf/crane_x7.gazebo.xacro b/urdf/crane_x7.gazebo.xacro
index c2380f2..6d709b2 100644
--- a/urdf/crane_x7.gazebo.xacro
+++ b/urdf/crane_x7.gazebo.xacro
@@ -29,111 +29,89 @@
     </visual>
   </xacro:macro>
 
-  <xacro:macro name="gazebo_robot_settings"
-    params="name_link_mounting_plate
-            name_link_base
-            name_link_1
-            name_link_2
-            name_link_3
-            name_link_4
-            name_link_5
-            name_link_6
-            name_link_cover_2_l
-            name_link_cover_2_r
-            name_link_cover_4_l
-            name_link_cover_4_r
-            name_link_logo_symbol_l
-            name_link_logo_symbol_r
-            name_link_logo_text_l
-            name_link_logo_text_r
-            name_link_gripper_base
-            name_link_gripper_finger_a
-            name_link_gripper_finger_b
-            config_package
-            config_file_path
-            ">
+  <xacro:macro name="gazebo_robot_settings">
 
     <gazebo>
       <plugin filename="libgz_ros2_control-system.so" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
-        <parameters>$(find ${config_package})/${config_file_path}</parameters>
+        <parameters>$(find ${GZ_CONTROL_CONFIG_PACKAGE})/${GZ_CONTROL_CONFIG_FILE_PATH}</parameters>
       </plugin>
     </gazebo>
 
-    <gazebo reference="${name_link_mounting_plate}">
+    <gazebo reference="${NAME_LINK_MOUNTING_PLATE}">
       <xacro:material_gazebo_black/>
     </gazebo>
 
-    <gazebo reference="${name_link_base}">
+    <gazebo reference="${NAME_LINK_BASE}">
       <xacro:material_gazebo_red/>
     </gazebo>
 
-    <gazebo reference="${name_link_1}">
+    <gazebo reference="${NAME_LINK_1}">
       <xacro:material_gazebo_white/>
     </gazebo>
 
-    <gazebo reference="${name_link_2}">
+    <gazebo reference="${NAME_LINK_2}">
       <xacro:material_gazebo_white/>
     </gazebo>
 
-    <gazebo reference="${name_link_3}">
+    <gazebo reference="${NAME_LINK_3}">
       <xacro:material_gazebo_white/>
     </gazebo>
 
-    <gazebo reference="${name_link_4}">
+    <gazebo reference="${NAME_LINK_4}">
       <xacro:material_gazebo_white/>
     </gazebo>
 
-    <gazebo reference="${name_link_5}">
+    <gazebo reference="${NAME_LINK_5}">
       <xacro:material_gazebo_white/>
     </gazebo>
 
-    <gazebo reference="${name_link_6}">
+    <gazebo reference="${NAME_LINK_6}">
       <xacro:material_gazebo_white/>
     </gazebo>
 
-    <gazebo reference="${name_link_cover_2_l}">
+    <gazebo reference="${NAME_LINK_COVER_2_L}">
       <xacro:material_gazebo_red/>
     </gazebo>
 
-    <gazebo reference="${name_link_cover_2_r}">
+    <gazebo reference="${NAME_LINK_COVER_2_R}">
       <xacro:material_gazebo_red/>
     </gazebo>
 
-    <gazebo reference="${name_link_cover_4_l}">
+    <gazebo reference="${NAME_LINK_COVER_4_L}">
       <xacro:material_gazebo_red/>
     </gazebo>
 
-    <gazebo reference="${name_link_cover_4_r}">
+    <gazebo reference="${NAME_LINK_COVER_4_R}">
       <xacro:material_gazebo_red/>
     </gazebo>
 
-    <gazebo reference="${name_link_logo_symbol_l}">
+    <gazebo reference="${NAME_LINK_LOGO_SYMBOL_L}">
       <xacro:material_gazebo_red/>
     </gazebo>
 
-    <gazebo reference="${name_link_logo_symbol_r}">
+    <gazebo reference="${NAME_LINK_LOGO_SYMBOL_R}">
       <xacro:material_gazebo_red/>
     </gazebo>
 
-    <gazebo reference="${name_link_logo_text_l}">
+    <gazebo reference="${NAME_LINK_LOGO_TEXT_L}">
       <xacro:material_gazebo_black/>
     </gazebo>
 
-    <gazebo reference="${name_link_logo_text_r}">
+    <gazebo reference="${NAME_LINK_LOGO_TEXT_R}">
       <xacro:material_gazebo_black/>
     </gazebo>
 
-    <gazebo reference="${name_link_gripper_base}">
+    <gazebo reference="${NAME_LINK_GRIPPER_BASE}">
       <xacro:material_gazebo_red/>
     </gazebo>
 
-    <gazebo reference="${name_link_gripper_finger_a}">
+    <gazebo reference="${NAME_LINK_GRIPPER_FINGER_A}">
       <mu1>0.8</mu1>
       <mu2>0.8</mu2>
       <xacro:material_gazebo_red/>
     </gazebo>
 
-    <gazebo reference="${name_link_gripper_finger_b}">
+    <gazebo reference="${NAME_LINK_GRIPPER_FINGER_B}">
       <mu1>0.8</mu1>
       <mu2>0.8</mu2>
       <xacro:material_gazebo_red/>
diff --git a/urdf/crane_x7.gazebo_ros2_control.xacro b/urdf/crane_x7.gazebo_ros2_control.xacro
index 753a8f4..c4acdb3 100644
--- a/urdf/crane_x7.gazebo_ros2_control.xacro
+++ b/urdf/crane_x7.gazebo_ros2_control.xacro
@@ -1,122 +1,95 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-  <xacro:macro name="crane_x7_gazebo_ros2_control_settings"
-    params="name
-            name_joint_1
-            name_joint_2
-            name_joint_3
-            name_joint_4
-            name_joint_5
-            name_joint_6
-            name_joint_7
-            name_joint_gripper_finger_a
-            name_joint_gripper_finger_b
-            joint_1_lower_limit
-            joint_1_upper_limit
-            joint_2_lower_limit
-            joint_2_upper_limit
-            joint_3_lower_limit
-            joint_3_upper_limit
-            joint_4_lower_limit
-            joint_4_upper_limit
-            joint_5_lower_limit
-            joint_5_upper_limit
-            joint_6_lower_limit
-            joint_6_upper_limit
-            joint_7_lower_limit
-            joint_7_upper_limit
-            joint_hand_lower_limit
-            joint_hand_upper_limit
-            ">
+  <xacro:macro name="crane_x7_gazebo_ros2_control_settings">
 
-    <ros2_control name="${name}" type="system">
+    <ros2_control name="crane_x7" type="system">
       <hardware>
         <plugin>gz_ros2_control/GazeboSimSystem</plugin>
       </hardware>
 
-      <joint name="${name_joint_1}">
+      <joint name="${NAME_JOINT_1}">
         <command_interface name="position">
-          <param name="min">${joint_1_lower_limit}</param>
-          <param name="max">${joint_1_upper_limit}</param>
+          <param name="min">${JOINT_1_LOWER_LIMIT}</param>
+          <param name="max">${JOINT_1_UPPER_LIMIT}</param>
         </command_interface>
         <state_interface name="position"/>
         <state_interface name="velocity"/>
         <state_interface name="effort"/>
       </joint>
 
-      <joint name="${name_joint_2}">
+      <joint name="${NAME_JOINT_2}">
         <command_interface name="position">
-          <param name="min">${joint_2_lower_limit}</param>
-          <param name="max">${joint_2_upper_limit}</param>
+          <param name="min">${JOINT_2_LOWER_LIMIT}</param>
+          <param name="max">${JOINT_2_UPPER_LIMIT}</param>
         </command_interface>
         <state_interface name="position"/>
         <state_interface name="velocity"/>
         <state_interface name="effort"/>
       </joint>
 
-      <joint name="${name_joint_3}">
+      <joint name="${NAME_JOINT_3}">
         <command_interface name="position">
-          <param name="min">${joint_3_lower_limit}</param>
-          <param name="max">${joint_3_upper_limit}</param>
+          <param name="min">${JOINT_3_LOWER_LIMIT}</param>
+          <param name="max">${JOINT_3_UPPER_LIMIT}</param>
         </command_interface>
         <state_interface name="position"/>
         <state_interface name="velocity"/>
         <state_interface name="effort"/>
       </joint>
 
-      <joint name="${name_joint_4}">
+      <joint name="${NAME_JOINT_4}">
         <command_interface name="position">
-          <param name="min">${joint_4_lower_limit}</param>
-          <param name="max">${joint_4_upper_limit}</param>
+          <param name="min">${JOINT_4_LOWER_LIMIT}</param>
+          <param name="max">${JOINT_4_UPPER_LIMIT}</param>
         </command_interface>
         <state_interface name="position"/>
         <state_interface name="velocity"/>
         <state_interface name="effort"/>
       </joint>
 
-      <joint name="${name_joint_5}">
+      <joint name="${NAME_JOINT_5}">
         <command_interface name="position">
-          <param name="min">${joint_5_lower_limit}</param>
-          <param name="max">${joint_5_upper_limit}</param>
+          <param name="min">${JOINT_5_LOWER_LIMIT}</param>
+          <param name="max">${JOINT_5_UPPER_LIMIT}</param>
         </command_interface>
         <state_interface name="position"/>
         <state_interface name="velocity"/>
         <state_interface name="effort"/>
       </joint>
 
-      <joint name="${name_joint_6}">
+      <joint name="${NAME_JOINT_6}">
         <command_interface name="position">
-          <param name="min">${joint_6_lower_limit}</param>
-          <param name="max">${joint_6_upper_limit}</param>
+          <param name="min">${JOINT_6_LOWER_LIMIT}</param>
+          <param name="max">${JOINT_6_UPPER_LIMIT}</param>
         </command_interface>
         <state_interface name="position"/>
         <state_interface name="velocity"/>
         <state_interface name="effort"/>
       </joint>
 
-      <joint name="${name_joint_7}">
+      <joint name="${NAME_JOINT_7}">
         <command_interface name="position">
-          <param name="min">${joint_7_lower_limit}</param>
-          <param name="max">${joint_7_upper_limit}</param>
+          <param name="min">${JOINT_7_LOWER_LIMIT}</param>
+          <param name="max">${JOINT_7_UPPER_LIMIT}</param>
         </command_interface>
         <state_interface name="position"/>
         <state_interface name="velocity"/>
         <state_interface name="effort"/>
       </joint>
 
-      <joint name="${name_joint_gripper_finger_a}">
+      <joint name="${NAME_JOINT_GRIPPER_FINGER_A}">
         <command_interface name="position">
-          <param name="min">${joint_hand_lower_limit}</param>
-          <param name="max">${joint_hand_upper_limit}</param>
+          <param name="min">${JOINT_HAND_LOWER_LIMIT}</param>
+          <param name="max">${JOINT_HAND_UPPER_LIMIT}</param>
         </command_interface>
         <state_interface name="position"/>
         <state_interface name="velocity"/>
         <state_interface name="effort"/>
       </joint>
 
-      <joint name="${name_joint_gripper_finger_b}">
-        <param name="mimic">${name_joint_gripper_finger_a}</param>
+      <joint name="${NAME_JOINT_GRIPPER_FINGER_B}">
+        <param name="mimic">${NAME_JOINT_GRIPPER_FINGER_A}</param>
         <param name="multiplier">1</param>
         <state_interface name="position"/>
         <state_interface name="velocity"/>
diff --git a/urdf/crane_x7.ros2_control.xacro b/urdf/crane_x7.ros2_control.xacro
index 035abc3..68d9fd6 100644
--- a/urdf/crane_x7.ros2_control.xacro
+++ b/urdf/crane_x7.ros2_control.xacro
@@ -1,59 +1,27 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-  <xacro:macro name="crane_x7_ros2_control_settings"
-    params="name
-            name_joint_1
-            name_joint_2
-            name_joint_3
-            name_joint_4
-            name_joint_5
-            name_joint_6
-            name_joint_7
-            name_joint_gripper_finger_a
-            joint_1_lower_limit
-            joint_1_upper_limit
-            joint_2_lower_limit
-            joint_2_upper_limit
-            joint_3_lower_limit
-            joint_3_upper_limit
-            joint_4_lower_limit
-            joint_4_upper_limit
-            joint_5_lower_limit
-            joint_5_upper_limit
-            joint_6_lower_limit
-            joint_6_upper_limit
-            joint_7_lower_limit
-            joint_7_upper_limit
-            joint_hand_lower_limit
-            joint_hand_upper_limit
-            port_name
-            baudrate
-            timeout_seconds
-            manipulator_config_file_path
-            manipulator_links_file_path
-            use_mock_components
-            ">
+  <xacro:macro name="crane_x7_ros2_control_settings">
 
-    <ros2_control name="${name}" type="system">
+    <ros2_control name="crane_x7" 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>crane_x7_hardware/CraneX7Hardware</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="manipulator_links_file_path">${manipulator_links_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>
+          <param name="manipulator_links_file_path">${MANIPULATOR_LINKS_FILE_PATH}</param>
         </xacro:unless>
       </hardware>
 
-      <joint name="${name_joint_1}">
+      <joint name="${NAME_JOINT_1}">
         <command_interface name="position">
-          <param name="min">${joint_1_lower_limit}</param>
-          <param name="max">${joint_1_upper_limit}</param>
+          <param name="min">${JOINT_1_LOWER_LIMIT}</param>
+          <param name="max">${JOINT_1_UPPER_LIMIT}</param>
         </command_interface>
         <state_interface name="position"/>
         <state_interface name="velocity"/>
@@ -61,10 +29,10 @@
         <param name="current_to_effort">2.20</param>
       </joint>
 
-      <joint name="${name_joint_2}">
+      <joint name="${NAME_JOINT_2}">
         <command_interface name="position">
-          <param name="min">${joint_2_lower_limit}</param>
-          <param name="max">${joint_2_upper_limit}</param>
+          <param name="min">${JOINT_2_LOWER_LIMIT}</param>
+          <param name="max">${JOINT_2_UPPER_LIMIT}</param>
         </command_interface>
         <state_interface name="position"/>
         <state_interface name="velocity"/>
@@ -72,10 +40,10 @@
         <param name="current_to_effort">3.60</param>
       </joint>
 
-      <joint name="${name_joint_3}">
+      <joint name="${NAME_JOINT_3}">
         <command_interface name="position">
-          <param name="min">${joint_3_lower_limit}</param>
-          <param name="max">${joint_3_upper_limit}</param>
+          <param name="min">${JOINT_3_LOWER_LIMIT}</param>
+          <param name="max">${JOINT_3_UPPER_LIMIT}</param>
         </command_interface>
         <state_interface name="position"/>
         <state_interface name="velocity"/>
@@ -83,10 +51,10 @@
         <param name="current_to_effort">2.20</param>
       </joint>
 
-      <joint name="${name_joint_4}">
+      <joint name="${NAME_JOINT_4}">
         <command_interface name="position">
-          <param name="min">${joint_4_lower_limit}</param>
-          <param name="max">${joint_4_upper_limit}</param>
+          <param name="min">${JOINT_4_LOWER_LIMIT}</param>
+          <param name="max">${JOINT_4_UPPER_LIMIT}</param>
         </command_interface>
         <state_interface name="position"/>
         <state_interface name="velocity"/>
@@ -94,10 +62,10 @@
         <param name="current_to_effort">2.20</param>
       </joint>
 
-      <joint name="${name_joint_5}">
+      <joint name="${NAME_JOINT_5}">
         <command_interface name="position">
-          <param name="min">${joint_5_lower_limit}</param>
-          <param name="max">${joint_5_upper_limit}</param>
+          <param name="min">${JOINT_5_LOWER_LIMIT}</param>
+          <param name="max">${JOINT_5_UPPER_LIMIT}</param>
         </command_interface>
         <state_interface name="position"/>
         <state_interface name="velocity"/>
@@ -105,10 +73,10 @@
         <param name="current_to_effort">2.20</param>
       </joint>
 
-      <joint name="${name_joint_6}">
+      <joint name="${NAME_JOINT_6}">
         <command_interface name="position">
-          <param name="min">${joint_6_lower_limit}</param>
-          <param name="max">${joint_6_upper_limit}</param>
+          <param name="min">${JOINT_6_LOWER_LIMIT}</param>
+          <param name="max">${JOINT_6_UPPER_LIMIT}</param>
         </command_interface>
         <state_interface name="position"/>
         <state_interface name="velocity"/>
@@ -116,10 +84,10 @@
         <param name="current_to_effort">2.20</param>
       </joint>
 
-      <joint name="${name_joint_7}">
+      <joint name="${NAME_JOINT_7}">
         <command_interface name="position">
-          <param name="min">${joint_7_lower_limit}</param>
-          <param name="max">${joint_7_upper_limit}</param>
+          <param name="min">${JOINT_7_LOWER_LIMIT}</param>
+          <param name="max">${JOINT_7_UPPER_LIMIT}</param>
         </command_interface>
         <state_interface name="position"/>
         <state_interface name="velocity"/>
@@ -127,10 +95,10 @@
         <param name="current_to_effort">2.20</param>
       </joint>
 
-      <joint name="${name_joint_gripper_finger_a}">
+      <joint name="${NAME_JOINT_GRIPPER_FINGER_A}">
         <command_interface name="position">
-          <param name="min">${joint_hand_lower_limit}</param>
-          <param name="max">${joint_hand_upper_limit}</param>
+          <param name="min">${JOINT_HAND_LOWER_LIMIT}</param>
+          <param name="max">${JOINT_HAND_UPPER_LIMIT}</param>
         </command_interface>
         <state_interface name="position"/>
         <state_interface name="velocity"/>
diff --git a/urdf/crane_x7.urdf.xacro b/urdf/crane_x7.urdf.xacro
index dc73331..86a1243 100644
--- a/urdf/crane_x7.urdf.xacro
+++ b/urdf/crane_x7.urdf.xacro
@@ -1,11 +1,6 @@
 <?xml version="1.0"?>
 
-<robot
-    name="crane_x7"
-    xmlns:xacro="http://ros.org/wiki/xacro"
-    xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
-    xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
-    xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface">
+<robot name="crane_x7" xmlns:xacro="http://ros.org/wiki/xacro">
 
   <!-- Include the Crane X7 generator macro -->
   <xacro:include filename="$(find crane_x7_description)/urdf/crane_x7.xacro"/>
@@ -23,19 +18,34 @@
   <xacro:arg name="manipulator_links_file_path" default="" />
   <xacro:arg name="gz_control_config_package" default="" />
   <xacro:arg name="gz_control_config_file_path" default="" />
-
-  <!-- Link to provide the location reference for the arm -->
-  <link name="base_link"/>
-
-  <!-- Used for fixing robot to Gazebo 'base_link' -->
-  <link name="world"/>
-
-  <joint name="base_link_to_world_joint" type="fixed">
-    <parent link="world"/>
-    <child link="base_link"/>
-  </joint>
-
-  <xacro:property name="M_PI" value="3.14159"/>
+  <xacro:property name="USE_D435" value="$(arg use_d435)"/>
+  <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="MANIPULATOR_LINKS_FILE_PATH" value="$(arg manipulator_links_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)"/>
+
+  <material name="black">
+    <color rgba="0.2 0.2 0.2 1.0"/>
+  </material>
+  <material name="white">
+    <color rgba="0.95 0.95 0.95 1.0"/>
+  </material>
+  <material name="silver">
+    <color rgba="0.75 0.75 0.75 1.0"/>
+  </material>
+  <material name="red">
+    <color rgba="1.0 0.3 0.3 1.0"/>
+  </material>
+  <material name="green">
+    <color rgba="0.3 1.0 0.3 1.0"/>
+  </material>
+  <material name="blue">
+    <color rgba="0.3 0.3 1.0 1.0"/>
+  </material>
 
   <xacro:property name="NAME_LINK_MOUNTING_PLATE" value="crane_x7_mounting_plate_link"/>
   <xacro:property name="NAME_LINK_BASE" value="crane_x7_shoulder_fixed_part_link"/>
@@ -110,170 +120,29 @@
   <xacro:property name="JOINT_HAND_LOWER_LIMIT" value="${radians(-5)}"/>
   <xacro:property name="JOINT_HAND_UPPER_LIMIT" value="${radians(90)}"/>
 
+  <!-- Used for fixing robot 'base_link' to Gazebo 'world' -->
+  <link name="world"/>
+  <link name="base_link"/>
+
+  <joint name="base_link_to_world_joint" type="fixed">
+    <parent link="world"/>
+    <child link="base_link"/>
+  </joint>
+
   <!-- Use the crane_x7 macro to configure a Crane X7 arm with desired joint
        velocities limit and lower/upper limits for the first joint. This
        allows accessing different operational areas, e.g. left-handed vs.
        right-handed robots. -->
-  <xacro:crane_x7
-    parent="base_link"
-    name_link_mounting_plate="${NAME_LINK_MOUNTING_PLATE}"
-    name_link_base="${NAME_LINK_BASE}" 
-    name_link_1="${NAME_LINK_1}"
-    name_link_2="${NAME_LINK_2}"
-    name_link_3="${NAME_LINK_3}"
-    name_link_4="${NAME_LINK_4}"
-    name_link_5="${NAME_LINK_5}"
-    name_link_6="${NAME_LINK_6}"
-    name_link_7="${NAME_LINK_7}"
-    name_link_cover_2_l="${NAME_LINK_COVER_2_L}"
-    name_link_cover_2_r="${NAME_LINK_COVER_2_R}"
-    name_link_cover_4_l="${NAME_LINK_COVER_4_L}"
-    name_link_cover_4_r="${NAME_LINK_COVER_4_R}"
-    name_link_logo_symbol_l="${NAME_LINK_LOGO_SYMBOL_L}"
-    name_link_logo_symbol_r="${NAME_LINK_LOGO_SYMBOL_R}"
-    name_link_logo_text_l="${NAME_LINK_LOGO_TEXT_L}"
-    name_link_logo_text_r="${NAME_LINK_LOGO_TEXT_R}"
-    name_link_gripper_base="${NAME_LINK_GRIPPER_BASE}"
-    name_link_gripper_finger_a="${NAME_LINK_GRIPPER_FINGER_A}"
-    name_link_gripper_finger_b="${NAME_LINK_GRIPPER_FINGER_B}"
-    name_joint_mounting_plate="${NAME_JOINT_MOUNTING_PLATE}"
-    name_joint_base="${NAME_JOINT_BASE}"
-    name_joint_1="${NAME_JOINT_1}"
-    name_joint_2="${NAME_JOINT_2}"
-    name_joint_3="${NAME_JOINT_3}"
-    name_joint_4="${NAME_JOINT_4}"
-    name_joint_5="${NAME_JOINT_5}"
-    name_joint_6="${NAME_JOINT_6}"
-    name_joint_7="${NAME_JOINT_7}"
-    name_joint_cover_2_l="${NAME_JOINT_COVER_2_L}"
-    name_joint_cover_2_r="${NAME_JOINT_COVER_2_R}"
-    name_joint_cover_4_l="${NAME_JOINT_COVER_4_L}"
-    name_joint_cover_4_r="${NAME_JOINT_COVER_4_R}"
-    name_joint_logo_symbol_l="${NAME_JOINT_LOGO_SYMBOL_L}"
-    name_joint_logo_symbol_r="${NAME_JOINT_LOGO_SYMBOL_R}"
-    name_joint_logo_text_l="${NAME_JOINT_LOGO_TEXT_L}"
-    name_joint_logo_text_r="${NAME_JOINT_LOGO_TEXT_R}"
-    name_joint_gripper_finger_a="${NAME_JOINT_GRIPPER_FINGER_A}"
-    name_joint_gripper_finger_b="${NAME_JOINT_GRIPPER_FINGER_B}"
-    color_link_base="${COLOR_LINK_BASE}"
-    color_link_1="${COLOR_LINK_1}"
-    color_link_cover_2="${COLOR_LINK_COVER_2}"
-    color_link_2="${COLOR_LINK_2}"
-    color_link_3="${COLOR_LINK_3}"
-    color_link_cover_4="${COLOR_LINK_COVER_4}"
-    color_link_4="${COLOR_LINK_4}"
-    color_link_5="${COLOR_LINK_5}"
-    color_link_6="${COLOR_LINK_6}"
-    color_hand="${COLOR_HAND}"
-    joint_velocity_limit="${JOINT_VELOCITY_LIMIT}"
-    joint_1_lower_limit="${JOINT_1_LOWER_LIMIT}"
-    joint_1_upper_limit="${JOINT_1_UPPER_LIMIT}"
-    joint_2_lower_limit="${JOINT_2_LOWER_LIMIT}"
-    joint_2_upper_limit="${JOINT_2_UPPER_LIMIT}"
-    joint_3_lower_limit="${JOINT_3_LOWER_LIMIT}"
-    joint_3_upper_limit="${JOINT_3_UPPER_LIMIT}"
-    joint_4_lower_limit="${JOINT_4_LOWER_LIMIT}"
-    joint_4_upper_limit="${JOINT_4_UPPER_LIMIT}"
-    joint_5_lower_limit="${JOINT_5_LOWER_LIMIT}"
-    joint_5_upper_limit="${JOINT_5_UPPER_LIMIT}"
-    joint_6_lower_limit="${JOINT_6_LOWER_LIMIT}"
-    joint_6_upper_limit="${JOINT_6_UPPER_LIMIT}"
-    joint_7_lower_limit="${JOINT_7_LOWER_LIMIT}"
-    joint_7_upper_limit="${JOINT_7_UPPER_LIMIT}"
-    joint_hand_lower_limit="${JOINT_HAND_LOWER_LIMIT}"
-    joint_hand_upper_limit="${JOINT_HAND_UPPER_LIMIT}"
-    use_d435="$(arg use_d435)">
-    <origin xyz="0 0 0"/>
+  <xacro:crane_x7 parent="base_link">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
   </xacro:crane_x7>
 
   <xacro:unless value="$(arg use_gazebo)">
-    <xacro:crane_x7_ros2_control_settings
-      name="crane_x7"
-      name_joint_1="${NAME_JOINT_1}"
-      name_joint_2="${NAME_JOINT_2}"
-      name_joint_3="${NAME_JOINT_3}"
-      name_joint_4="${NAME_JOINT_4}"
-      name_joint_5="${NAME_JOINT_5}"
-      name_joint_6="${NAME_JOINT_6}"
-      name_joint_7="${NAME_JOINT_7}"
-      name_joint_gripper_finger_a="${NAME_JOINT_GRIPPER_FINGER_A}"
-      joint_1_lower_limit="${JOINT_1_LOWER_LIMIT}"
-      joint_1_upper_limit="${JOINT_1_UPPER_LIMIT}"
-      joint_2_lower_limit="${JOINT_2_LOWER_LIMIT}"
-      joint_2_upper_limit="${JOINT_2_UPPER_LIMIT}"
-      joint_3_lower_limit="${JOINT_3_LOWER_LIMIT}"
-      joint_3_upper_limit="${JOINT_3_UPPER_LIMIT}"
-      joint_4_lower_limit="${JOINT_4_LOWER_LIMIT}"
-      joint_4_upper_limit="${JOINT_4_UPPER_LIMIT}"
-      joint_5_lower_limit="${JOINT_5_LOWER_LIMIT}"
-      joint_5_upper_limit="${JOINT_5_UPPER_LIMIT}"
-      joint_6_lower_limit="${JOINT_6_LOWER_LIMIT}"
-      joint_6_upper_limit="${JOINT_6_UPPER_LIMIT}"
-      joint_7_lower_limit="${JOINT_7_LOWER_LIMIT}"
-      joint_7_upper_limit="${JOINT_7_UPPER_LIMIT}"
-      joint_hand_lower_limit="${JOINT_HAND_LOWER_LIMIT}"
-      joint_hand_upper_limit="${JOINT_HAND_UPPER_LIMIT}" 
-      port_name="$(arg port_name)"
-      baudrate="$(arg baudrate)"
-      timeout_seconds="$(arg timeout_seconds)"
-      manipulator_config_file_path="$(arg manipulator_config_file_path)"
-      manipulator_links_file_path="$(arg manipulator_links_file_path)"
-      use_mock_components="$(arg use_mock_components)"
-      />
-
+    <xacro:crane_x7_ros2_control_settings/>
   </xacro:unless>
 
   <xacro:if value="$(arg use_gazebo)">
-    <xacro:gazebo_robot_settings
-      name_link_mounting_plate="${NAME_LINK_MOUNTING_PLATE}"
-      name_link_base="${NAME_LINK_BASE}"
-      name_link_1="${NAME_LINK_1}"
-      name_link_2="${NAME_LINK_2}"
-      name_link_3="${NAME_LINK_3}"
-      name_link_4="${NAME_LINK_4}"
-      name_link_5="${NAME_LINK_5}"
-      name_link_6="${NAME_LINK_6}"
-      name_link_cover_2_l="${NAME_LINK_COVER_2_L}"
-      name_link_cover_2_r="${NAME_LINK_COVER_2_R}"
-      name_link_cover_4_l="${NAME_LINK_COVER_4_L}"
-      name_link_cover_4_r="${NAME_LINK_COVER_4_R}"
-      name_link_logo_symbol_l="${NAME_LINK_LOGO_SYMBOL_L}"
-      name_link_logo_symbol_r="${NAME_LINK_LOGO_SYMBOL_R}"
-      name_link_logo_text_l="${NAME_LINK_LOGO_TEXT_L}"
-      name_link_logo_text_r="${NAME_LINK_LOGO_TEXT_R}"
-      name_link_gripper_base="${NAME_LINK_GRIPPER_BASE}"
-      name_link_gripper_finger_a="${NAME_LINK_GRIPPER_FINGER_A}"
-      name_link_gripper_finger_b="${NAME_LINK_GRIPPER_FINGER_B}" 
-      config_package="$(arg gz_control_config_package)" 
-      config_file_path="$(arg gz_control_config_file_path)" 
-      />
-
-    <xacro:crane_x7_gazebo_ros2_control_settings
-      name="crane_x7"
-      name_joint_1="${NAME_JOINT_1}"
-      name_joint_2="${NAME_JOINT_2}"
-      name_joint_3="${NAME_JOINT_3}"
-      name_joint_4="${NAME_JOINT_4}"
-      name_joint_5="${NAME_JOINT_5}"
-      name_joint_6="${NAME_JOINT_6}"
-      name_joint_7="${NAME_JOINT_7}"
-      name_joint_gripper_finger_a="${NAME_JOINT_GRIPPER_FINGER_A}"
-      name_joint_gripper_finger_b="${NAME_JOINT_GRIPPER_FINGER_B}"
-      joint_1_lower_limit="${JOINT_1_LOWER_LIMIT}"
-      joint_1_upper_limit="${JOINT_1_UPPER_LIMIT}"
-      joint_2_lower_limit="${JOINT_2_LOWER_LIMIT}"
-      joint_2_upper_limit="${JOINT_2_UPPER_LIMIT}"
-      joint_3_lower_limit="${JOINT_3_LOWER_LIMIT}"
-      joint_3_upper_limit="${JOINT_3_UPPER_LIMIT}"
-      joint_4_lower_limit="${JOINT_4_LOWER_LIMIT}"
-      joint_4_upper_limit="${JOINT_4_UPPER_LIMIT}"
-      joint_5_lower_limit="${JOINT_5_LOWER_LIMIT}"
-      joint_5_upper_limit="${JOINT_5_UPPER_LIMIT}"
-      joint_6_lower_limit="${JOINT_6_LOWER_LIMIT}"
-      joint_6_upper_limit="${JOINT_6_UPPER_LIMIT}"
-      joint_7_lower_limit="${JOINT_7_LOWER_LIMIT}"
-      joint_7_upper_limit="${JOINT_7_UPPER_LIMIT}"
-      joint_hand_lower_limit="${JOINT_HAND_LOWER_LIMIT}"
-      joint_hand_upper_limit="${JOINT_HAND_UPPER_LIMIT}" />
+    <xacro:gazebo_robot_settings/>
+    <xacro:crane_x7_gazebo_ros2_control_settings/>
   </xacro:if>
 </robot>
diff --git a/urdf/crane_x7.xacro b/urdf/crane_x7.xacro
index ffef7b0..e18ca07 100644
--- a/urdf/crane_x7.xacro
+++ b/urdf/crane_x7.xacro
@@ -6,173 +6,24 @@
   <xacro:include filename="$(find crane_x7_description)/urdf/crane_x7_wide_two_finger_gripper.xacro"/>
   <xacro:include filename="$(find crane_x7_description)/urdf/crane_x7_rt_logos.xacro"/>
 
-  <material name="black">
-    <color rgba="0.2 0.2 0.2 1.0"/>
-  </material>
-  <material name="white">
-    <color rgba="0.95 0.95 0.95 1.0"/>
-  </material>
-  <material name="silver">
-    <color rgba="0.75 0.75 0.75 1.0"/>
-  </material>
-  <material name="red">
-    <color rgba="1.0 0.3 0.3 1.0"/>
-  </material>
-  <material name="green">
-    <color rgba="0.3 1.0 0.3 1.0"/>
-  </material>
-  <material name="blue">
-    <color rgba="0.3 0.3 1.0 1.0"/>
-  </material>
-
-  <xacro:macro name="crane_x7"
-      params="parent
-              name_link_mounting_plate
-              name_link_base
-              name_link_1
-              name_link_2
-              name_link_3
-              name_link_4
-              name_link_5
-              name_link_6
-              name_link_7
-              name_link_cover_2_l
-              name_link_cover_2_r
-              name_link_cover_4_l
-              name_link_cover_4_r
-              name_link_logo_symbol_l
-              name_link_logo_symbol_r
-              name_link_logo_text_l
-              name_link_logo_text_r
-              name_link_gripper_base
-              name_link_gripper_finger_a
-              name_link_gripper_finger_b
-              name_joint_mounting_plate
-              name_joint_base
-              name_joint_1
-              name_joint_2
-              name_joint_3
-              name_joint_4
-              name_joint_5
-              name_joint_6
-              name_joint_7
-              name_joint_cover_2_l
-              name_joint_cover_2_r
-              name_joint_cover_4_l
-              name_joint_cover_4_r
-              name_joint_logo_symbol_l
-              name_joint_logo_symbol_r
-              name_joint_logo_text_l
-              name_joint_logo_text_r
-              name_joint_gripper_finger_a
-              name_joint_gripper_finger_b
-              color_link_base
-              color_link_1
-              color_link_cover_2
-              color_link_2
-              color_link_3
-              color_link_cover_4
-              color_link_4
-              color_link_5
-              color_link_6
-              color_hand
-              joint_velocity_limit
-              joint_1_lower_limit
-              joint_1_upper_limit
-              joint_2_lower_limit
-              joint_2_upper_limit
-              joint_3_lower_limit
-              joint_3_upper_limit
-              joint_4_lower_limit
-              joint_4_upper_limit
-              joint_5_lower_limit
-              joint_5_upper_limit
-              joint_6_lower_limit
-              joint_6_upper_limit
-              joint_7_lower_limit
-              joint_7_upper_limit
-              joint_hand_lower_limit
-              joint_hand_upper_limit
-              use_d435
-              *origin">
+  <xacro:macro name="crane_x7" params="parent *origin">
 
     <!-- Mounting plate
          (Root of the manipulator's TF tree) -->
-    <xacro:crane_x7_mounting_plate
-        parent="${parent}"
-        name_link_mounting_plate="${name_link_mounting_plate}"
-        name_joint_mounting_plate="${name_joint_mounting_plate}"
-        color="black">
+    <xacro:crane_x7_mounting_plate parent="${parent}" color="black">
       <xacro:insert_block name="origin"/>
     </xacro:crane_x7_mounting_plate>
 
     <!-- Arm -->
-    <xacro:crane_x7_arm
-        parent="${name_link_mounting_plate}"
-        name_link_base="${name_link_base}"
-        name_link_1="${name_link_1}"
-        name_link_2="${name_link_2}"
-        name_link_3="${name_link_3}"
-        name_link_4="${name_link_4}"
-        name_link_5="${name_link_5}"
-        name_link_6="${name_link_6}"
-        name_link_7="${name_link_7}"
-        name_link_cover_2_l="${name_link_cover_2_l}"
-        name_link_cover_2_r="${name_link_cover_2_r}"
-        name_link_cover_4_l="${name_link_cover_4_l}"
-        name_link_cover_4_r="${name_link_cover_4_r}"
-        name_link_logo_symbol_l="${name_link_logo_symbol_l}"
-        name_link_logo_symbol_r="${name_link_logo_symbol_r}"
-        name_link_logo_text_l="${name_link_logo_text_l}"
-        name_link_logo_text_r="${name_link_logo_text_r}"
-        name_joint_base="${name_joint_base}"
-        name_joint_1="${name_joint_1}"
-        name_joint_2="${name_joint_2}"
-        name_joint_3="${name_joint_3}"
-        name_joint_4="${name_joint_4}"
-        name_joint_5="${name_joint_5}"
-        name_joint_6="${name_joint_6}"
-        name_joint_7="${name_joint_7}"
-        name_joint_cover_2_l="${name_joint_cover_2_l}"
-        name_joint_cover_2_r="${name_joint_cover_2_r}"
-        name_joint_cover_4_l="${name_joint_cover_4_l}"
-        name_joint_cover_4_r="${name_joint_cover_4_r}"
-        name_joint_logo_symbol_l="${name_joint_logo_symbol_l}"
-        name_joint_logo_symbol_r="${name_joint_logo_symbol_r}"
-        name_joint_logo_text_l="${name_joint_logo_text_l}"
-        name_joint_logo_text_r="${name_joint_logo_text_r}"
-        color_link_base="${color_link_base}"
-        color_link_1="${color_link_1}"
-        color_link_cover_2="${color_link_cover_2}"
-        color_link_2="${color_link_2}"
-        color_link_3="${color_link_3}"
-        color_link_cover_4="${color_link_cover_4}"
-        color_link_4="${color_link_4}"
-        color_link_5="${color_link_5}"
-        color_link_6="${color_link_6}"
-        joint_velocity_limit="${joint_velocity_limit}"
-        joint_1_lower_limit="${joint_1_lower_limit}"
-        joint_1_upper_limit="${joint_1_upper_limit}"
-        joint_2_lower_limit="${joint_2_lower_limit}"
-        joint_2_upper_limit="${joint_2_upper_limit}"
-        joint_3_lower_limit="${joint_3_lower_limit}"
-        joint_3_upper_limit="${joint_3_upper_limit}"
-        joint_4_lower_limit="${joint_4_lower_limit}"
-        joint_4_upper_limit="${joint_4_upper_limit}"
-        joint_5_lower_limit="${joint_5_lower_limit}"
-        joint_5_upper_limit="${joint_5_upper_limit}"
-        joint_6_lower_limit="${joint_6_lower_limit}"
-        joint_6_upper_limit="${joint_6_upper_limit}"
-        joint_7_lower_limit="${joint_7_lower_limit}"
-        joint_7_upper_limit="${joint_7_upper_limit}">
+    <xacro:crane_x7_arm parent="${NAME_LINK_MOUNTING_PLATE}">
       <origin xyz="0 0 ${MOUNTING_PLATE_THICKNESS}" rpy="0 0 0"/>
     </xacro:crane_x7_arm>
 
     <!-- RealSense D435 -->
-    <xacro:if value="$(arg use_d435)">
+    <xacro:if value="${USE_D435}">
       <xacro:include filename="$(find realsense2_description)/urdf/_d435.urdf.xacro" />
       <xacro:sensor_d435
-          parent="${name_link_7}"
+          parent="${NAME_LINK_7}"
           use_nominal_extrinsics="false"
           add_plug="false"
           use_mesh="true">
@@ -181,15 +32,6 @@
     </xacro:if>
 
     <!-- End effector -->
-    <xacro:crane_x7_wide_two_finger_gripper
-        name_link_gripper_base="${name_link_gripper_base}"
-        name_link_gripper_finger_a="${name_link_gripper_finger_a}"
-        name_link_gripper_finger_b="${name_link_gripper_finger_b}"
-        name_joint_gripper_finger_a="${name_joint_gripper_finger_a}"
-        name_joint_gripper_finger_b="${name_joint_gripper_finger_b}"
-        color="${color_hand}"
-        joint_velocity_limit="${joint_velocity_limit}"
-        joint_hand_lower_limit="${joint_hand_lower_limit}"
-        joint_hand_upper_limit="${joint_hand_upper_limit}"/>
+    <xacro:crane_x7_wide_two_finger_gripper/>
   </xacro:macro>
 </robot>
diff --git a/urdf/crane_x7_arm.xacro b/urdf/crane_x7_arm.xacro
index 5cecfac..5202b42 100644
--- a/urdf/crane_x7_arm.xacro
+++ b/urdf/crane_x7_arm.xacro
@@ -1,86 +1,26 @@
 <?xml version="1.0"?>
 
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
-  <xacro:property name="SCALE_MM" value="0.001"/>
-  <xacro:property name="SCALE_CM" value="0.01"/>
 
   <!-- Macro for the standard CRANE-X7 configuration -->
-  <xacro:macro name="crane_x7_arm"
-      params="parent
-              name_link_base
-              name_link_1
-              name_link_2
-              name_link_3
-              name_link_4
-              name_link_5
-              name_link_6
-              name_link_7
-              name_link_cover_2_l
-              name_link_cover_2_r
-              name_link_cover_4_l
-              name_link_cover_4_r
-              name_link_logo_symbol_l
-              name_link_logo_symbol_r
-              name_link_logo_text_l
-              name_link_logo_text_r
-              name_joint_base
-              name_joint_1
-              name_joint_2
-              name_joint_3
-              name_joint_4
-              name_joint_5
-              name_joint_6
-              name_joint_7
-              name_joint_cover_2_l
-              name_joint_cover_2_r
-              name_joint_cover_4_l
-              name_joint_cover_4_r
-              name_joint_logo_symbol_l
-              name_joint_logo_symbol_r
-              name_joint_logo_text_l
-              name_joint_logo_text_r
-              color_link_base
-              color_link_1
-              color_link_cover_2
-              color_link_2
-              color_link_3
-              color_link_cover_4
-              color_link_4
-              color_link_5
-              color_link_6
-              joint_velocity_limit
-              joint_1_lower_limit
-              joint_1_upper_limit
-              joint_2_lower_limit
-              joint_2_upper_limit
-              joint_3_lower_limit
-              joint_3_upper_limit
-              joint_4_lower_limit
-              joint_4_upper_limit
-              joint_5_lower_limit
-              joint_5_upper_limit
-              joint_6_lower_limit
-              joint_6_upper_limit
-              joint_7_lower_limit
-              joint_7_upper_limit
-              *origin">
+  <xacro:macro name="crane_x7_arm" params="parent *origin">
 
     <!-- crane_x7_joint_base -->
-    <joint name="${name_joint_base}" type="fixed">
+    <joint name="${NAME_JOINT_BASE}" type="fixed">
       <xacro:insert_block name="origin"/>
       <parent link="${parent}"/>
-      <child link="${name_link_base}"/>
+      <child link="${NAME_LINK_BASE}"/>
     </joint>
 
     <!-- crane_x7_link_base -->
-    <link name="${name_link_base}">
+    <link name="${NAME_LINK_BASE}">
       <visual>
         <origin xyz="0 0 0" rpy="0 0 0"/>
         <geometry>
           <mesh filename="package://crane_x7_description/meshes/visual/base_fixed_part.stl"
               scale="1 1 1"/>
         </geometry>
-        <material name="${color_link_base}"/>
+        <material name="${COLOR_LINK_BASE}"/>
       </visual>
 
       <collision>
@@ -101,28 +41,33 @@
     </link>
 
     <!-- crane_x7_joint_1 -->
-    <joint name="${name_joint_1}" type="revolute">
+    <joint name="${NAME_JOINT_1}" type="revolute">
       <origin xyz="0 0 0.036" rpy="0 0 0"/>
       <axis xyz="0 0 1"/>
-      <limit effort="10.0" velocity="${joint_velocity_limit}" lower="${joint_1_lower_limit}" upper="${joint_1_upper_limit}"/>
-      <parent link="${name_link_base}"/>
-      <child link="${name_link_1}"/>
+      <limit
+        effort="10.0"
+        velocity="${JOINT_VELOCITY_LIMIT}"
+        lower="${JOINT_1_LOWER_LIMIT}"
+        upper="${JOINT_1_UPPER_LIMIT}"
+        />
+      <parent link="${NAME_LINK_BASE}"/>
+      <child link="${NAME_LINK_1}"/>
       <dynamics damping="1.0e-6" friction="2.0e-1"/>
     </joint>
 
     <!-- crane_x7_link_1 -->
-    <link name="${name_link_1}">
+    <link name="${NAME_LINK_1}">
       <visual>
-        <origin xyz="0 0 0" rpy="0 0 ${M_PI}"/>
+        <origin xyz="0 0 0" rpy="0 0 ${radians(180)}"/>
         <geometry>
           <mesh filename="package://crane_x7_description/meshes/visual/base_revolute_part.stl"
               scale="1 1 1"/>
         </geometry>
-        <material name="${color_link_1}"/>
+        <material name="${COLOR_LINK_1}"/>
       </visual>
 
       <collision>
-        <!--<origin xyz="0 0 0" rpy="0 0 ${M_PI}"/>-->
+        <!--<origin xyz="0 0 0" rpy="0 0 ${radians(180)}"/>-->
         <!--<geometry>-->
           <!--<mesh filename="package://crane_x7_description/meshes/collision/base_revolute_part.stl"-->
               <!--scale="1 1 1"/>-->
@@ -133,7 +78,7 @@
         </geometry>
       </collision>
       <collision>
-        <origin xyz="0 0 0.064" rpy="${M_PI/2} 0 0"/>
+        <origin xyz="0 0 0.064" rpy="${radians(90)} 0 0"/>
         <geometry>
           <cylinder radius="0.034" length="0.0491"/>
         </geometry>
@@ -149,28 +94,33 @@
     </link>
 
     <!-- crane_x7_joint_2 -->
-    <joint name="${name_joint_2}" type="revolute">
+    <joint name="${NAME_JOINT_2}" type="revolute">
       <origin xyz="0 0 0.064" rpy="0 0 0"/>
       <axis xyz="0 -1 0"/>
-      <limit effort="10.0" velocity="${joint_velocity_limit}" lower="${joint_2_lower_limit}" upper="${joint_2_upper_limit}"/>
-      <parent link="${name_link_1}"/>
-      <child link="${name_link_2}"/>
+      <limit
+        effort="10.0"
+        velocity="${JOINT_VELOCITY_LIMIT}"
+        lower="${JOINT_2_LOWER_LIMIT}"
+        upper="${JOINT_2_UPPER_LIMIT}"
+        />
+      <parent link="${NAME_LINK_1}"/>
+      <child link="${NAME_LINK_2}"/>
       <dynamics damping="1.0e-6" friction="2.0e-1"/>
     </joint>
 
     <!-- crane_x7_link_2 -->
-    <link name="${name_link_2}">
+    <link name="${NAME_LINK_2}">
       <visual>
-        <origin xyz="0 0 0" rpy="${M_PI/2} 0 0"/>
+        <origin xyz="0 0 0" rpy="${radians(90)} 0 0"/>
         <geometry>
           <mesh filename="package://crane_x7_description/meshes/visual/rotating_link_type_1_fixed_part.stl"
               scale="1 1 1"/>
         </geometry>
-        <material name="${color_link_2}"/>
+        <material name="${COLOR_LINK_2}"/>
       </visual>
 
       <collision>
-        <origin xyz="0 0 0" rpy="${M_PI/2} 0 0"/>
+        <origin xyz="0 0 0" rpy="${radians(90)} 0 0"/>
         <geometry>
           <mesh filename="package://crane_x7_description/meshes/collision/rotating_link_type_1_fixed_part.stl"
               scale="1 1 1"/>
@@ -179,7 +129,7 @@
 
       <inertial>
         <mass value="0.136"/>
-        <origin xyz="3.10E-05 1.90E-05 3.42E-02" rpy="${M_PI/2} 0 0"/>
+        <origin xyz="3.10E-05 1.90E-05 3.42E-02" rpy="${radians(90)} 0 0"/>
         <inertia ixx="1.68E-04" ixy="2.05E-08" ixz="-4.00E-07"
           iyy="9.62E-05" iyz="6.24E-08"
           izz="1.34E-04"/>
@@ -187,25 +137,25 @@
     </link>
 
     <!-- crane_x7_joint_cover_2_l -->
-    <joint name="${name_joint_cover_2_l}" type="fixed">
-      <parent link="${name_link_2}"/>
-      <child link="${name_link_cover_2_l}"/>
+    <joint name="${NAME_JOINT_COVER_2_L}" type="fixed">
+      <parent link="${NAME_LINK_2}"/>
+      <child link="${NAME_LINK_COVER_2_L}"/>
       <origin xyz="0 0.0316 0" rpy="0 0 0"/>
     </joint>
 
     <!-- crane_x7_link_cover_2_l -->
-    <link name="${name_link_cover_2_l}">
+    <link name="${NAME_LINK_COVER_2_L}">
       <visual>
-        <origin xyz="0 0 0" rpy="-${M_PI/2} 0 0"/>
+        <origin xyz="0 0 0" rpy="${radians(-90)} 0 0"/>
         <geometry>
           <mesh filename="package://crane_x7_description/meshes/visual/joint_cover.stl"
               scale="1 1 1"/>
         </geometry>
-        <material name="${color_link_cover_2}"/>
+        <material name="${COLOR_LINK_COVER_2}"/>
       </visual>
 
       <collision>
-        <origin xyz="0 0 0" rpy="-${M_PI/2} 0 0"/>
+        <origin xyz="0 0 0" rpy="${radians(-90)} 0 0"/>
         <geometry>
           <mesh filename="package://crane_x7_description/meshes/visual/joint_cover.stl"
               scale="1 1 1"/>
@@ -214,25 +164,25 @@
     </link>
 
     <!-- crane_x7_joint_cover_2_r -->
-    <joint name="${name_joint_cover_2_r}" type="fixed">
-      <parent link="${name_link_2}"/>
-      <child link="${name_link_cover_2_r}"/>
+    <joint name="${NAME_JOINT_COVER_2_R}" type="fixed">
+      <parent link="${NAME_LINK_2}"/>
+      <child link="${NAME_LINK_COVER_2_R}"/>
       <origin xyz="0 -0.0316 0" rpy="0 0 0"/>
     </joint>
 
     <!-- crane_x7_link_cover_2_r -->
-    <link name="${name_link_cover_2_r}">
+    <link name="${NAME_LINK_COVER_2_R}">
       <visual>
-        <origin xyz="0 0 0" rpy="${M_PI/2} 0 0"/>
+        <origin xyz="0 0 0" rpy="${radians(90)} 0 0"/>
         <geometry>
           <mesh filename="package://crane_x7_description/meshes/visual/joint_cover.stl"
               scale="1 1 1"/>
         </geometry>
-        <material name="${color_link_cover_2}"/>
+        <material name="${COLOR_LINK_COVER_2}"/>
       </visual>
 
       <collision>
-        <origin xyz="0 0 0" rpy="${M_PI/2} 0 0"/>
+        <origin xyz="0 0 0" rpy="${radians(90)} 0 0"/>
         <geometry>
           <mesh filename="package://crane_x7_description/meshes/visual/joint_cover.stl"
               scale="1 1 1"/>
@@ -241,27 +191,29 @@
     </link>
 
     <!-- crane_x7_joint_3 -->
-    <joint name="${name_joint_3}" type="revolute">
+    <joint name="${NAME_JOINT_3}" type="revolute">
       <origin xyz="0 0 0.065" rpy="0 0 0"/>
       <axis xyz="0 0 1"/>
-      <limit effort="4.0"
-          velocity="${joint_velocity_limit}"
-          lower="${joint_3_lower_limit}"
-          upper="${joint_3_upper_limit}"/>
-      <parent link="${name_link_2}"/>
-      <child link="${name_link_3}"/>
+      <limit
+        effort="4.0"
+        velocity="${JOINT_VELOCITY_LIMIT}"
+        lower="${JOINT_3_LOWER_LIMIT}"
+        upper="${JOINT_3_UPPER_LIMIT}"
+        />
+      <parent link="${NAME_LINK_2}"/>
+      <child link="${NAME_LINK_3}"/>
       <dynamics damping="1.0e-6" friction="2.0e-1"/>
     </joint>
 
     <!-- crane_x7_link_3 -->
-    <link name="${name_link_3}">
+    <link name="${NAME_LINK_3}">
       <visual>
         <origin xyz="0 0 0" rpy="0 0 0"/>
         <geometry>
           <mesh filename="package://crane_x7_description/meshes/visual/rotating_link_type_1_revolute_part.stl"
               scale="1 1 1"/>
         </geometry>
-        <material name="${color_link_3}"/>
+        <material name="${COLOR_LINK_3}"/>
       </visual>
 
       <collision>
@@ -282,30 +234,32 @@
     </link>
 
     <!-- crane_x7_joint_4 -->
-    <joint name="${name_joint_4}" type="revolute">
+    <joint name="${NAME_JOINT_4}" type="revolute">
       <origin xyz="0 0 0.185" rpy="0 0 0"/>
       <axis xyz="0 -1 0"/>
-      <limit effort="4.0"
-          velocity="${joint_velocity_limit}"
-          lower="${joint_4_lower_limit}"
-          upper="${joint_4_upper_limit}"/>
-      <parent link="${name_link_3}"/>
-      <child link="${name_link_4}"/>
+      <limit
+        effort="4.0"
+        velocity="${JOINT_VELOCITY_LIMIT}"
+        lower="${JOINT_4_LOWER_LIMIT}"
+        upper="${JOINT_4_UPPER_LIMIT}"
+        />
+      <parent link="${NAME_LINK_3}"/>
+      <child link="${NAME_LINK_4}"/>
       <dynamics damping="1.0e-6" friction="2.0e-1"/>
     </joint>
     <!-- crane_x7_link_4 -->
-    <link name="${name_link_4}">
+    <link name="${NAME_LINK_4}">
       <visual>
-        <origin xyz="0 0 0" rpy="${M_PI/2} 0 0"/>
+        <origin xyz="0 0 0" rpy="${radians(90)} 0 0"/>
         <geometry>
           <mesh filename="package://crane_x7_description/meshes/visual/rotating_link_type_2_fixed_part.stl"
               scale="1 1 1"/>
         </geometry>
-        <material name="${color_link_4}"/>
+        <material name="${COLOR_LINK_4}"/>
       </visual>
 
       <collision>
-        <origin xyz="0 0 0" rpy="${M_PI/2} 0 0"/>
+        <origin xyz="0 0 0" rpy="${radians(90)} 0 0"/>
         <geometry>
           <mesh filename="package://crane_x7_description/meshes/collision/rotating_link_type_2_fixed_part.stl"
               scale="1 1 1"/>
@@ -314,7 +268,7 @@
 
       <inertial>
         <mass value="0.222"/>
-        <origin xyz="-9.39E-03 6.40E-05 8.04E-02" rpy="${M_PI/2} 0 0"/>
+        <origin xyz="-9.39E-03 6.40E-05 8.04E-02" rpy="${radians(90)} 0 0"/>
         <inertia ixx="4.89E-04" ixy="-3.90E-06" ixz="8.29E-08"
           iyy="1.14E-04" iyz="2.82E-06"
           izz="5.03E-04"/>
@@ -322,25 +276,25 @@
     </link>
 
     <!-- crane_x7_joint_cover_4_l -->
-    <joint name="${name_joint_cover_4_l}" type="fixed">
-      <parent link="${name_link_4}"/>
-      <child link="${name_link_cover_4_l}"/>
+    <joint name="${NAME_JOINT_COVER_4_L}" type="fixed">
+      <parent link="${NAME_LINK_4}"/>
+      <child link="${NAME_LINK_COVER_4_L}"/>
       <origin xyz="0 0.025 0" rpy="0 0 0"/>
     </joint>
 
     <!-- crane_x7_link_cover_4_l -->
-    <link name="${name_link_cover_4_l}">
+    <link name="${NAME_LINK_COVER_4_L}">
       <visual>
-        <origin xyz="0 0 0" rpy="-${M_PI/2} 0 0"/>
+        <origin xyz="0 0 0" rpy="${radians(-90)} 0 0"/>
         <geometry>
           <mesh filename="package://crane_x7_description/meshes/visual/joint_cover.stl"
               scale="1 1 1"/>
         </geometry>
-        <material name="${color_link_cover_4}"/>
+        <material name="${COLOR_LINK_COVER_4}"/>
       </visual>
 
       <collision>
-        <origin xyz="0 0 0" rpy="-${M_PI/2} 0 0"/>
+        <origin xyz="0 0 0" rpy="${radians(-90)} 0 0"/>
         <geometry>
           <mesh filename="package://crane_x7_description/meshes/visual/joint_cover.stl"
               scale="1 1 1"/>
@@ -349,25 +303,25 @@
     </link>
 
     <!-- crane_x7_joint_cover_4_r -->
-    <joint name="${name_joint_cover_4_r}" type="fixed">
-      <parent link="${name_link_4}"/>
-      <child link="${name_link_cover_4_r}"/>
+    <joint name="${NAME_JOINT_COVER_4_R}" type="fixed">
+      <parent link="${NAME_LINK_4}"/>
+      <child link="${NAME_LINK_COVER_4_R}"/>
       <origin xyz="0 -0.025 0" rpy="0 0 0"/>
     </joint>
 
     <!-- crane_x7_link_cover_4_r -->
-    <link name="${name_link_cover_4_r}">
+    <link name="${NAME_LINK_COVER_4_R}">
       <visual>
-        <origin xyz="0 0 0" rpy="${M_PI/2} 0 0"/>
+        <origin xyz="0 0 0" rpy="${radians(90)} 0 0"/>
         <geometry>
           <mesh filename="package://crane_x7_description/meshes/visual/joint_cover.stl"
               scale="1 1 1"/>
         </geometry>
-        <material name="${color_link_cover_4}"/>
+        <material name="${COLOR_LINK_COVER_4}"/>
       </visual>
 
       <collision>
-        <origin xyz="0 0 0" rpy="${M_PI/2} 0 0"/>
+        <origin xyz="0 0 0" rpy="${radians(90)} 0 0"/>
         <geometry>
           <mesh filename="package://crane_x7_description/meshes/visual/joint_cover.stl"
               scale="1 1 1"/>
@@ -376,24 +330,29 @@
     </link>
 
     <!-- crane_x7_joint_5 -->
-    <joint name="${name_joint_5}" type="revolute">
+    <joint name="${NAME_JOINT_5}" type="revolute">
       <origin xyz="0 0 0.121" rpy="0 0 0"/>
       <axis xyz="0 0 1"/>
-      <limit effort="4.0" velocity="${joint_velocity_limit}" lower="${joint_5_lower_limit}" upper="${joint_5_upper_limit}"/>
-      <parent link="${name_link_4}"/>
-      <child link="${name_link_5}"/>
+      <limit
+        effort="4.0"
+        velocity="${JOINT_VELOCITY_LIMIT}"
+        lower="${JOINT_5_LOWER_LIMIT}"
+        upper="${JOINT_5_UPPER_LIMIT}"
+        />
+      <parent link="${NAME_LINK_4}"/>
+      <child link="${NAME_LINK_5}"/>
       <dynamics damping="1.0e-6" friction="2.0e-1"/>
     </joint>
 
     <!-- crane_x7_link_5 -->
-    <link name="${name_link_5}">
+    <link name="${NAME_LINK_5}">
       <visual>
         <origin xyz="0 0 0" rpy="0 0 0"/>
         <geometry>
           <mesh filename="package://crane_x7_description/meshes/visual/rotating_link_type_2_revolute_part.stl"
               scale="1 1 1"/>
         </geometry>
-        <material name="${color_link_5}"/>
+        <material name="${COLOR_LINK_5}"/>
       </visual>
 
       <collision>
@@ -414,40 +373,36 @@
     </link>
 
     <!-- logo -->
-    <xacro:crane_x7_logos 
-      parent="${name_link_5}"
-      name_link_logo_symbol_l="${name_link_logo_symbol_l}"
-      name_link_logo_symbol_r="${name_link_logo_symbol_r}"
-      name_link_logo_text_l="${name_link_logo_text_l}"
-      name_link_logo_text_r="${name_link_logo_text_r}"
-      name_joint_logo_symbol_l="${name_joint_logo_symbol_l}"
-      name_joint_logo_symbol_r="${name_joint_logo_symbol_r}"
-      name_joint_logo_text_l="${name_joint_logo_text_l}"
-      name_joint_logo_text_r="${name_joint_logo_text_r}"/>
+    <xacro:crane_x7_logos parent="${NAME_LINK_5}"/>
 
     <!-- crane_x7_joint_6 -->
-    <joint name="${name_joint_6}" type="revolute">
+    <joint name="${NAME_JOINT_6}" type="revolute">
       <origin xyz="0 0 0.129" rpy="0 0 0"/>
       <axis xyz="0 -1 0"/>
-      <limit effort="4.0" velocity="${joint_velocity_limit}" lower="${joint_6_lower_limit}" upper="${joint_6_upper_limit}"/>
-      <parent link="${name_link_5}"/>
-      <child link="${name_link_6}"/>
+      <limit
+        effort="4.0"
+        velocity="${JOINT_VELOCITY_LIMIT}"
+        lower="${JOINT_6_LOWER_LIMIT}"
+        upper="${JOINT_6_UPPER_LIMIT}"
+        />
+      <parent link="${NAME_LINK_5}"/>
+      <child link="${NAME_LINK_6}"/>
       <dynamics damping="1.0e-6" friction="2.0e-1"/>
     </joint>
 
     <!-- crane_x7_link_6 -->
-    <link name="${name_link_6}">
+    <link name="${NAME_LINK_6}">
       <visual>
-        <origin xyz="0 0 0" rpy="${M_PI/2} 0 0"/>
+        <origin xyz="0 0 0" rpy="${radians(90)} 0 0"/>
         <geometry>
           <mesh filename="package://crane_x7_description/meshes/visual/wrist.stl"
               scale="1 1 1"/>
         </geometry>
-        <material name="${color_link_6}"/>
+        <material name="${COLOR_LINK_6}"/>
       </visual>
 
       <collision>
-        <origin xyz="0 0 0" rpy="${M_PI/2} 0 0"/>
+        <origin xyz="0 0 0" rpy="${radians(90)} 0 0"/>
         <geometry>
           <mesh filename="package://crane_x7_description/meshes/collision/wrist.stl"
               scale="1 1 1"/>
@@ -456,7 +411,7 @@
 
       <inertial>
         <mass value="0.14"/>
-        <origin xyz="6.10E-03 8.26E-04 -3.55E-03" rpy="${M_PI/2} 0 0"/>
+        <origin xyz="6.10E-03 8.26E-04 -3.55E-03" rpy="${radians(90)} 0 0"/>
         <inertia ixx="4.01E-05" ixy="-2.91E-06" ixz="6.39E-07"
           iyy="6.13E-05" iyz="-4.90E-07"
           izz="7.35E-05"/>
@@ -464,12 +419,17 @@
     </link>
 
     <!-- crane_x7_joint_7 -->
-    <joint name="${name_joint_7}" type="revolute">
+    <joint name="${NAME_JOINT_7}" type="revolute">
       <origin xyz="0 0 0.019" rpy="0 0 0"/>
       <axis xyz="0 0 1"/>
-      <limit effort="4.0" velocity="${joint_velocity_limit}" lower="${joint_7_lower_limit}" upper="${joint_7_upper_limit}"/>
-      <parent link="${name_link_6}"/>
-      <child link="${name_link_7}"/>
+      <limit
+        effort="4.0"
+        velocity="${JOINT_VELOCITY_LIMIT}"
+        lower="${JOINT_7_LOWER_LIMIT}"
+        upper="${JOINT_7_UPPER_LIMIT}"
+        />
+      <parent link="${NAME_LINK_6}"/>
+      <child link="${NAME_LINK_7}"/>
       <dynamics damping="1.0e-6" friction="2.0e-1"/>
     </joint>
   </xacro:macro>
diff --git a/urdf/crane_x7_mounting_plate.xacro b/urdf/crane_x7_mounting_plate.xacro
index fc7804b..72dd542 100644
--- a/urdf/crane_x7_mounting_plate.xacro
+++ b/urdf/crane_x7_mounting_plate.xacro
@@ -1,25 +1,17 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
-  <xacro:property name="M_PI" value="3.14159"/>
-  <xacro:property name="SCALE_MM" value="0.001"/>
-  <xacro:property name="SCALE_CM" value="0.01"/>
 
   <xacro:property name="MOUNTING_PLATE_THICKNESS" value="0.005"/>
 
-  <xacro:macro name="crane_x7_mounting_plate"
-      params="parent
-              name_link_mounting_plate
-              name_joint_mounting_plate
-              color
-              *origin">
+  <xacro:macro name="crane_x7_mounting_plate" params="parent color *origin">
 
-    <joint name="${name_joint_mounting_plate}" type="fixed">
+    <joint name="${NAME_JOINT_MOUNTING_PLATE}" type="fixed">
       <xacro:insert_block name="origin"/>
       <parent link="${parent}"/>
-      <child link="${name_link_mounting_plate}"/>
+      <child link="${NAME_LINK_MOUNTING_PLATE}"/>
     </joint>
 
-    <link name="${name_link_mounting_plate}">
+    <link name="${NAME_LINK_MOUNTING_PLATE}">
       <visual>
         <origin xyz="0 0 0" rpy="0 0 0"/>
         <geometry>
diff --git a/urdf/crane_x7_rt_logos.xacro b/urdf/crane_x7_rt_logos.xacro
index 64157df..c0acda9 100644
--- a/urdf/crane_x7_rt_logos.xacro
+++ b/urdf/crane_x7_rt_logos.xacro
@@ -1,21 +1,12 @@
 <?xml version="1.0"?>
 
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
-  <xacro:macro name="crane_x7_logos"
-      params="parent
-              name_link_logo_symbol_l
-              name_link_logo_symbol_r
-              name_link_logo_text_l
-              name_link_logo_text_r
-              name_joint_logo_symbol_l
-              name_joint_logo_symbol_r
-              name_joint_logo_text_l
-              name_joint_logo_text_r">
+  <xacro:macro name="crane_x7_logos" params="parent">
 
     <!-- Left side logo text -->
-    <link name="${name_link_logo_text_l}">
+    <link name="${NAME_LINK_LOGO_TEXT_L}">
       <visual>
-        <origin xyz="0 0 0" rpy="-${M_PI/2} ${M_PI/2} 0"/>
+        <origin xyz="0 0 0" rpy="${radians(-90)} ${radians(90)} 0"/>
         <geometry>
           <mesh filename="package://crane_x7_description/meshes/visual/rtcorp_logo_name.stl"
               scale="1 1 1"/>
@@ -24,23 +15,23 @@
       </visual>
 
       <collision>
-        <origin xyz="0 0 0" rpy="-${M_PI/2} ${M_PI/2} 0"/>
+        <origin xyz="0 0 0" rpy="${radians(-90)} ${radians(90)} 0"/>
         <geometry>
           <mesh filename="package://crane_x7_description/meshes/visual/rtcorp_logo_name.stl"
               scale="1 1 1"/>
         </geometry>
       </collision>
     </link>
-    <joint name="${name_joint_logo_text_l}" type="fixed">
+    <joint name="${NAME_JOINT_LOGO_TEXT_L}" type="fixed">
       <parent link="${parent}"/>
-      <child link="${name_link_logo_text_l}"/>
+      <child link="${NAME_LINK_LOGO_TEXT_L}"/>
       <origin xyz="0 0.0345 0.095" rpy="0 0 0"/>
     </joint>
 
     <!-- Left side logo symbol -->
-    <link name="${name_link_logo_symbol_l}">
+    <link name="${NAME_LINK_LOGO_SYMBOL_L}">
       <visual>
-        <origin xyz="0 0 0" rpy="-${M_PI/2} ${M_PI/2} 0"/>
+        <origin xyz="0 0 0" rpy="${radians(-90)} ${radians(90)} 0"/>
         <geometry>
           <mesh filename="package://crane_x7_description/meshes/visual/rtcorp_logo_rabbit.stl"
               scale="1 1 1"/>
@@ -49,23 +40,23 @@
       </visual>
 
       <collision>
-        <origin xyz="0 0 0" rpy="-${M_PI/2} ${M_PI/2} 0"/>
+        <origin xyz="0 0 0" rpy="${radians(-90)} ${radians(90)} 0"/>
         <geometry>
           <mesh filename="package://crane_x7_description/meshes/visual/rtcorp_logo_rabbit.stl"
               scale="1 1 1"/>
         </geometry>
       </collision>
     </link>
-    <joint name="${name_joint_logo_symbol_l}" type="fixed">
+    <joint name="${NAME_JOINT_LOGO_SYMBOL_L}" type="fixed">
       <parent link="${parent}"/>
-      <child link="${name_link_logo_symbol_l}"/>
+      <child link="${NAME_LINK_LOGO_SYMBOL_L}"/>
       <origin xyz="0 0.0345 0.132" rpy="0 0 0"/>
     </joint>
 
     <!-- Right side logo text -->
-    <link name="${name_link_logo_text_r}">
+    <link name="${NAME_LINK_LOGO_TEXT_R}">
       <visual>
-        <origin xyz="0 0 0" rpy="${M_PI/2} -${M_PI/2} 0"/>
+        <origin xyz="0 0 0" rpy="${radians(90)} ${radians(-90)} 0"/>
         <geometry>
           <mesh filename="package://crane_x7_description/meshes/visual/rtcorp_logo_name.stl"
               scale="1 1 1"/>
@@ -74,23 +65,23 @@
       </visual>
 
       <collision>
-        <origin xyz="0 0 0" rpy="${M_PI/2} -${M_PI/2} 0"/>
+        <origin xyz="0 0 0" rpy="${radians(90)} ${radians(-90)} 0"/>
         <geometry>
           <mesh filename="package://crane_x7_description/meshes/visual/rtcorp_logo_name.stl"
               scale="1 1 1"/>
         </geometry>
       </collision>
     </link>
-    <joint name="${name_joint_logo_text_r}" type="fixed">
+    <joint name="${NAME_JOINT_LOGO_TEXT_R}" type="fixed">
       <parent link="${parent}"/>
-      <child link="${name_link_logo_text_r}"/>
+      <child link="${NAME_LINK_LOGO_TEXT_R}"/>
       <origin xyz="0 -0.0345 0.104" rpy="0 0 0"/>
     </joint>
 
     <!-- Right side logo symbol -->
-    <link name="${name_link_logo_symbol_r}">
+    <link name="${NAME_LINK_LOGO_SYMBOL_R}">
       <visual>
-        <origin xyz="0 0 0" rpy="${M_PI/2} -${M_PI/2} 0"/>
+        <origin xyz="0 0 0" rpy="${radians(90)} ${radians(-90)} 0"/>
         <geometry>
           <mesh filename="package://crane_x7_description/meshes/visual/rtcorp_logo_rabbit.stl"
               scale="1 1 1"/>
@@ -99,16 +90,16 @@
       </visual>
 
       <collision>
-        <origin xyz="0 0 0" rpy="${M_PI/2} -${M_PI/2} 0"/>
+        <origin xyz="0 0 0" rpy="${radians(90)} ${radians(-90)} 0"/>
         <geometry>
           <mesh filename="package://crane_x7_description/meshes/visual/rtcorp_logo_rabbit.stl"
               scale="1 1 1"/>
         </geometry>
       </collision>
     </link>
-    <joint name="${name_joint_logo_symbol_r}" type="fixed">
+    <joint name="${NAME_JOINT_LOGO_SYMBOL_R}" type="fixed">
       <parent link="${parent}"/>
-      <child link="${name_link_logo_symbol_r}"/>
+      <child link="${NAME_LINK_LOGO_SYMBOL_R}"/>
       <origin xyz="0 -0.0345 0.065" rpy="0 0 0"/>
     </joint>
   </xacro:macro>
diff --git a/urdf/crane_x7_wide_two_finger_gripper.xacro b/urdf/crane_x7_wide_two_finger_gripper.xacro
index 5dc2ebf..ce43766 100644
--- a/urdf/crane_x7_wide_two_finger_gripper.xacro
+++ b/urdf/crane_x7_wide_two_finger_gripper.xacro
@@ -1,28 +1,16 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
-  <xacro:property name="M_PI" value="3.14159"/>
-  <xacro:property name="SCALE_MM" value="0.001"/>
-  <xacro:property name="SCALE_CM" value="0.01"/>
 
-  <xacro:macro name="crane_x7_wide_two_finger_gripper"
-    params="name_link_gripper_base
-            name_link_gripper_finger_a
-            name_link_gripper_finger_b
-            name_joint_gripper_finger_a
-            name_joint_gripper_finger_b
-            color
-            joint_velocity_limit
-            joint_hand_lower_limit
-            joint_hand_upper_limit">
+  <xacro:macro name="crane_x7_wide_two_finger_gripper">
 
-    <link name="${name_link_gripper_base}">
+    <link name="${NAME_LINK_GRIPPER_BASE}">
       <visual>
         <origin xyz="0 0 0" rpy="0 0 0"/>
         <geometry>
           <mesh filename="package://crane_x7_description/meshes/visual/wide_two_finger_gripper_actuator.stl"
               scale="1 1 1"/>
         </geometry>
-        <material name="${color}"/>
+        <material name="${COLOR_HAND}"/>
       </visual>
 
       <collision>
@@ -42,27 +30,31 @@
       </inertial>
     </link>
 
-    <joint name="${name_joint_gripper_finger_a}" type="revolute">
+    <joint name="${NAME_JOINT_GRIPPER_FINGER_A}" type="revolute">
       <origin xyz="-0.012 0.0 0.024" rpy="0 0 0"/>
       <axis xyz="0 -1 0"/>
-      <limit effort="4.0" velocity="${joint_velocity_limit}" lower="${joint_hand_lower_limit}" upper="${joint_hand_upper_limit}"/>
-      <parent link="${name_link_gripper_base}"/>
-      <child link="${name_link_gripper_finger_a}"/>
+      <limit
+        effort="4.0"
+        velocity="${JOINT_VELOCITY_LIMIT}"
+        lower="${JOINT_HAND_LOWER_LIMIT}"
+        upper="${JOINT_HAND_UPPER_LIMIT}"/>
+      <parent link="${NAME_LINK_GRIPPER_BASE}"/>
+      <child link="${NAME_LINK_GRIPPER_FINGER_A}"/>
       <dynamics damping="1.0e-6" friction="8.0e-1"/>
     </joint>
 
-    <link name="${name_link_gripper_finger_a}">
+    <link name="${NAME_LINK_GRIPPER_FINGER_A}">
       <visual>
-        <origin xyz="0 0 0" rpy="${M_PI/2} 0 0"/>
+        <origin xyz="0 0 0" rpy="${radians(90)} 0 0"/>
         <geometry>
           <mesh filename="package://crane_x7_description/meshes/visual/wide_two_finger_gripper_finger_a.stl"
               scale="1 1 1"/>
         </geometry>
-        <material name="${color}"/>
+        <material name="${COLOR_HAND}"/>
       </visual>
 
       <collision>
-        <origin xyz="0 0 0" rpy="${M_PI/2} 0 0"/>
+        <origin xyz="0 0 0" rpy="${radians(90)} 0 0"/>
         <geometry>
           <mesh filename="package://crane_x7_description/meshes/collision/wide_two_finger_gripper_finger_a.stl"
               scale="1 1 1"/>
@@ -71,7 +63,7 @@
 
       <inertial>
         <mass value="0.0158"/>
-        <origin xyz="-2.65E-03 3.54E-03 2.69E-02" rpy="${M_PI/2} 0 0"/>
+        <origin xyz="-2.65E-03 3.54E-03 2.69E-02" rpy="${radians(90)} 0 0"/>
         <inertia ixx="1.01E-05" ixy="-1.97E-07" ixz="-9.89E-08"
         iyy="4.95E-06" iyz="5.37E-07"
         izz="6.24E-06"/>
@@ -81,34 +73,39 @@
     <!--Dummy joint/link to prevent error with mimic joint
     See https://github.com/ros-controls/gazebo_ros2_control/issues/173
     -->
-    <joint name="${name_joint_gripper_finger_b}_mimic" type="fixed">
+    <joint name="${NAME_JOINT_GRIPPER_FINGER_B}_mimic" type="fixed">
       <parent link="world" />
       <child link="dummy_mimic_fix" />
     </joint>
     <link name="dummy_mimic_fix"/>
 
-    <joint name="${name_joint_gripper_finger_b}" type="revolute">
+    <joint name="${NAME_JOINT_GRIPPER_FINGER_B}" type="revolute">
       <origin xyz="0.012 0.0 0.024" rpy="0 0 0"/>
       <axis xyz="0 1 0"/>
-      <limit effort="4.0" velocity="${joint_velocity_limit}" lower="${joint_hand_lower_limit}" upper="${joint_hand_upper_limit}"/>
-      <parent link="${name_link_gripper_base}"/>
-      <child link="${name_link_gripper_finger_b}"/>
-      <mimic joint="${name_joint_gripper_finger_a}"/>
+      <limit
+        effort="4.0"
+        velocity="${JOINT_VELOCITY_LIMIT}"
+        lower="${JOINT_HAND_LOWER_LIMIT}"
+        upper="${JOINT_HAND_UPPER_LIMIT}"
+        />
+      <parent link="${NAME_LINK_GRIPPER_BASE}"/>
+      <child link="${NAME_LINK_GRIPPER_FINGER_B}"/>
+      <mimic joint="${NAME_JOINT_GRIPPER_FINGER_A}"/>
       <dynamics damping="1.0e-6" friction="8.0e-1"/>
     </joint>
 
-    <link name="${name_link_gripper_finger_b}">
+    <link name="${NAME_LINK_GRIPPER_FINGER_B}">
       <visual>
-        <origin xyz="0 0 0" rpy="${M_PI/2} 0 0"/>
+        <origin xyz="0 0 0" rpy="${radians(90)} 0 0"/>
         <geometry>
           <mesh filename="package://crane_x7_description/meshes/visual/wide_two_finger_gripper_finger_b.stl"
               scale="1 1 1"/>
         </geometry>
-        <material name="${color}"/>
+        <material name="${COLOR_HAND}"/>
       </visual>
 
       <collision>
-        <origin xyz="0 0 0" rpy="${M_PI/2} 0 0"/>
+        <origin xyz="0 0 0" rpy="${radians(90)} 0 0"/>
         <geometry>
           <mesh filename="package://crane_x7_description/meshes/collision/wide_two_finger_gripper_finger_b.stl"
               scale="1 1 1"/>
@@ -117,7 +114,7 @@
 
       <inertial>
         <mass value="0.0139"/>
-        <origin xyz="3.01E-03 2.85E-03 3.09E-02" rpy="${M_PI/2} 0 0"/>
+        <origin xyz="3.01E-03 2.85E-03 3.09E-02" rpy="${radians(90)} 0 0"/>
         <inertia ixx="7.48E-06" ixy="3.61E-07" ixz="6.34E-08"
         iyy="4.09E-06" iyz="2.28E-07"
         izz="4.37E-06"/>