diff --git a/README.md b/README.md index d0b30d4..68964df 100644 --- a/README.md +++ b/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 ``` \ No newline at end of file diff --git a/src/ur_robotiq_description/config/ur_robotiq_controllers.yaml b/src/ur_robotiq_description/config/ur_robotiq_controllers.yaml index d6a2f5d..da32265 100644 --- a/src/ur_robotiq_description/config/ur_robotiq_controllers.yaml +++ b/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 - -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 } \ No newline at end of file + default: true \ No newline at end of file diff --git a/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py b/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py index d2a2f37..6771f73 100644 --- a/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py +++ b/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 diff --git a/src/ur_robotiq_moveit_config/config/moveit_controllers.yaml b/src/ur_robotiq_moveit_config/config/moveit_controllers.yaml index accd281..cfb0748 100644 --- a/src/ur_robotiq_moveit_config/config/moveit_controllers.yaml +++ b/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 - diff --git a/src/ur_robotiq_servo/config/ps5-params.yaml b/src/ur_robotiq_servo/config/ps5-params.yaml index 46f47e1..679a2f1 100644 --- a/src/ur_robotiq_servo/config/ps5-params.yaml +++ b/src/ur_robotiq_servo/config/ps5-params.yaml @@ -1,3 +1,6 @@ ps5_servo_node: ros__parameters: - linear_speed_multiplier: 0.2 \ No newline at end of file + linear_speed_multiplier: 0.2 + gripper_speed_multiplier: 0.02 + gripper_lower_limit: 0.0 + gripper_upper_limit: 0.70 \ No newline at end of file diff --git a/src/ur_robotiq_servo/ur_robotiq_servo/ps5_control.py b/src/ur_robotiq_servo/ur_robotiq_servo/ps5_control.py index fa259c6..7334328 100644 --- a/src/ur_robotiq_servo/ur_robotiq_servo/ps5_control.py +++ b/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)