From afa8120d08d3bf9992eac3d58b65eef699583c70 Mon Sep 17 00:00:00 2001
From: Zimeng Chai <zimeng.chai@gatech.edu>
Date: Sun, 6 Oct 2024 16:40:35 -0400
Subject: [PATCH] fix: arm ros2 model

---
 .../WalliArm_V2_Block_base.ros2_control.xacro | 119 +++++++++---------
 .../config/WalliArm_V2_Block_base.urdf.xacro  |   5 +-
 .../config/ros2_controllers.yaml              |   5 +-
 3 files changed, 63 insertions(+), 66 deletions(-)

diff --git a/walli_arm_v2_moveit_config/config/WalliArm_V2_Block_base.ros2_control.xacro b/walli_arm_v2_moveit_config/config/WalliArm_V2_Block_base.ros2_control.xacro
index 48a4b0d0..04dc031b 100644
--- a/walli_arm_v2_moveit_config/config/WalliArm_V2_Block_base.ros2_control.xacro
+++ b/walli_arm_v2_moveit_config/config/WalliArm_V2_Block_base.ros2_control.xacro
@@ -1,64 +1,63 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
-    <xacro:macro name="WalliArm_V2_Block_base_ros2_control" params="name initial_positions_file">
-        <xacro:property name="initial_positions" value="${load_yaml(initial_positions_file)['initial_positions']}"/>
+  <xacro:macro name="WalliArm_V2_Block_base_ros2_control" params="name initial_positions_file">
+    <xacro:property name="initial_positions"
+      value="${load_yaml(initial_positions_file)['initial_positions']}" />
 
-        <ros2_control name="${name}" type="system">
-            <hardware>
-                <!-- By default, set up controllers for simulation. This won't work on real hardware -->
-                <plugin>mock_components/GenericSystem</plugin>
-            </hardware>
-            <joint name="base_yaw_joint">
-                <command_interface name="position"/>
-                <command_interface name="velocity"/>
-                <command_interface name="effort"/>
-                <state_interface name="position">
-                  <param name="initial_value">${initial_positions['base_yaw_joint']}</param>
-                </state_interface>
-                <state_interface name="velocity"/>
-                <state_interface name="effort"/>
-            </joint>
-            <joint name="upper_arm_joint">
-                <command_interface name="position"/>
-                <command_interface name="velocity"/>
-                <command_interface name="effort"/>
-                <state_interface name="position">
-                  <param name="initial_value">${initial_positions['upper_arm_joint']}</param>
-                </state_interface>
-                <state_interface name="velocity"/>
-                <state_interface name="effort"/>
-            </joint>
-            <joint name="fore_arm_joint">
-                <command_interface name="position"/>
-                <command_interface name="velocity"/>
-                <command_interface name="effort"/>
-                <state_interface name="position">
-                  <param name="initial_value">${initial_positions['fore_arm_joint']}</param>
-                </state_interface>
-                <state_interface name="velocity"/>
-                <state_interface name="effort"/>
-            </joint>
-            <joint name="ee_roll_jont">
-                <command_interface name="position"/>
-                <command_interface name="velocity"/>
-                <command_interface name="effort"/>
-                <state_interface name="position">
-                  <param name="initial_value">${initial_positions['ee_roll_jont']}</param>
-                </state_interface>
-                <state_interface name="velocity"/>
-                <state_interface name="effort"/>
-            </joint>
-            <joint name="ee_pitch_joint">
-                <command_interface name="position"/>
-                <command_interface name="velocity"/>
-                <command_interface name="effort"/>
-                <state_interface name="position">
-                  <param name="initial_value">${initial_positions['ee_pitch_joint']}</param>
-                </state_interface>
-                <state_interface name="velocity"/>
-                <state_interface name="effort"/>
-            </joint>
+    <ros2_control name="${name}" type="system">
+      <hardware>
+        <!-- By default, set up controllers for simulation. This won't work on real hardware -->
+        <plugin>mock_components/GenericSystem</plugin>
+      </hardware>
+      <joint name="base_yaw_joint">
+        <command_interface name="position" />
+        <command_interface name="velocity" />
+        <state_interface name="position">
+          <param name="initial_value">${initial_positions['base_yaw_joint']}</param>
+        </state_interface>
+        <state_interface name="velocity" />
+      </joint>
+      <joint name="upper_arm_joint">
+        <command_interface name="position" />
+        <command_interface name="velocity" />
+        <command_interface name="effort" />
+        <state_interface name="position">
+          <param name="initial_value">${initial_positions['upper_arm_joint']}</param>
+        </state_interface>
+        <state_interface name="velocity" />
 
