From 2f10a5379159cfeb9a5b9c2a0e4e90a4f1c65839 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Mon, 25 Oct 2021 14:58:31 +0200
Subject: [PATCH 1/5] Simplify use of arguments

No need to declare an intermediate property
---
 .../robots/panda_arm.urdf.xacro               | 40 +++++++++----------
 1 file changed, 19 insertions(+), 21 deletions(-)

diff --git a/franka_description/robots/panda_arm.urdf.xacro b/franka_description/robots/panda_arm.urdf.xacro
index d5b8d0061..8fa6a317c 100644
--- a/franka_description/robots/panda_arm.urdf.xacro
+++ b/franka_description/robots/panda_arm.urdf.xacro
@@ -5,16 +5,14 @@
   <xacro:arg name="hand"   default="false" /> <!-- Should a franka_gripper be mounted at the flange?" -->
   <xacro:arg name="gazebo" default="false" /> <!-- Is the robot being simulated in gazebo?" -->
 
-  <xacro:property name="arm_id" value="$(arg arm_id)" />
-
   <xacro:unless value="$(arg gazebo)">
     <!-- Create a URDF for a real hardware -->
     <xacro:include filename="$(find franka_description)/robots/panda_arm.xacro" />
-    <xacro:panda_arm arm_id="${arm_id}" safety_distance="0.03"/>
+    <xacro:panda_arm arm_id="$(arg arm_id)" safety_distance="0.03"/>
 
     <xacro:if value="$(arg hand)">
       <xacro:include filename="$(find franka_description)/robots/hand.xacro"/>
-      <xacro:hand ns="${arm_id}" rpy="0 0 ${-pi/4}" connected_to="${arm_id}_link8" safety_distance="0.03"/>
+      <xacro:hand ns="$(arg arm_id)" rpy="0 0 ${-pi/4}" connected_to="$(arg arm_id)_link8" safety_distance="0.03"/>
     </xacro:if>
   </xacro:unless>
 
@@ -27,12 +25,12 @@
     <xacro:include filename="$(find franka_description)/robots/utils.xacro" />
     <xacro:include filename="$(find franka_description)/robots/panda_gazebo.xacro" />
 
-    <xacro:panda_arm arm_id="${arm_id}" />
+    <xacro:panda_arm arm_id="$(arg arm_id)" />
 
     <xacro:if value="$(arg hand)">
-      <xacro:hand ns="${arm_id}" rpy="0 0 ${-pi/4}" connected_to="${arm_id}_link8" />
-      <xacro:gazebo-joint joint="${arm_id}_finger_joint1" transmission="hardware_interface/EffortJointInterface" />
-      <xacro:gazebo-joint joint="${arm_id}_finger_joint2" transmission="hardware_interface/EffortJointInterface" />
+      <xacro:hand ns="$(arg arm_id)" rpy="0 0 ${-pi/4}" connected_to="$(arg arm_id)_link8" />
+      <xacro:gazebo-joint joint="$(arg arm_id)_finger_joint1" transmission="hardware_interface/EffortJointInterface" />
+      <xacro:gazebo-joint joint="$(arg arm_id)_finger_joint2" transmission="hardware_interface/EffortJointInterface" />
     </xacro:if>
 
     <!-- Gazebo requires a joint to a link called "world" for statically mounted robots -->
@@ -40,27 +38,27 @@
     <joint name="world_joint" type="fixed">
       <origin xyz="$(arg xyz)" rpy="$(arg rpy)" />
       <parent link="world" />
-      <child  link="${arm_id}_link0" />
+      <child  link="$(arg arm_id)_link0" />
     </joint>
 
 
