diff --git a/helix_bringup/launch/helix_bringup.launch.py b/helix_bringup/launch/helix_bringup.launch.py
index 1c65a63..eea647b 100644
--- 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
\ No newline at end of file
diff --git a/helix_description/config/controllers.yaml b/helix_description/config/controllers.yaml
index 8529c68..31ec9b2 100644
--- 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
\ No newline at end of file
diff --git a/helix_description/urdf/helix.ros2_control.xacro b/helix_description/urdf/helix.ros2_control.xacro
index 6675834..e701269 100644
--- a/helix_description/urdf/helix.ros2_control.xacro
+++ b/helix_description/urdf/helix.ros2_control.xacro
@@ -84,6 +84,14 @@
+
+ 9
+
+
+
+
+
+
diff --git a/helix_description/urdf/motor_head.xacro b/helix_description/urdf/motor_head.xacro
index 870ec92..4fac954 100644
--- a/helix_description/urdf/motor_head.xacro
+++ b/helix_description/urdf/motor_head.xacro
@@ -211,6 +211,25 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file