Merge pull request #5 from helix-robotics-ag/seb/add_current_control

Seb/add current control
helix-robotics-ag · Feb 15, 2024 · 0016f80 · 0016f80
2 parents 1b0a895 + c9d13f0
commit 0016f80
Show file tree

Hide file tree

Showing 4 changed files with 48 additions and 38 deletions.
diff --git a/dynamixel_hardware b/dynamixel_hardware
diff --git a/helix_bringup/launch/helix_bringup.launch.py b/helix_bringup/launch/helix_bringup.launch.py
@@ -6,6 +6,8 @@
 
 from launch import LaunchDescription
 from launch_ros.actions import Node
+# from launch.actions import RegisterEventHandler
+# from launch.event_handlers import OnProcessExit
 
 def generate_launch_description():
 
@@ -53,6 +55,14 @@ def generate_launch_description():
         output="screen",
     )
 
+    # ros2_control 'controller' (broadcaster) for motor joint states
+    motor_head_joint_state_broadcaster_node = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["motor_head_joint_state_broadcaster", "--controller-manager", "/controller_manager"],
+        output="screen",
+    )
+
     # ros2_control controller for motor joint positions
     motor_head_joint_position_controller_node = Node(
         package="controller_manager",
@@ -62,27 +72,27 @@ def generate_launch_description():
     )
 
     # ros2_control controller for motor joint efforts
-    # motor_head_helix_joint_effort_controller_node = Node(
-    #         package="controller_manager",
-    #         executable="spawner",
-    #         arguments=["motor_head_joint_effort_controller", "--inactive", "-c", "/controller_manager"],
-    #         output="screen",
-    # )
-
-    # ros2_control 'controller' (broadcaster) for motor joint states
-    motor_head_joint_state_broadcaster_node = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["motor_head_joint_state_broadcaster", "--controller-manager", "/controller_manager"],
-        output="screen",
+    motor_head_helix_joint_effort_controller_node = Node(
+            package="controller_manager",
+            executable="spawner",
+            arguments=["motor_head_joint_effort_controller", "--inactive", "-c", "/controller_manager"],
+            output="screen",
     )
 
+    # # Delay start of motor_head_joint_position_controller_node after `motor_head_joint_state_broadcaster_node`
+    # delay_joint_controller_after_broadcaster = RegisterEventHandler(
+    #     event_handler=OnProcessExit(
+    #         target_action=motor_head_joint_state_broadcaster_node,
+    #         on_exit=[motor_head_joint_position_controller_node],
+    #     )
+    # )
 
     ld.add_action(robot_state_publisher)
     ld.add_action(joint_state_publisher_node)
     ld.add_action(helix_ros2_control_node)
     ld.add_action(motor_head_joint_position_controller_node)
-    # ld.add_action(motor_head_helix_joint_effort_controller_node)
+    ld.add_action(motor_head_helix_joint_effort_controller_node)
     ld.add_action(motor_head_joint_state_broadcaster_node)
+    # ld.add_action(delay_joint_controller_after_broadcaster)
 
     return ld
diff --git a/helix_description/config/controllers.yaml b/helix_description/config/controllers.yaml
@@ -5,8 +5,8 @@ controller_manager:
     motor_head_joint_position_controller:
       type: position_controllers/JointGroupPositionController
 
-    # motor_head_joint_effort_controller: 
-    #   type: effort_controllers/JointGroupEffortController
+    motor_head_joint_effort_controller: 
+      type: effort_controllers/JointGroupEffortController
 
     motor_head_joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
@@ -24,18 +24,18 @@ motor_head_joint_position_controller:
       - joint7
       - joint8
 
-# motor_head_joint_effort_controller:
-#   ros__parameters:
-#     joints:
-#       - joint0
-#       - joint1
-#       - joint2
-#       - joint3
-#       - joint4
-#       - joint5
-#       - joint6
-#       - joint7
-#       - joint8
+motor_head_joint_effort_controller:
+  ros__parameters:
+    joints:
+      - joint0
+      - joint1
+      - joint2
+      - joint3
+      - joint4
+      - joint5
+      - joint6
+      - joint7
+      - joint8
 
 motor_head_joint_state_broadcaster:
   ros__parameters:

diff --git a/helix_description/urdf/helix.ros2_control.xacro b/helix_description/urdf/helix.ros2_control.xacro
@@ -15,71 +15,71 @@
         <param name="id">0</param>
         <command_interface name="position"/>
         <!-- effort interface for current control -->
-        <!-- <command_interface name="effort"/> -->
+        <command_interface name="effort"/>
         <state_interface name="position"/>
         <state_interface name="velocity"/>
         <state_interface name="effort"/>
       </joint>
       <joint name="joint1">
         <param name="id">1</param>
         <command_interface name="position"/>
-        <!-- <command_interface name="effort"/> -->
+        <command_interface name="effort"/>
         <state_interface name="position"/>
         <state_interface name="velocity"/>
         <state_interface name="effort"/>
       </joint>
       <joint name="joint2">
         <param name="id">2</param>
         <command_interface name="position"/>
-        <!-- <command_interface name="effort"/> -->
+        <command_interface name="effort"/>
         <state_interface name="position"/>
         <state_interface name="velocity"/>
         <state_interface name="effort"/>
       </joint>
       <joint name="joint3">
         <param name="id">3</param>
         <command_interface name="position"/>
-        <!-- <command_interface name="effort"/> -->
+        <command_interface name="effort"/>
         <state_interface name="position"/>
         <state_interface name="velocity"/>
         <state_interface name="effort"/>
       </joint>
       <joint name="joint4">
         <param name="id">4</param>
         <command_interface name="position"/>
-        <!-- <command_interface name="effort"/> -->
+        <command_interface name="effort"/>
         <state_interface name="position"/>
         <state_interface name="velocity"/>
         <state_interface name="effort"/>
       </joint>
       <joint name="joint5">
         <param name="id">5</param>
         <command_interface name="position"/>
-        <!-- <command_interface name="effort"/> -->
+        <command_interface name="effort"/>
         <state_interface name="position"/>
         <state_interface name="velocity"/>
         <state_interface name="effort"/>
       </joint>
       <joint name="joint6">
         <param name="id">6</param>
         <command_interface name="position"/>
-        <!-- <command_interface name="effort"/> -->
+        <command_interface name="effort"/>
         <state_interface name="position"/>
         <state_interface name="velocity"/>
         <state_interface name="effort"/>
       </joint>
       <joint name="joint7">
         <param name="id">7</param>
         <command_interface name="position"/>
-        <!-- <command_interface name="effort"/> -->
+        <command_interface name="effort"/>
         <state_interface name="position"/>
         <state_interface name="velocity"/>
         <state_interface name="effort"/>
       </joint>
       <joint name="joint8">
         <param name="id">8</param>
         <command_interface name="position"/>
-        <!-- <command_interface name="effort"/> -->
+        <command_interface name="effort"/>
         <state_interface name="position"/>
         <state_interface name="velocity"/>
         <state_interface name="effort"/>