-    <xacro:gazebo-joint joint="${arm_id}_joint1" transmission="hardware_interface/EffortJointInterface" />
-    <xacro:gazebo-joint joint="${arm_id}_joint2" transmission="hardware_interface/EffortJointInterface" />
-    <xacro:gazebo-joint joint="${arm_id}_joint3" transmission="hardware_interface/EffortJointInterface" />
-    <xacro:gazebo-joint joint="${arm_id}_joint4" transmission="hardware_interface/EffortJointInterface" />
-    <xacro:gazebo-joint joint="${arm_id}_joint5" transmission="hardware_interface/EffortJointInterface" />
-    <xacro:gazebo-joint joint="${arm_id}_joint6" transmission="hardware_interface/EffortJointInterface" />
-    <xacro:gazebo-joint joint="${arm_id}_joint7" transmission="hardware_interface/EffortJointInterface" />
+    <xacro:gazebo-joint joint="$(arg arm_id)_joint1" transmission="hardware_interface/EffortJointInterface" />
+    <xacro:gazebo-joint joint="$(arg arm_id)_joint2" transmission="hardware_interface/EffortJointInterface" />
+    <xacro:gazebo-joint joint="$(arg arm_id)_joint3" transmission="hardware_interface/EffortJointInterface" />
+    <xacro:gazebo-joint joint="$(arg arm_id)_joint4" transmission="hardware_interface/EffortJointInterface" />
+    <xacro:gazebo-joint joint="$(arg arm_id)_joint5" transmission="hardware_interface/EffortJointInterface" />
+    <xacro:gazebo-joint joint="$(arg arm_id)_joint6" transmission="hardware_interface/EffortJointInterface" />
+    <xacro:gazebo-joint joint="$(arg arm_id)_joint7" transmission="hardware_interface/EffortJointInterface" />
 
-    <xacro:transmission-franka-state arm_id="${arm_id}" />
-    <xacro:transmission-franka-model arm_id="${arm_id}"
-       root="${arm_id}_joint1"
-       tip="${arm_id}_joint8"
+    <xacro:transmission-franka-state arm_id="$(arg arm_id)" />
+    <xacro:transmission-franka-model arm_id="$(arg arm_id)"
+       root="$(arg arm_id)_joint1"
+       tip="$(arg arm_id)_joint8"
      />
 
     <gazebo>
       <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
-        <robotNamespace>${arm_id}</robotNamespace>
+        <robotNamespace>$(arg arm_id)</robotNamespace>
         <controlPeriod>0.001</controlPeriod>
         <robotSimType>franka_gazebo/FrankaHWSim</robotSimType>
       </plugin>

From da8a709fd69595fd68df7b5979eafdb837e78295 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Mon, 25 Oct 2021 18:37:02 +0200
Subject: [PATCH 2/5] Place XML comments in front of xacro commands

... to hide them in the final URDF
---
 franka_description/robots/panda_arm.urdf.xacro | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

diff --git a/franka_description/robots/panda_arm.urdf.xacro b/franka_description/robots/panda_arm.urdf.xacro
index 8fa6a317c..8dc6cd9bb 100644
--- a/franka_description/robots/panda_arm.urdf.xacro
+++ b/franka_description/robots/panda_arm.urdf.xacro
@@ -1,9 +1,12 @@
 <?xml version='1.0' encoding='utf-8'?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
 
-  <xacro:arg name="arm_id" default="panda" /> <!-- Name of this panda -->
-  <xacro:arg name="hand"   default="false" /> <!-- Should a franka_gripper be mounted at the flange?" -->
-  <xacro:arg name="gazebo" default="false" /> <!-- Is the robot being simulated in gazebo?" -->
+  <!-- Name of this panda -->
+  <xacro:arg name="arm_id" default="panda" />
+  <!-- Should a franka_gripper be mounted at the flange?" -->
+  <xacro:arg name="hand" default="false" />
+  <!-- Is the robot being simulated in gazebo?" -->
+  <xacro:arg name="gazebo" default="false" />
 
   <xacro:unless value="$(arg gazebo)">
     <!-- Create a URDF for a real hardware -->

From 5bc246f22cc229f0a8f067ddffe128f55c9439bd Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Mon, 25 Oct 2021 18:38:11 +0200
Subject: [PATCH 3/5] Fix typo

---
 franka_description/robots/panda_gazebo.xacro | 2 +-
 franka_gazebo/test/launch/panda-gazebo.urdf  | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/franka_description/robots/panda_gazebo.xacro b/franka_description/robots/panda_gazebo.xacro
index 984597546..0eb1df4f5 100644
--- a/franka_description/robots/panda_gazebo.xacro
+++ b/franka_description/robots/panda_gazebo.xacro
@@ -8,7 +8,7 @@
   <!-- || PandaRobot With Retrieval of Feasible Parameters Using    || -->
   <!-- || Penalty-Based Optimization                                || -->
   <!-- || by: Claudio Gaz, Marco Cognetti, Alexander Oliva,         || -->
-  <!-- || Paolo Robuffo Giordano, Alessandro de Lucaa               || -->
+  <!-- || Paolo Robuffo Giordano, Alessandro de Luca                || -->
   <!-- =============================================================== -->
 
   <xacro:macro name="panda_arm" params="arm_id:='panda'">
