From bcba4ef3d229a4ffef3ce354bde7249d2f2b4241 Mon Sep 17 00:00:00 2001 From: NikoFeith Date: Mon, 13 May 2024 11:34:34 +0200 Subject: [PATCH] controller from ur_robotiq_control.launch.py starts up ur_robotiq_moveit.launch.py cant find gripper_gap joint --- .idea/vcs.xml | 1 - .../urdf/robotiq_2f_140_macro.urdf.xacro | 2 +- .../urdf/ur_robotiq.urdf.xacro | 2 +- .../config/joint_limits.yaml | 122 ++++++++++++------ .../config/moveit_controllers.yaml | 1 - .../config/ur_servo.yaml | 2 +- .../launch/ur_robotiq_moveit.launch.py | 66 ++++++++-- .../srdf/ur_robotiq_macro.srdf.xacro | 4 +- src/ur_robotiq_servo/config/ps5-params.yaml | 4 +- src/ur_robotiq_servo/package.xml | 1 + .../ur_robotiq_servo/ps5_control.py | 35 +++-- 11 files changed, 157 insertions(+), 83 deletions(-) diff --git a/.idea/vcs.xml b/.idea/vcs.xml index 4dc5475..ea0d475 100644 --- a/.idea/vcs.xml +++ b/.idea/vcs.xml @@ -4,6 +4,5 @@ - \ 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 37f50ee..c54f805 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 @@ -12,7 +12,7 @@ robot_port:=63352"> - + diff --git a/src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro b/src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro index be5d28c..1048d96 100644 --- a/src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro +++ b/src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro @@ -1,7 +1,7 @@ - + diff --git a/src/ur_robotiq_moveit_config/config/joint_limits.yaml b/src/ur_robotiq_moveit_config/config/joint_limits.yaml index 9021a01..588aec6 100644 --- a/src/ur_robotiq_moveit_config/config/joint_limits.yaml +++ b/src/ur_robotiq_moveit_config/config/joint_limits.yaml @@ -1,50 +1,90 @@ -# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed - -# For beginners, we downscale velocity and acceleration limits. -# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. -default_velocity_scaling_factor: 0.1 -default_acceleration_scaling_factor: 0.1 - -# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] -# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] +# Joints limits +# +# Sources: +# +# - Universal Robots e-Series, User Manual, UR3e, Version 5.8 +# https://s3-eu-west-1.amazonaws.com/ur-support-site/69043/99403_UR3e_User_Manual_en_Global.pdf +# - Support > Articles > UR articles > Max. joint torques +# https://www.universal-robots.com/articles/ur-articles/max-joint-torques +# retrieved: 2020-06-16, last modified: 2020-06-09 joint_limits: - elbow_joint: - has_velocity_limits: true - max_velocity: 3.1415926535897931 - has_acceleration_limits: false - max_acceleration: 0 - left_inner_knuckle_joint: - has_velocity_limits: true - max_velocity: 2 - has_acceleration_limits: false - max_acceleration: 0 - right_inner_knuckle_joint: - has_velocity_limits: true - max_velocity: 2 - has_acceleration_limits: false - max_acceleration: 0 - shoulder_lift_joint: - has_velocity_limits: true - max_velocity: 3.1415926535897931 - has_acceleration_limits: false - max_acceleration: 0 shoulder_pan_joint: - has_velocity_limits: true - max_velocity: 3.1415926535897931 + # acceleration limits are not publicly available has_acceleration_limits: false - max_acceleration: 0 + has_effort_limits: true + has_position_limits: true + has_velocity_limits: true + max_effort: 56.0 + max_position: !degrees 360.0 + max_velocity: !degrees 180.0 + min_position: !degrees -360.0 + shoulder_lift_joint: + # acceleration limits are not publicly available + has_acceleration_limits: false + has_effort_limits: true + has_position_limits: true + has_velocity_limits: true + max_effort: 56.0 + max_position: !degrees 360.0 + max_velocity: !degrees 180.0 + min_position: !degrees -360.0 + elbow_joint: + # acceleration limits are not publicly available + has_acceleration_limits: false + has_effort_limits: true + has_position_limits: true + has_velocity_limits: true + max_effort: 28.0 + # we artificially limit this joint to half its actual joint position limit + # to avoid (MoveIt/OMPL) planning problems, as due to the physical + # construction of the robot, it's impossible to rotate the 'elbow_joint' + # over more than approx +- 1 pi (the shoulder lift joint gets in the way). + # + # This leads to planning problems as the search space will be divided into + # two sections, with no connections from one to the other. + # + # Refer to https://github.com/ros-industrial/universal_robot/issues/265 for + # more information. + max_position: !degrees 180.0 + max_velocity: !degrees 180.0 + min_position: !degrees -180.0 wrist_1_joint: - has_velocity_limits: true - max_velocity: 6.2831853071795862 + # acceleration limits are not publicly available has_acceleration_limits: false - max_acceleration: 0 + has_effort_limits: true + has_position_limits: true + has_velocity_limits: true + max_effort: 12.0 + max_position: !degrees 360.0 + max_velocity: !degrees 360.0 + min_position: !degrees -360.0 wrist_2_joint: - has_velocity_limits: true - max_velocity: 6.2831853071795862 + # acceleration limits are not publicly available has_acceleration_limits: false - max_acceleration: 0 + has_effort_limits: true + has_position_limits: true + has_velocity_limits: true + max_effort: 12.0 + max_position: !degrees 360.0 + max_velocity: !degrees 360.0 + min_position: !degrees -360.0 wrist_3_joint: - has_velocity_limits: true - max_velocity: 6.2831853071795862 + # acceleration limits are not publicly available has_acceleration_limits: false - max_acceleration: 0 \ No newline at end of file + has_effort_limits: true + has_position_limits: true + has_velocity_limits: true + max_effort: 12.0 + max_position: !degrees 360.0 + max_velocity: !degrees 360.0 + min_position: !degrees -360.0 + gripper_gap: + # acceleration limits are not publicly available + has_acceleration_limits: false + has_effort_limits: true + has_position_limits: true + has_velocity_limits: true + max_effort: 235.0 + max_position: 0.14 + max_velocity: 0.15 + min_position: 0.0 \ No newline at end of file diff --git a/src/ur_robotiq_moveit_config/config/moveit_controllers.yaml b/src/ur_robotiq_moveit_config/config/moveit_controllers.yaml index d36929b..2cedcfc 100644 --- a/src/ur_robotiq_moveit_config/config/moveit_controllers.yaml +++ b/src/ur_robotiq_moveit_config/config/moveit_controllers.yaml @@ -1,4 +1,3 @@ - controller_names: - scaled_joint_trajectory_controller - joint_trajectory_controller diff --git a/src/ur_robotiq_moveit_config/config/ur_servo.yaml b/src/ur_robotiq_moveit_config/config/ur_servo.yaml index 8e64d56..75d5e2b 100644 --- a/src/ur_robotiq_moveit_config/config/ur_servo.yaml +++ b/src/ur_robotiq_moveit_config/config/ur_servo.yaml @@ -2,7 +2,7 @@ # Modify all parameters related to servoing here ############################################### use_gazebo: false # Whether the robot is started in a Gazebo simulation environment -use_intra_process_comms : true +use_intra_process_comms : True ## Properties of incoming commands 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: diff --git a/src/ur_robotiq_moveit_config/launch/ur_robotiq_moveit.launch.py b/src/ur_robotiq_moveit_config/launch/ur_robotiq_moveit.launch.py index 6e1d62b..11d2a9c 100644 --- a/src/ur_robotiq_moveit_config/launch/ur_robotiq_moveit.launch.py +++ b/src/ur_robotiq_moveit_config/launch/ur_robotiq_moveit.launch.py @@ -40,17 +40,17 @@ def launch_setup(context, *args, **kwargs): launch_servo = LaunchConfiguration("launch_servo") # File and package names - ur_description_package = "ur_description" - ur_robotiq_description_package = "ur_robotiq_description" - ur_robotiq_description_file = "ur_robotiq.urdf.xacro" - moveit_config_package = "ur_robotiq_moveit_config" - moveit_config_file = "ur_robotiq.srdf.xacro" + ur_description_package = LaunchConfiguration("ur_description_package") + ur_robotiq_description_package = LaunchConfiguration("ur_robotiq_description_package") + ur_robotiq_description_file = LaunchConfiguration("ur_robotiq_description_file") + moveit_config_package = LaunchConfiguration("moveit_config_package") + moveit_config_file = LaunchConfiguration("moveit_config_file") joint_limit_params = PathJoinSubstitution( [FindPackageShare(ur_description_package), "config", ur_type, "joint_limits.yaml"] ) kinematics_params = PathJoinSubstitution( - [FindPackageShare(ur_description_package), "config", ur_type, "default_kinematics.yaml"] + [FindPackageShare(ur_robotiq_description_package), "config", "ur_robotiq_calibration.yaml"] ) physical_params = PathJoinSubstitution( [FindPackageShare(ur_description_package), "config", ur_type, "physical_parameters.yaml"] @@ -59,6 +59,8 @@ def launch_setup(context, *args, **kwargs): [FindPackageShare(ur_description_package), "config", ur_type, "visual_parameters.yaml"] ) + + robot_description_content = Command( [ PathJoinSubstitution([FindExecutable(name="xacro")]), @@ -126,7 +128,7 @@ def launch_setup(context, *args, **kwargs): robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content} - robot_description_kinematics_params = load_yaml(moveit_config_package, "config/kinematics.yaml") + robot_description_kinematics_params = load_yaml("ur_robotiq_moveit_config", "config/kinematics.yaml") robot_description_kinematics = {"robot_description_kinematics": robot_description_kinematics_params} # robot_description_planning = { # "robot_description_planning": load_yaml(ur_moveit_config_package, "config/joint_limits.yaml") @@ -140,16 +142,15 @@ def launch_setup(context, *args, **kwargs): "start_state_max_bounds_error": 0.1, } } - - ompl_planning_yaml = load_yaml(moveit_config_package, "config/ompl_planning.yaml") + ompl_planning_yaml = load_yaml("ur_robotiq_moveit_config", "config/ompl_planning.yaml") ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml) # Trajectory Execution Configuration # the scaled_joint_trajectory_controller does not work on fake hardware use_fake_hw = use_fake_hardware.perform(context) - controllers_yaml = load_yaml(moveit_config_package, "config/moveit_controllers.yaml") + controllers_yaml = load_yaml("ur_robotiq_moveit_config", "config/moveit_controllers.yaml") - if use_fake_hw == "true": + if use_fake_hw == "true" or use_fake_hw == "True": controllers_yaml["scaled_joint_trajectory_controller"]["default"] = False controllers_yaml["joint_trajectory_controller"]["default"] = True @@ -219,7 +220,7 @@ def launch_setup(context, *args, **kwargs): # Servo node for realtime control - servo_yaml = load_yaml(moveit_config_package, "config/ur_servo.yaml") + servo_yaml = load_yaml("ur_robotiq_moveit_config", "config/ur_servo.yaml") servo_params = {"moveit_servo": servo_yaml} servo_node = Node( package="moveit_servo", @@ -241,7 +242,46 @@ def launch_setup(context, *args, **kwargs): def generate_launch_description(): # Declare the Launch arguments declared_arguments = [] - # UR specific arguments + + declared_arguments.append( + DeclareLaunchArgument( + "ur_description_package", + description="Description package with robot URDF/XACRO files. Usually the argument \ + is not set, it enables use of a custom description.", + default_value="ur_description", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "ur_robotiq_description_package", + description="Description package with robot and gripper URDF/XACRO files. Usually the argument \ + is not set, it enables use of a custom description.", + default_value="ur_robotiq_description", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "ur_robotiq_description_file", + description="URDF/XACRO description file with the robot.", + default_value="ur_robotiq.urdf.xacro", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "moveit_config_package", + description="MoveIt config package with robot and gripper SRDF/XACRO files. Usually the argument \ + is not set, it enables use of a custom moveit config.", + default_value="ur_robotiq_moveit_config", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "moveit_config_file", + description="MoveIt SRDF/XACRO description file with the robot and gripper.", + default_value="ur_robotiq.srdf.xacro", + ) + ) + declared_arguments.append( DeclareLaunchArgument( "ur_type", 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 c1a3f4c..a4778c0 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 @@ -30,10 +30,10 @@ - + - + diff --git a/src/ur_robotiq_servo/config/ps5-params.yaml b/src/ur_robotiq_servo/config/ps5-params.yaml index 679a2f1..1cc29a7 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_speed_multiplier: 0.02 + gripper_position_multiplier: 0.02 gripper_lower_limit: 0.0 - gripper_upper_limit: 0.70 \ No newline at end of file + gripper_upper_limit: 0.14 \ No newline at end of file diff --git a/src/ur_robotiq_servo/package.xml b/src/ur_robotiq_servo/package.xml index 3099325..65a77da 100644 --- a/src/ur_robotiq_servo/package.xml +++ b/src/ur_robotiq_servo/package.xml @@ -11,6 +11,7 @@ sensor_msgs geometry_msgs std_msgs + robotiq_2f_msgs std_srvs joy moveit_servo 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 7334328..639d9ad 100644 --- a/src/ur_robotiq_servo/ur_robotiq_servo/ps5_control.py +++ b/src/ur_robotiq_servo/ur_robotiq_servo/ps5_control.py @@ -4,7 +4,7 @@ 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 +from robotiq_2f_msgs.msg import ForwardCommand class PS5ControllerNode(Node): @@ -24,17 +24,17 @@ 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') + self.gripper_position_multiplier = self.declare_parameter('gripper_position_multiplier', 1.0) + self.gripper_position_multiplier = (self.get_parameter('gripper_position_multiplier') .get_parameter_value().double_value) - self.get_logger().info(f"Gripper speed multiplier: {self.gripper_speed_multiplier}") + self.get_logger().info(f"Gripper position multiplier: {self.gripper_position_multiplier}") - self.gripper_lower_limit = self.declare_parameter('gripper_lower_limit', 1.0) + self.gripper_lower_limit = self.declare_parameter('gripper_lower_limit', 0.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.declare_parameter('gripper_upper_limit', 0.14) 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}") @@ -62,15 +62,15 @@ class PS5ControllerNode(Node): 10) self.gripper_cmd_pub = self.create_publisher( - Float64MultiArray, - '/forward_gripper_position_controller/commands', + ForwardCommand, + '/forward_gripper_position_controller/command', 10) # Services self.servo_client = self.create_client(Trigger, '/servo_node/start_servo') srv_msg = Trigger.Request() - while not self.servo_client.wait_for_service(timeout_sec=1.0): + while not self.servo_client.wait_for_service(timeout_sec=5.0): self.get_logger().info('/servo_node/start_servo service not available, waiting again...') self.call_start_servo() @@ -89,7 +89,7 @@ class PS5ControllerNode(Node): def joint_state_callback(self, msg): try: - index = msg.name.index('finger_joint') + index = msg.name.index('gripper_gap') self.last_finger_joint_angle = msg.position[index] except ValueError: self.get_logger().error('finger_joint not found in /joint_states msg') @@ -140,19 +140,14 @@ class PS5ControllerNode(Node): # Gripper controller new_finger_joint_angle = (self.last_finger_joint_angle + - (msg.buttons[1] - msg.buttons[0]) * self.gripper_speed_multiplier) + (msg.buttons[1] - msg.buttons[0]) * self.gripper_position_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] + gripper_msg = ForwardCommand() + gripper_msg.position = new_finger_joint_angle + gripper_msg.max_velocity = 0.15 + gripper_msg.max_effort = 235.0 self.gripper_cmd_pub.publish(gripper_msg)