Merge remote-tracking branch 'origin/main' into 12-get_entity_state-BT-node

rasmt_support/config/rasmt_ros2_controllers.yaml

@@ -6,7 +6,7 @@ controller_manager:
       type: joint_trajectory_controller/JointTrajectoryController
 
     rasmt_hand_controller:
-      type: position_controllers/GripperActionController
+      type: joint_trajectory_controller/JointTrajectoryController
 
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
@@ -29,4 +29,11 @@ rasmt_arm_controller:
 
 rasmt_hand_controller:
   ros__parameters:
-    joint: rasmt_Slide_1
+    joints: 
+      - rasmt_Slide_1
+      - rasmt_Slide_2
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity

rasmt_support/urdf/rasmt.xacro

@@ -8,7 +8,7 @@
 <link name="world"/>
 
 <xacro:include filename="$(find rasmt_support)/urdf/robot/rasmt_single_macro.xacro"/>
-<xacro:rasmt_single prefix="$(arg robot_name)" parent="world" sim="$(arg sim)"/>
+<xacro:rasmt_single prefix="$(arg robot_name)" parent="world" sim="$(arg sim)" xyz="0 0 0"/>
 
 <xacro:if value="$(arg grip)">
     <xacro:include filename="$(find rasmt_support)/urdf/tools/rasmt_hand_macro.xacro"/>

rasmt_support/urdf/robot/rasmt_single_macro.xacro

@@ -4,14 +4,14 @@
   <xacro:include filename="$(find rasmt_support)/urdf/robot/rasmt_single_control.xacro"/>
   <xacro:include filename="$(find rasmt_support)/urdf/robot/rasmt_single_gazebo.xacro"/>
 
-<xacro:macro name="rasmt_single" params="prefix parent sim:=^|true">
+<xacro:macro name="rasmt_single" params="prefix parent sim xyz">
 
 
   <joint name="to_${parent}" type="fixed">
     <parent link="${parent}"/>
     <child link="${prefix}_Base_Link"/>
     <!--origin xyz="0 0 1.15" rpy="3.14159 0 3.14159"/-->
-    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <origin xyz="${xyz}" rpy="0 0 0"/>
     <axis xyz="0 0 0"/>
   </joint>
 

rasmt_support/urdf/tools/rasmt_hand_control.xacro

@@ -28,8 +28,8 @@
             </joint>
 
             <joint name="${prefix}_Slide_2">
-                <param name="mimic">${prefix}_Slide_1</param>
-                <param name="multiplier">1</param>
+                <!-- <param name="mimic">${prefix}_Slide_1</param>
+                <param name="multiplier">1</param> -->
                 <command_interface name="position">
                     <param name="min">-1</param>
                     <param name="max"> 1</param>

rasmt_support/urdf/tools/rasmt_hand_macro.xacro

@@ -179,7 +179,7 @@
       lower="-0.02"
       upper="0.04"
       velocity="0.2"/>
-    <mimic joint="${prefix}_Slide_1"/>
+    <!--mimic joint="${prefix}_Slide_1"/-->
   </joint>
 
   <xacro:rasmt_hand_hi prefix="${prefix}" sim="${sim}"/>