-        </ros2_control>
-    </xacro:macro>
-</robot>
+      </joint>
+      <joint name="fore_arm_joint">
+        <command_interface name="position" />
+        <command_interface name="velocity" />
+        <command_interface name="effort" />
+        <state_interface name="position">
+          <param name="initial_value">${initial_positions['fore_arm_joint']}</param>
+        </state_interface>
+        <state_interface name="velocity" />
+
+      </joint>
+      <joint name="ee_roll_jont">
+        <command_interface name="position" />
+        <command_interface name="velocity" />
+        <command_interface name="effort" />
+        <state_interface name="position">
+          <param name="initial_value">${initial_positions['ee_roll_jont']}</param>
+        </state_interface>
+        <state_interface name="velocity" />
+
+      </joint>
+      <joint name="ee_pitch_joint">
+        <command_interface name="position" />
+        <command_interface name="velocity" />
+        <command_interface name="effort" />
+        <state_interface name="position">
+          <param name="initial_value">${initial_positions['ee_pitch_joint']}</param>
+        </state_interface>
+        <state_interface name="velocity" />
+
+      </joint>
+
+    </ros2_control>
+  </xacro:macro>
+</robot>
\ No newline at end of file
diff --git a/walli_arm_v2_moveit_config/config/WalliArm_V2_Block_base.urdf.xacro b/walli_arm_v2_moveit_config/config/WalliArm_V2_Block_base.urdf.xacro
index 06ed459b..1d22a5fe 100644
--- a/walli_arm_v2_moveit_config/config/WalliArm_V2_Block_base.urdf.xacro
+++ b/walli_arm_v2_moveit_config/config/WalliArm_V2_Block_base.urdf.xacro
@@ -9,6 +9,7 @@
     <xacro:include filename="WalliArm_V2_Block_base.ros2_control.xacro" />
 
 
-    <xacro:WalliArm_V2_Block_base_ros2_control name="FakeSystem" initial_positions_file="$(arg initial_positions_file)"/>
+    <xacro:WalliArm_V2_Block_base_ros2_control name="FakeSystem"
+        initial_positions_file="$(arg initial_positions_file)" />
 
-</robot>
+</robot>
\ No newline at end of file
diff --git a/walli_arm_v2_moveit_config/config/ros2_controllers.yaml b/walli_arm_v2_moveit_config/config/ros2_controllers.yaml
index bd94601f..6736ba4e 100644
--- a/walli_arm_v2_moveit_config/config/ros2_controllers.yaml
+++ b/walli_arm_v2_moveit_config/config/ros2_controllers.yaml
@@ -1,12 +1,11 @@
 # This config file is used by ros2_control
 controller_manager:
   ros__parameters:
-    update_rate: 100  # Hz
+    update_rate: 100 # Hz
 
     walli_arm_v2_block_controller:
       type: joint_trajectory_controller/JointTrajectoryController
 
-
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
@@ -21,8 +20,6 @@ walli_arm_v2_block_controller:
     command_interfaces:
       - position
       - velocity
-      - effort
     state_interfaces:
       - position
       - velocity
-      - effort
\ No newline at end of file