From 779724fd2c55455d7ab6019528f90aa200677e10 Mon Sep 17 00:00:00 2001
From: Sebastian Jahr <sebastian.jahr@picknik.ai>
Date: Tue, 19 Dec 2023 18:58:05 +0100
Subject: [PATCH 1/2] Update ros2_control usage

---
 .../config/panda.ros2_control.xacro           | 28 ++++++++++++++-----
 .../config/panda_hand.ros2_control.xacro      |  4 +++
 .../launch/demo.launch.py                     |  5 +++-
 .../config/fanuc.ros2_control.xacro           | 24 ++++++++++++----
 fanuc_moveit_config/launch/demo.launch.py     |  5 +++-
 .../config/panda.ros2_control.xacro           | 28 ++++++++++++++-----
 .../config/panda_hand.ros2_control.xacro      |  8 ++++--
 panda_moveit_config/launch/demo.launch.py     |  5 +++-
 8 files changed, 82 insertions(+), 25 deletions(-)

diff --git a/dual_arm_panda_moveit_config/config/panda.ros2_control.xacro b/dual_arm_panda_moveit_config/config/panda.ros2_control.xacro
index 55703004..81f87355 100644
--- a/dual_arm_panda_moveit_config/config/panda.ros2_control.xacro
+++ b/dual_arm_panda_moveit_config/config/panda.ros2_control.xacro
@@ -13,49 +13,63 @@
                 <state_interface name="position">
                   <param name="initial_value">${initial_positions['panda_joint1']}</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                    <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
             <joint name="${prefix}panda_joint2">
                 <command_interface name="position"/>
                 <state_interface name="position">
                   <param name="initial_value">${initial_positions['panda_joint2']}</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                    <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
             <joint name="${prefix}panda_joint3">
                 <command_interface name="position"/>
                 <state_interface name="position">
                   <param name="initial_value">${initial_positions['panda_joint3']}</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                    <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
             <joint name="${prefix}panda_joint4">
                 <command_interface name="position"/>
                 <state_interface name="position">
                   <param name="initial_value">${initial_positions['panda_joint4']}</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                    <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
             <joint name="${prefix}panda_joint5">
                 <command_interface name="position"/>
                 <state_interface name="position">
                   <param name="initial_value">${initial_positions['panda_joint5']}</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                    <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
             <joint name="${prefix}panda_joint6">
                 <command_interface name="position"/>
                 <state_interface name="position">
                   <param name="initial_value">${initial_positions['panda_joint6']}</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                    <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
             <joint name="${prefix}panda_joint7">
                 <command_interface name="position"/>
                 <state_interface name="position">
                   <param name="initial_value">${initial_positions['panda_joint7']}</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                    <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
         </ros2_control>
     </xacro:macro>
diff --git a/dual_arm_panda_moveit_config/config/panda_hand.ros2_control.xacro b/dual_arm_panda_moveit_config/config/panda_hand.ros2_control.xacro
index 8195da40..3e4ed3a6 100644
--- a/dual_arm_panda_moveit_config/config/panda_hand.ros2_control.xacro
+++ b/dual_arm_panda_moveit_config/config/panda_hand.ros2_control.xacro
@@ -12,6 +12,8 @@
                   <param name="initial_value">0.0</param>
                 </state_interface>
                 <state_interface name="velocity"/>
+                    <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
             <joint name="${prefix}panda_finger_joint2">
                 <param name="mimic">${prefix}panda_finger_joint1</param>
@@ -21,6 +23,8 @@
                   <param name="initial_value">0.0</param>
                 </state_interface>
                 <state_interface name="velocity"/>
+                    <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
         </ros2_control>
     </xacro:macro>
diff --git a/dual_arm_panda_moveit_config/launch/demo.launch.py b/dual_arm_panda_moveit_config/launch/demo.launch.py
index d0c3a4e4..1cf9de4f 100644
--- a/dual_arm_panda_moveit_config/launch/demo.launch.py
+++ b/dual_arm_panda_moveit_config/launch/demo.launch.py
@@ -61,7 +61,10 @@ def generate_launch_description():
     ros2_control_node = Node(
         package="controller_manager",
         executable="ros2_control_node",
-        parameters=[moveit_config.robot_description, ros2_controllers_path],
+        parameters=[ros2_controllers_path],
+        remappings=[
+            ("/controller_manager/robot_description", "/robot_description"),
+        ],
         output="both",
     )
 
diff --git a/fanuc_moveit_config/config/fanuc.ros2_control.xacro b/fanuc_moveit_config/config/fanuc.ros2_control.xacro
index c731a4bc..10e07222 100644
--- a/fanuc_moveit_config/config/fanuc.ros2_control.xacro
+++ b/fanuc_moveit_config/config/fanuc.ros2_control.xacro
@@ -13,42 +13,54 @@
                 <state_interface name="position">
                   <param name="initial_value">${initial_positions['joint_1']}</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                    <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
             <joint name="joint_2">
                 <command_interface name="position"/>
                 <state_interface name="position">
                   <param name="initial_value">${initial_positions['joint_2']}</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                    <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
             <joint name="joint_3">
                 <command_interface name="position"/>
                 <state_interface name="position">
                   <param name="initial_value">${initial_positions['joint_3']}</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                    <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
             <joint name="joint_4">
                 <command_interface name="position"/>
                 <state_interface name="position">
                   <param name="initial_value">${initial_positions['joint_4']}</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                    <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
             <joint name="joint_5">
                 <command_interface name="position"/>
                 <state_interface name="position">
                   <param name="initial_value">${initial_positions['joint_5']}</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                    <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
             <joint name="joint_6">
                 <command_interface name="position"/>
                 <state_interface name="position">
                   <param name="initial_value">${initial_positions['joint_6']}</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                    <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
         </ros2_control>
     </xacro:macro>