diff --git a/franka_gazebo/test/launch/panda-gazebo.urdf b/franka_gazebo/test/launch/panda-gazebo.urdf
index cd262f36e..3ef04c6c5 100644
--- a/franka_gazebo/test/launch/panda-gazebo.urdf
+++ b/franka_gazebo/test/launch/panda-gazebo.urdf
@@ -7,7 +7,7 @@
   <!-- || PandaRobot With Retrieval of Feasible Parameters Using    || -->
   <!-- || Penalty-Based Optimization                                || -->
   <!-- || by: Claudio Gaz, Marco Cognetti, Alexander Oliva,         || -->
-  <!-- || Paolo Robuffo Giordano,Alessandro de Lucaa third-party    || -->
+  <!-- || Paolo Robuffo Giordano, Alessandro de Luca third-party    || -->
   <!-- =============================================================== -->
   <link name="world"/>
   <joint name="panda_joint_panda_mount" type="fixed">

From 689e9469ea7ab253125f8ffbe03972010f4dc0af Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Wed, 27 Oct 2021 16:27:26 +0200
Subject: [PATCH 4/5] Simplify use of argument `paused`

... avoiding code duplication
---
 franka_gazebo/launch/panda.launch | 17 ++++-------------
 1 file changed, 4 insertions(+), 13 deletions(-)

diff --git a/franka_gazebo/launch/panda.launch b/franka_gazebo/launch/panda.launch
index 527802021..9226b13f5 100644
--- a/franka_gazebo/launch/panda.launch
+++ b/franka_gazebo/launch/panda.launch
@@ -31,6 +31,7 @@
 
   <include file="$(find gazebo_ros)/launch/empty_world.launch" if="$(arg gazebo)">
     <arg name="world_name" value="$(arg world)"/>
+    <!-- Always start in paused mode, and only unpause when spawning the model -->
     <arg name="paused" value="true"/>
     <arg name="gui" value="$(eval not arg('headless'))"/>
     <arg name="use_sim_time" value="true"/>
@@ -51,23 +52,13 @@
 
     <param name="m_ee" value="0.76" if="$(arg use_gripper)" />
 
+    <arg name="unpause" value="$(eval '' if arg('paused') else '-unpause')" />
     <node name="$(arg arm_id)_model_spawner"
           pkg="gazebo_ros"
           type="spawn_model"
-          if="$(arg paused)"
-          args="-param robot_description -urdf -model $(arg arm_id)
+          args="-param robot_description -urdf -model $(arg arm_id) $(arg unpause)
                 $(arg initial_joint_positions)
-                ">
-    </node>
-    <node name="$(arg arm_id)_model_spawner"
-          pkg="gazebo_ros"
-          type="spawn_model"
-          unless="$(arg paused)"
-          args="-param robot_description -urdf -model $(arg arm_id) -unpause
-                $(arg initial_joint_positions)
-                ">
-    </node>
-
+                "/>
 
     <!-- Spawn required ROS controllers -->
     <node pkg="controller_manager"

From bd69b6150c6dce343137d97c5e65d764f7e57784 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Mon, 25 Oct 2021 22:33:00 +0200
Subject: [PATCH 5/5] Move `remap from=equilibrium_pose` to the
 `interactive_marker.py` node

... to clarify that this node requires the remapping.
---
 franka_gazebo/launch/panda.launch | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/franka_gazebo/launch/panda.launch b/franka_gazebo/launch/panda.launch
index 9226b13f5..39b287439 100644
--- a/franka_gazebo/launch/panda.launch
+++ b/franka_gazebo/launch/panda.launch
@@ -76,8 +76,6 @@
           args="franka_state_controller $(arg controller)"
     />
 
-    <remap to="cartesian_impedance_example_controller/equilibrium_pose" from="equilibrium_pose" />
-
     <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
     <node name="joint_state_publisher" type="joint_state_publisher" pkg="joint_state_publisher">
       <rosparam param="source_list">[franka_state_controller/joint_states, franka_gripper/joint_states] </rosparam>
@@ -95,6 +93,7 @@
           type="interactive_marker.py"
           if="$(eval arg('controller') == 'cartesian_impedance_example_controller')">
       <param name="link_name" value="$(arg arm_id)_link0" />
+      <remap to="cartesian_impedance_example_controller/equilibrium_pose" from="equilibrium_pose" />
     </node>
 
   </group>