Merge pull request #18 from helix-robotics-ag/seb/add_gripper_motor_c…

…ontroller Seb/add gripper motor controller
helix-robotics-ag · May 7, 2024 · c4a4a58 · c4a4a58
2 parents da3c2f1 + faea29c
commit c4a4a58
Show file tree

Hide file tree

Showing 4 changed files with 101 additions and 4 deletions.
diff --git a/helix_bringup/launch/helix_bringup.launch.py b/helix_bringup/launch/helix_bringup.launch.py
@@ -35,7 +35,7 @@ def generate_launch_description():
         executable='joint_state_publisher',
         name='joint_state_publisher',
         parameters=[{
-            'source_list': ['motor_head_joint_state_broadcaster/joint_states'], 
+            'source_list': ['motor_head_joint_state_broadcaster/joint_states','gripper_joint_state_broadcaster/joint_states'], 
         }]
     )
 
@@ -72,7 +72,7 @@ def generate_launch_description():
     )
 
     # ros2_control controller for motor joint efforts
-    motor_head_helix_joint_effort_controller_node = Node(
+    motor_head_joint_effort_controller_node = Node(
             package="controller_manager",
             executable="spawner",
             arguments=["motor_head_joint_effort_controller", "--inactive", "-c", "/controller_manager"],
@@ -86,12 +86,39 @@ def generate_launch_description():
         output="screen",
     )
 
+    # ros2_control 'controller' (broadcaster) for gripper joint state
+    gripper_joint_state_broadcaster_node = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["gripper_joint_state_broadcaster", "--controller-manager", "/controller_manager"],
+        output="screen",
+    )
+
+    # ros2_control controller for gripper joint position
+    gripper_joint_position_controller_node = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["gripper_joint_position_controller", "--inactive", "-c", "/controller_manager"],
+        output="screen",
+    )
+
+    # ros2_control controller for gripper joint effort
+    gripper_joint_effort_controller_node = Node(
+            package="controller_manager",
+            executable="spawner",
+            arguments=["gripper_joint_effort_controller", "-c", "/controller_manager"],
+            output="screen",
+    )
+
     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_joint_state_broadcaster_node)
+    ld.add_action(motor_head_joint_position_controller_node)
+    ld.add_action(motor_head_joint_effort_controller_node)
     ld.add_action(tendon_transmission_node)
+    ld.add_action(gripper_joint_state_broadcaster_node)
+    ld.add_action(gripper_joint_position_controller_node)
+    ld.add_action(gripper_joint_effort_controller_node)
 
     return ld
diff --git a/helix_description/config/controllers.yaml b/helix_description/config/controllers.yaml
@@ -11,6 +11,15 @@ controller_manager:
     motor_head_joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
+    gripper_joint_position_controller:
+      type: position_controllers/JointGroupPositionController
+
+    gripper_joint_effort_controller: 
+      type: effort_controllers/JointGroupEffortController
+
+    gripper_joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
 motor_head_joint_position_controller:
   ros__parameters:
     joints: # TODO - descriptive names for joints/motors?
@@ -40,3 +49,37 @@ motor_head_joint_effort_controller:
 motor_head_joint_state_broadcaster:
   ros__parameters:
     use_local_topics: True
+    joints:
+      - joint0
+      - joint1
+      - joint2
+      - joint3
+      - joint4
+      - joint5
+      - joint6
+      - joint7
+      - joint8
+    interfaces:
+      - position
+      - velocity
+      - effort
+
+gripper_joint_position_controller:
+  ros__parameters:
+    joints:
+      - joint_gripper
+
+gripper_joint_effort_controller:
+  ros__parameters:
+    joints:
+      - joint_gripper
+
+gripper_joint_state_broadcaster:
+  ros__parameters:
+    use_local_topics: True
+    joints:
+      - joint_gripper
+    interfaces:
+      - position
+      - velocity
+      - effort
diff --git a/helix_description/urdf/helix.ros2_control.xacro b/helix_description/urdf/helix.ros2_control.xacro
@@ -84,6 +84,14 @@
         <state_interface name="velocity"/>
         <state_interface name="effort"/>
       </joint>
+      <joint name="joint_gripper">
+        <param name="id">9</param>
+        <command_interface name="position"/>
+        <command_interface name="effort"/>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+        <state_interface name="effort"/>
+      </joint>
     </ros2_control>
 
   </xacro:macro>

diff --git a/helix_description/urdf/motor_head.xacro b/helix_description/urdf/motor_head.xacro
@@ -211,6 +211,25 @@
     </visual>
   </link>
 
+  <!-- Gripper Joint -->
+  <joint name="joint_gripper" type="continuous">
+    <parent link="base"/>
+    <child link="link_gripper"/>
+    <origin xyz="0 0 0.2" rpy="0 0 0"/>
+    <axis xyz="1 0 0"/>
+  </joint>
+
+   <!-- Gripper Link -->
+  <link name="link_gripper">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 1.57 0"/>
+      <geometry>
+        <cylinder length="0.015" radius="0.02"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+  </link>
+
 </xacro:macro>
 
 </robot>