From 5077372894e9e427c72c7f31dae61cf5255fa2dc Mon Sep 17 00:00:00 2001 From: NikoFeith Date: Mon, 13 May 2024 17:38:57 +0200 Subject: [PATCH] Real robot works with servo gripper needs different update loop and sometimes has too much current and then the whole robot goes into error recovery mode --- README.md | 2 +- .../urdf/robotiq_2f_140_macro.urdf.xacro | 30 +++++++++---------- .../urdf/robotiq_2f_85_macro.urdf.xacro | 27 ----------------- .../src/hardware_interface.cpp | 2 +- .../launch/ur_robotiq_control.launch.py | 8 ----- .../config/moveit_controllers.yaml | 8 ++--- .../config/ur_servo.yaml | 7 +++-- .../srdf/ur_robotiq_macro.srdf.xacro | 3 +- src/ur_robotiq_servo/config/joy-params.yaml | 2 +- src/ur_robotiq_servo/config/ps5-params.yaml | 4 +-- .../ur_robotiq_servo/ps5_control.py | 14 ++++----- 11 files changed, 36 insertions(+), 71 deletions(-) diff --git a/README.md b/README.md index 68964df..3697e70 100644 --- a/README.md +++ b/README.md @@ -88,7 +88,7 @@ After launching you need to change the controller to enable the servo mode.\ Terminal 4: ```bash ros2 control switch_controllers --deactivate joint_trajectory_controller -ros2 control switch_controllers --deactivate robotiq_gripper_joint_trajectory_controller +ros2 control switch_controllers --deactivate robotiq_gripper_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/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro index c54f805..031e32d 100644 --- a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro +++ b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro @@ -436,8 +436,8 @@ - - + + @@ -446,25 +446,23 @@ - - - - - - - - - + + + + + - - - - - + + + + + + + diff --git a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85_macro.urdf.xacro b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85_macro.urdf.xacro index afdd6fb..f55b6d1 100644 --- a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85_macro.urdf.xacro +++ b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85_macro.urdf.xacro @@ -304,32 +304,5 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp b/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp index b5e3878..c242270 100644 --- a/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp +++ b/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp @@ -510,7 +510,7 @@ double RobotiqGripperHardwareInterface::convertRawToAngle(int raw_position) { if (std::isnan(max_angle_) || 0.0 >= max_angle_) { throw std::runtime_error("Invalid gripper angle limits: max_angle is not configured correctly."); } - double scaled_angle = (static_cast(raw_position) / 255.0) * max_angle_; + double scaled_angle = ((230 - static_cast(raw_position)) / 227.0) * max_angle_; return std::max(0.0, std::min(scaled_angle, max_angle_)); } 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 08e2ffb..9f43b31 100644 --- a/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py +++ b/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py @@ -217,13 +217,6 @@ def launch_setup(context, *args, **kwargs): package="controller_manager", executable="spawner", arguments=["robotiq_gripper_controller", "-c", "/controller_manager"], - condition=UnlessCondition(use_fake_hardware), - ) - robotiq_gripper_joint_trajectory_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["robotiq_gripper_joint_trajectory_controller", "-c", "/controller_manager"], - condition=IfCondition(use_fake_hardware), ) robotiq_activation_controller_spawner = Node( @@ -347,7 +340,6 @@ def launch_setup(context, *args, **kwargs): initial_joint_controller_spawner_stopped, initial_joint_controller_spawner_started, robotiq_gripper_controller_spawner, - robotiq_gripper_joint_trajectory_controller_spawner, robotiq_activation_controller_spawner, ] + controller_spawners diff --git a/src/ur_robotiq_moveit_config/config/moveit_controllers.yaml b/src/ur_robotiq_moveit_config/config/moveit_controllers.yaml index 2cedcfc..6814368 100644 --- a/src/ur_robotiq_moveit_config/config/moveit_controllers.yaml +++ b/src/ur_robotiq_moveit_config/config/moveit_controllers.yaml @@ -31,8 +31,8 @@ joint_trajectory_controller: robotiq_gripper_controller: - action_ns: robotiq_2f_controllers - type: RobotiqGripperCommandController + action_ns: gripper_cmd + type: GripperCommand default: true - joint: gripper_gap - max_gap: 0.14 + joints: + - gripper_gap diff --git a/src/ur_robotiq_moveit_config/config/ur_servo.yaml b/src/ur_robotiq_moveit_config/config/ur_servo.yaml index 75d5e2b..493ee9d 100644 --- a/src/ur_robotiq_moveit_config/config/ur_servo.yaml +++ b/src/ur_robotiq_moveit_config/config/ur_servo.yaml @@ -7,10 +7,10 @@ use_intra_process_comms : True command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s scale: # Scale parameters are only used if command_in_type=="unitless" - linear: 0.00001 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands. - rotational: 0.001 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands. + linear: 0.4 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands. + rotational: 0.8 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands. # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic. - joint: 0.01 + joint: 0.5 # This is a fudge factor to account for any latency in the system, e.g. network latency or poor low-level # controller performance. It essentially increases the timestep when calculating the target pose, to move the target # pose farther away. [seconds] @@ -38,6 +38,7 @@ low_pass_filter_coeff: 2.0 # Larger --> trust the filtered data more, trust the ## MoveIt properties move_group_name: ur_manipulator # Often 'manipulator' or 'arm' planning_frame: world # The MoveIt planning frame. Often 'base_link' or 'world' +is_primary_planning_scene_monitor: false ## Other frames ee_frame_name: tool0 # The name of the end effector link, used to return the EE pose diff --git a/src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro b/src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro index a4778c0..c4187e6 100644 --- a/src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro +++ b/src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro @@ -27,14 +27,15 @@ - + + diff --git a/src/ur_robotiq_servo/config/joy-params.yaml b/src/ur_robotiq_servo/config/joy-params.yaml index 8ad2d13..3119f4f 100644 --- a/src/ur_robotiq_servo/config/joy-params.yaml +++ b/src/ur_robotiq_servo/config/joy-params.yaml @@ -1,7 +1,7 @@ joy_node: ros__parameters: device_id: 0 - device_name: "PS5 Controller" + device_name: "Wireless Controller" deadzone: 0.5 autorepeat_rate: 20.0 sticky_buttons: false diff --git a/src/ur_robotiq_servo/config/ps5-params.yaml b/src/ur_robotiq_servo/config/ps5-params.yaml index 1cc29a7..01da6c4 100644 --- a/src/ur_robotiq_servo/config/ps5-params.yaml +++ b/src/ur_robotiq_servo/config/ps5-params.yaml @@ -1,6 +1,6 @@ ps5_servo_node: ros__parameters: - linear_speed_multiplier: 0.2 - gripper_position_multiplier: 0.02 + linear_speed_multiplier: 0.5 + gripper_position_multiplier: 0.004 gripper_lower_limit: 0.0 gripper_upper_limit: 0.14 \ 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 639d9ad..da78597 100644 --- a/src/ur_robotiq_servo/ur_robotiq_servo/ps5_control.py +++ b/src/ur_robotiq_servo/ur_robotiq_servo/ps5_control.py @@ -97,13 +97,13 @@ class PS5ControllerNode(Node): def joy_callback(self, msg): # Check for button press to toggle mode # Assuming button 2 (e.g., Triangle on PS5) for toggling mode - current_button_state = msg.buttons[16] + current_button_state = msg.buttons[8] if current_button_state == 1 and self.last_button_state == 0: self.mode = 'joint' if self.mode == 'twist' else 'twist' self.get_logger().info(f'Mode switched to: {self.mode}') self.last_button_state = current_button_state - left_trigger = (msg.axes[4] - 1) / - 2.0 + left_trigger = (msg.axes[2] - 1) / - 2.0 right_trigger = (msg.axes[5] - 1) / - 2.0 # Process control input based on current mode @@ -115,8 +115,8 @@ class PS5ControllerNode(Node): twist_msg.twist.linear.y = -msg.axes[1] * self.linear_speed_multiplier twist_msg.twist.linear.z = (left_trigger - right_trigger) * self.linear_speed_multiplier twist_msg.twist.angular.x = msg.axes[3] - twist_msg.twist.angular.y = msg.axes[2] - twist_msg.twist.angular.z = (msg.buttons[9] - msg.buttons[10]) * 1.0 + twist_msg.twist.angular.y = msg.axes[4] + twist_msg.twist.angular.z = (msg.buttons[4] - msg.buttons[5]) * 1.0 self.twist_pub.publish(twist_msg) elif self.mode == 'joint': joint_msg = JointJog() @@ -132,10 +132,10 @@ class PS5ControllerNode(Node): ] joint_msg.velocities = [msg.axes[0], msg.axes[1], - msg.axes[2], msg.axes[3], - (msg.buttons[11] - msg.buttons[12]) * 1.0, - (msg.buttons[13] - msg.buttons[14]) * 1.0] + msg.axes[4], + msg.axes[6], + msg.axes[7]] self.joint_pub.publish(joint_msg) # Gripper controller