diff --git a/fanuc_moveit_config/launch/demo.launch.py b/fanuc_moveit_config/launch/demo.launch.py
index 6becbcdf..80ec9541 100644
--- a/fanuc_moveit_config/launch/demo.launch.py
+++ b/fanuc_moveit_config/launch/demo.launch.py
@@ -79,7 +79,10 @@ def generate_launch_description():
     ros2_control_node = Node(
         package="controller_manager",
         executable="ros2_control_node",
-        parameters=[moveit_config.robot_description, ros2_controllers_path],
+        parameters=[ros2_controllers_path],
+        remappings=[
+            ("/controller_manager/robot_description", "/robot_description"),
+        ],
         output="both",
     )
 
diff --git a/panda_moveit_config/config/panda.ros2_control.xacro b/panda_moveit_config/config/panda.ros2_control.xacro
index addc3384..d66294c9 100644
--- a/panda_moveit_config/config/panda.ros2_control.xacro
+++ b/panda_moveit_config/config/panda.ros2_control.xacro
@@ -20,49 +20,63 @@
                 <state_interface name="position">
                   <param name="initial_value">${initial_positions['panda_joint1']}</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                  <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
             <joint name="panda_joint2">
                 <command_interface name="position"/>
                 <state_interface name="position">
                   <param name="initial_value">${initial_positions['panda_joint2']}</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                  <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
             <joint name="panda_joint3">
                 <command_interface name="position"/>
                 <state_interface name="position">
                   <param name="initial_value">${initial_positions['panda_joint3']}</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                  <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
             <joint name="panda_joint4">
                 <command_interface name="position"/>
                 <state_interface name="position">
                   <param name="initial_value">${initial_positions['panda_joint4']}</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                  <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
             <joint name="panda_joint5">
                 <command_interface name="position"/>
                 <state_interface name="position">
                   <param name="initial_value">${initial_positions['panda_joint5']}</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                  <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
             <joint name="panda_joint6">
                 <command_interface name="position"/>
                 <state_interface name="position">
                   <param name="initial_value">${initial_positions['panda_joint6']}</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                  <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
             <joint name="panda_joint7">
                 <command_interface name="position"/>
                 <state_interface name="position">
                   <param name="initial_value">${initial_positions['panda_joint7']}</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                  <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
         </ros2_control>
     </xacro:macro>
diff --git a/panda_moveit_config/config/panda_hand.ros2_control.xacro b/panda_moveit_config/config/panda_hand.ros2_control.xacro
index 06d4d177..9adc4f6c 100644
--- a/panda_moveit_config/config/panda_hand.ros2_control.xacro
+++ b/panda_moveit_config/config/panda_hand.ros2_control.xacro
@@ -18,7 +18,9 @@
                 <state_interface name="position">
                   <param name="initial_value">0.0</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                    <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
             <joint name="panda_finger_joint2">
                 <param name="mimic">panda_finger_joint1</param>
@@ -27,7 +29,9 @@
                 <state_interface name="position">
                   <param name="initial_value">0.0</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                    <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
         </ros2_control>
     </xacro:macro>
diff --git a/panda_moveit_config/launch/demo.launch.py b/panda_moveit_config/launch/demo.launch.py
index cf84d976..d0173937 100644
--- a/panda_moveit_config/launch/demo.launch.py
+++ b/panda_moveit_config/launch/demo.launch.py
@@ -105,7 +105,10 @@ def generate_launch_description():
     ros2_control_node = Node(
         package="controller_manager",
         executable="ros2_control_node",
-        parameters=[moveit_config.robot_description, ros2_controllers_path],
+        parameters=[ros2_controllers_path],
+        remappings=[
+            ("/controller_manager/robot_description", "/robot_description"),
+        ],
         output="screen",
     )
 

From ad665bd91ff81e65cf0c794d09e95b561263f6f4 Mon Sep 17 00:00:00 2001
From: Sebastian Jahr <sebastian.jahr@tuta.io>
Date: Wed, 20 Dec 2023 09:26:01 +0100
Subject: [PATCH 2/2] Update
 dual_arm_panda_moveit_config/config/panda_hand.ros2_control.xacro

Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>
---
 .../config/panda_hand.ros2_control.xacro                      | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/dual_arm_panda_moveit_config/config/panda_hand.ros2_control.xacro b/dual_arm_panda_moveit_config/config/panda_hand.ros2_control.xacro
index 3e4ed3a6..a45ea059 100644
--- a/dual_arm_panda_moveit_config/config/panda_hand.ros2_control.xacro
+++ b/dual_arm_panda_moveit_config/config/panda_hand.ros2_control.xacro
@@ -11,7 +11,7 @@
                 <state_interface name="position">
                   <param name="initial_value">0.0</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
                     <param name="initial_value">0.0</param>
                 </state_interface>
             </joint>
@@ -22,7 +22,7 @@
                 <state_interface name="position">
                   <param name="initial_value">0.0</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
                     <param name="initial_value">0.0</param>
                 </state_interface>
             </joint>