gripper controlled by ps5 controller

README.md

@@ -90,4 +90,5 @@ Terminal 4:
 ros2 control switch_controllers --deactivate joint_trajectory_controller
 ros2 control switch_controllers --deactivate robotiq_gripper_joint_trajectory_controller
 ros2 control switch_controllers --activate forward_position_controller
+ros2 control switch_controllers --activate forward_gripper_position_controller
 ```

src/ur_robotiq_description/config/ur_robotiq_controllers.yaml

@@ -24,6 +24,9 @@ controller_manager:
     forward_position_controller:
       type: position_controllers/JointGroupPositionController
 
+    forward_gripper_position_controller:
+      type: position_controllers/JointGroupPositionController
+
     robotiq_gripper_controller:
       type: position_controllers/GripperActionController
 
@@ -33,10 +36,6 @@ controller_manager:
     robotiq_activation_controller:
       type: robotiq_controllers/RobotiqActivationController
 
-    combined_joint_trajectory_controller:
-      type: joint_trajectory_controller/JointTrajectoryController
-
-
 speed_scaling_state_broadcaster:
   ros__parameters:
     state_publish_rate: 100.0
@@ -137,6 +136,11 @@ forward_position_controller:
       - $(var tf_prefix)wrist_2_joint
       - $(var tf_prefix)wrist_3_joint
 
+forward_gripper_position_controller:
+  ros__parameters:
+    joints:
+      - $(var tf_prefix)finger_joint
+
 
 robotiq_gripper_controller:
   ros__parameters:
@@ -163,33 +167,4 @@ robotiq_gripper_joint_trajectory_controller:
 
 robotiq_activation_controller:
   ros__parameters:
-    default: true
+    default: true
-
-combined_joint_trajectory_controller:
-  ros__parameters:
-    joints:
-      - $(var tf_prefix)shoulder_pan_joint
-      - $(var tf_prefix)shoulder_lift_joint
-      - $(var tf_prefix)elbow_joint
-      - $(var tf_prefix)wrist_1_joint
-      - $(var tf_prefix)wrist_2_joint
-      - $(var tf_prefix)wrist_3_joint
-      - $(var tf_prefix)finger_joint
-    command_interfaces:
-      - position
-    state_interfaces:
-      - position
-      - velocity
-    state_publish_rate: 100.0
-    action_monitor_rate: 20.0
-    allow_partial_joints_goal: false
-    constraints:
-      stopped_velocity_tolerance: 0.2
-      goal_time: 0.0
-      $(var tf_prefix)shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 }
-      $(var tf_prefix)shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 }
-      $(var tf_prefix)elbow_joint: { trajectory: 0.2, goal: 0.1 }
-      $(var tf_prefix)wrist_1_joint: { trajectory: 0.2, goal: 0.1 }
-      $(var tf_prefix)wrist_2_joint: { trajectory: 0.2, goal: 0.1 }
-      $(var tf_prefix)wrist_3_joint: { trajectory: 0.2, goal: 0.1 }
-      $(var tf_prefix)finger_joint: { trajectory: 0.2, goal: 0.1 }

src/ur_robotiq_description/launch/ur_robotiq_control.launch.py

@@ -271,8 +271,6 @@ def launch_setup(context, *args, **kwargs):
                     "force_torque_sensor_broadcaster",
                     "joint_state_broadcaster",
                     "speed_scaling_state_broadcaster",
-                    "robotiq_gripper_controller",
-                    "robotiq_activation_controller",
                 ]
             },
         ],
@@ -306,8 +304,7 @@ def launch_setup(context, *args, **kwargs):
                           "/controller_manager",
                           "--controller-manager-timeout",
                           controller_spawner_timeout,
-                      ]
+                      ] + inactive_flags,
-                      + inactive_flags,
         )
 
     controller_spawner_names = [
@@ -316,7 +313,8 @@ def launch_setup(context, *args, **kwargs):
         "speed_scaling_state_broadcaster",
         "force_torque_sensor_broadcaster",
     ]
-    controller_spawner_inactive_names = ["forward_position_controller", "combined_joint_trajectory_controller"]
+    controller_spawner_inactive_names = ["forward_position_controller",
+                                         "forward_gripper_position_controller"]
 
     controller_spawners = [controller_spawner(name) for name in controller_spawner_names] + [
         controller_spawner(name, active=False) for name in controller_spawner_inactive_names

src/ur_robotiq_moveit_config/config/moveit_controllers.yaml

@@ -45,17 +45,3 @@ robotiq_gripper_joint_trajectory_controller:
   default: false
   joints:
     - finger_joint
-
-combined_joint_trajectory_controller:
-  type: FollowJointTrajectory
-  action_ns: follow_joint_trajectory
-  default: true
-  joints:
-    - shoulder_pan_joint
-    - shoulder_lift_joint
-    - elbow_joint
-    - wrist_1_joint
-    - wrist_2_joint
-    - wrist_3_joint
-    - finger_joint
-

src/ur_robotiq_servo/config/ps5-params.yaml

@@ -1,3 +1,6 @@
 ps5_servo_node:
     ros__parameters:
-      linear_speed_multiplier: 0.2
+      linear_speed_multiplier: 0.2
+      gripper_speed_multiplier: 0.02
+      gripper_lower_limit: 0.0
+      gripper_upper_limit: 0.70

src/ur_robotiq_servo/ur_robotiq_servo/ps5_control.py

@@ -1,9 +1,10 @@
 import rclpy
 from rclpy.node import Node
-from sensor_msgs.msg import Joy
+from sensor_msgs.msg import Joy, JointState
 from geometry_msgs.msg import TwistStamped
 from control_msgs.msg import JointJog
 from std_srvs.srv import Trigger
+from std_msgs.msg import Float64MultiArray, MultiArrayDimension, MultiArrayLayout
 
 
 class PS5ControllerNode(Node):
@@ -12,6 +13,7 @@ class PS5ControllerNode(Node):
         # States
         self.mode = 'twist'  # Initialize mode to 'twist'. Alternatives: 'twist', 'joint'
         self.last_button_state = 0  # Track the last state of the toggle button to detect presses
+        self.last_finger_joint_angle = 0.0
 
         # Parameters
         self.linear_speed_multiplier = self.declare_parameter('linear_speed_multiplier', 1.0)
@@ -22,7 +24,27 @@ class PS5ControllerNode(Node):
         self.use_fake_hardware = self.get_parameter('use_fake_hardware').get_parameter_value().bool_value
         self.get_logger().info(f"Use fake hardware: {self.use_fake_hardware}")
 
+        self.gripper_speed_multiplier = self.declare_parameter('gripper_speed_multiplier', 1.0)
+        self.gripper_speed_multiplier = (self.get_parameter('gripper_speed_multiplier')
+                                         .get_parameter_value().double_value)
+        self.get_logger().info(f"Gripper speed multiplier: {self.gripper_speed_multiplier}")
+
+        self.gripper_lower_limit = self.declare_parameter('gripper_lower_limit', 1.0)
+        self.gripper_lower_limit = (self.get_parameter('gripper_lower_limit')
+                                         .get_parameter_value().double_value)
+        self.get_logger().info(f"Gripper lower limit: {self.gripper_lower_limit}")
+
+        self.gripper_upper_limit = self.declare_parameter('gripper_upper_limit', 1.0)
+        self.gripper_upper_limit = (self.get_parameter('gripper_upper_limit')
+                                         .get_parameter_value().double_value)
+        self.get_logger().info(f"Gripper upper limit: {self.gripper_upper_limit}")
         # Subscriber and Publisher
+        self.joint_state_sub = self.create_subscription(
+            JointState,
+            '/joint_states',
+            self.joint_state_callback,
+            10)
+
         self.joy_sub = self.create_subscription(
             Joy,
             '/joy',
@@ -39,6 +61,11 @@ class PS5ControllerNode(Node):
             '/servo_node/delta_joint_cmds',
             10)
 
+        self.gripper_cmd_pub = self.create_publisher(
+            Float64MultiArray,
+            '/forward_gripper_position_controller/commands',
+            10)
+
         # Services
         self.servo_client = self.create_client(Trigger, '/servo_node/start_servo')
 
@@ -60,6 +87,13 @@ class PS5ControllerNode(Node):
         else:
             self.get_logger().info('Failed to call start_servo')
 
+    def joint_state_callback(self, msg):
+        try:
+            index = msg.name.index('finger_joint')
+            self.last_finger_joint_angle = msg.position[index]
+        except ValueError:
+            self.get_logger().error('finger_joint not found in /joint_states msg')
+
     def joy_callback(self, msg):
         # Check for button press to toggle mode
         # Assuming button 2 (e.g., Triangle on PS5) for toggling mode
@@ -104,6 +138,23 @@ class PS5ControllerNode(Node):
                                     (msg.buttons[13] - msg.buttons[14]) * 1.0]
             self.joint_pub.publish(joint_msg)
 
+        # Gripper controller
+        new_finger_joint_angle = (self.last_finger_joint_angle +
+                                  (msg.buttons[1] - msg.buttons[0]) * self.gripper_speed_multiplier)
+        if self.gripper_lower_limit > new_finger_joint_angle or self.gripper_upper_limit < new_finger_joint_angle:
+            self.get_logger().debug(f"New finger joint angle out of bounds: {new_finger_joint_angle}")
+            new_finger_joint_angle = self.last_finger_joint_angle
+        gripper_msg = Float64MultiArray()
+        layout_msg = MultiArrayLayout()
+        layout_msg.dim = [MultiArrayDimension()]
+        layout_msg.dim[0].label = "finger_joint"
+        layout_msg.dim[0].size = 1
+        layout_msg.dim[0].stride = 1
+        layout_msg.data_offset = 0
+        gripper_msg.layout = layout_msg
+        gripper_msg.data = [new_finger_joint_angle]
+        self.gripper_cmd_pub.publish(gripper_msg)
+
 
 def main(args=None):
     rclpy.init(args=args)