From 5d45c4ee6e5a14677cfcaf27d417d77fc8c14e18 Mon Sep 17 00:00:00 2001 From: Niko Date: Thu, 4 Apr 2024 18:33:56 +0200 Subject: [PATCH] servo working without collision checking for twist and joints gripper not included in servo --- README.md | 17 +- .../ur_robotiq_cartesian_controllers.yaml | 233 ++++++++++++++++++ .../config/ur_robotiq_controllers.yaml | 2 +- .../ur_robotiq_cartesian_control.launch.py | 0 .../launch/ur_robotiq_control.launch.py | 2 +- .../config/initial_positions.yaml | 6 +- .../config/ur_servo.yaml | 18 +- ...srdf.xacro => ur_robotiq_macro.srdf.xacro} | 34 +-- src/ur_robotiq_servo/config/ps5-params.yaml | 3 + .../launch/ps5_servo.launch.py | 10 +- .../ur_robotiq_servo/ps5_control.py | 36 ++- 11 files changed, 312 insertions(+), 49 deletions(-) create mode 100644 src/ur_robotiq_description/config/ur_robotiq_cartesian_controllers.yaml create mode 100644 src/ur_robotiq_description/launch/ur_robotiq_cartesian_control.launch.py rename src/ur_robotiq_moveit_config/srdf/{ur_manipulator_macro.srdf.xacro => ur_robotiq_macro.srdf.xacro} (89%) create mode 100644 src/ur_robotiq_servo/config/ps5-params.yaml diff --git a/README.md b/README.md index 7fd07e0..d0b30d4 100644 --- a/README.md +++ b/README.md @@ -18,13 +18,15 @@ To download this repository and all of its included submodules, follow these ste * Navigate to the directory where you want to download the project. * Run the following Git command: - ```bash - git clone git@git.cps.unileoben.ac.at:Niko/UR_Robotiq.git - ``` + ```bash + git clone git@git.cps.unileoben.ac.at:Niko/UR_Robotiq.git + ``` 4. **Download necessary repos** ```bash vcs import src < UR_Robotiq.humble.repos ``` + The packages cartesian_controller_simulation and cartesian_controller_tests are excluded from colcon build by default. + If you want to build them to be build delete the COLCON_IGNORE file! ## How to Start the Docker Compose 1. **Prerequisites** @@ -33,7 +35,7 @@ To download this repository and all of its included submodules, follow these ste 2. **Starting the Containers** * Navigate to the project directory in your terminal: ```bash - cd UR-Robotiq + cd ``` * Run the following command to start the Docker Compose: @@ -76,7 +78,8 @@ ros2 launch ur_robotiq_moveit_config ur_robotiq_moveit.launch.py use_fake_hardwa ``` ### servoing -Terminal 3: +Terminal 3:\ +**At the moment the servoing doesnt work with the collision checker (self collision still works).** ```bash ros2 launch ur_robotiq_servo ps5_servo.launch.py ``` @@ -84,5 +87,7 @@ ros2 launch ur_robotiq_servo ps5_servo.launch.py After launching you need to change the controller to enable the servo mode.\ Terminal 4: ```bash -ros2 control switch_controllers --deactivate joint_trajectory_controller --activate forward_position_controller +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 ``` \ No newline at end of file diff --git a/src/ur_robotiq_description/config/ur_robotiq_cartesian_controllers.yaml b/src/ur_robotiq_description/config/ur_robotiq_cartesian_controllers.yaml new file mode 100644 index 0000000..2ed83db --- /dev/null +++ b/src/ur_robotiq_description/config/ur_robotiq_cartesian_controllers.yaml @@ -0,0 +1,233 @@ +controller_manager: + ros__parameters: + force_torque_sensor_broadcaster: + type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster + + scaled_joint_trajectory_controller: + type: ur_controllers/ScaledJointTrajectoryController + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + cartesian_compliance_controller: + type: cartesian_compliance_controller/CartesianComplianceController + + cartesian_force_controller: + type: cartesian_force_controller/CartesianForceController + + cartesian_motion_controller: + type: cartesian_motion_controller/CartesianMotionController + + motion_control_handle: + type: cartesian_controller_handles/MotionControlHandle + + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + +force_torque_sensor_broadcaster: + ros__parameters: + sensor_name: tcp_fts_sensor + state_interface_names: + - force.x + - force.y + - force.z + - torque.x + - torque.y + - torque.z + frame_id: tool0 + topic_name: wrench + +scaled_joint_trajectory_controller: + ros__parameters: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_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 + shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } + shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } + elbow_joint: { trajectory: 0.2, goal: 0.1 } + wrist_1_joint: { trajectory: 0.2, goal: 0.1 } + wrist_2_joint: { trajectory: 0.2, goal: 0.1 } + wrist_3_joint: { trajectory: 0.2, goal: 0.1 } + +cartesian_compliance_controller: + ros__parameters: + + # This is the tip of the robot tool that you usually use for your task. + # For instance, it could be the drilling bit of a screwdriver or a grinding + # tool. When you specify a target_wrench, i.e. some additional forces that + # your robot should apply to its environment, that target_wrench gets + # applied in this frame. + end_effector_link: "tool0" + + # This is usually the link directly before the first actuated joint. All + # controllers will build a kinematic chain from this link up to + # end_effector_link. It's also the reference frame for the superposition + # of error components in all controllers. + robot_base_link: "base_link" + + # This is the URDF link of your sensor. Sensor signals are assumed to be + # given in this frame. It's important that this link is located somewhere + # between end_effector_link and robot_base_link. If that's not the case, + # the controllers won't initialize and will emit an error message. + ft_sensor_ref_link: "tool0" + + # This is the link that the robot feels compliant about. It does not need + # to coincide with the end_effector_link, but for many use cases, this + # configuration is handy. When working with a screwdriver, for instance, + # setting compliance_ref_link == end_effector_link makes it easy to specify + # downward pushing forces without generating unwanted offset moments. + # On the other hand, an application could benefit from yielding a little in + # the robot's wrist while drawing a line on a surface with a pen. + compliance_ref_link: "tool0" + + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + + # Choose between position or velocity. In fact, the controllers allow to + # set both at the same time, but not all drivers will support this. + # In general, position control is a little smoother due to the double + # time-integrated commands from the solver. If available on a robot, it + # should be the default. On some drivers, the velocity interface provides + # faster control cycles, and hence could improve stability in + # contact-dominated tasks. A drawback is that we lose one time integration + # step here and obtain noisier command signals in comparison to the + # position interface. It's probably suitable to test both on a new robot + # and decide for what works best. + command_interfaces: + - position + #- velocity + + stiffness: # w.r.t. compliance_ref_link coordinates + trans_x: 500.0 + trans_y: 500.0 + trans_z: 500.0 + rot_x: 20.0 + rot_y: 20.0 + rot_z: 20.0 + + solver: + error_scale: 0.5 + iterations: 1 + publish_state_feedback: True + + # For all controllers, these gains are w.r.t. the robot_base_link coordinates. + pd_gains: + trans_x: {p: 0.05, d: 0.005} + trans_y: {p: 0.05, d: 0.005} + trans_z: {p: 0.05, d: 0.005} + rot_x: {p: 1.5} + rot_y: {p: 1.5} + rot_z: {p: 1.5} + +cartesian_force_controller: + ros__parameters: + + # See the cartesian_compliance_controller + end_effector_link: "tool0" + robot_base_link: "base_link" + ft_sensor_ref_link: "tool0" + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + + # See the cartesian_compliance_controller + command_interfaces: + - position + #- velocity + + solver: + error_scale: 0.5 + publish_state_feedback: True + + pd_gains: + trans_x: {p: 0.05} + trans_y: {p: 0.05} + trans_z: {p: 0.05} + rot_x: {p: 1.5} + rot_y: {p: 1.5} + rot_z: {p: 1.5} + +cartesian_motion_controller: + ros__parameters: + + # See the cartesian_compliance_controller + end_effector_link: "tool0" + robot_base_link: "base_link" + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + + # See the cartesian_compliance_controller + command_interfaces: + - position + #- velocity + + solver: + error_scale: 1.0 + iterations: 10 + publish_state_feedback: True + + pd_gains: + trans_x: {p: 1.0} + trans_y: {p: 1.0} + trans_z: {p: 1.0} + rot_x: {p: 0.5} + rot_y: {p: 0.5} + rot_z: {p: 0.5} + +motion_control_handle: + ros__parameters: + end_effector_link: "tool0" + robot_base_link: "base_link" + ft_sensor_ref_link: "tool0" + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + +joint_trajectory_controller: + ros__parameters: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + command_interfaces: + - position + state_interfaces: + - position + - velocity \ 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 6ecfce1..d6a2f5d 100644 --- a/src/ur_robotiq_description/config/ur_robotiq_controllers.yaml +++ b/src/ur_robotiq_description/config/ur_robotiq_controllers.yaml @@ -165,7 +165,7 @@ robotiq_activation_controller: ros__parameters: default: true -combined_trajectory_controller: +combined_joint_trajectory_controller: ros__parameters: joints: - $(var tf_prefix)shoulder_pan_joint diff --git a/src/ur_robotiq_description/launch/ur_robotiq_cartesian_control.launch.py b/src/ur_robotiq_description/launch/ur_robotiq_cartesian_control.launch.py new file mode 100644 index 0000000..e69de29 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 c391a22..d2a2f37 100644 --- a/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py +++ b/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py @@ -316,7 +316,7 @@ def launch_setup(context, *args, **kwargs): "speed_scaling_state_broadcaster", "force_torque_sensor_broadcaster", ] - controller_spawner_inactive_names = ["forward_position_controller"] + controller_spawner_inactive_names = ["forward_position_controller", "combined_joint_trajectory_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/initial_positions.yaml b/src/ur_robotiq_moveit_config/config/initial_positions.yaml index fa41209..dcd628e 100644 --- a/src/ur_robotiq_moveit_config/config/initial_positions.yaml +++ b/src/ur_robotiq_moveit_config/config/initial_positions.yaml @@ -1,7 +1,7 @@ -shoulder_pan_joint: 0.0 +shoulder_pan_joint: 1.4 shoulder_lift_joint: -1.57 -elbow_joint: 0.0 +elbow_joint: 1.57 wrist_1_joint: -1.57 -wrist_2_joint: 0.0 +wrist_2_joint: -1.57 wrist_3_joint: 0.0 finger_joint: 20.0 \ No newline at end of file diff --git a/src/ur_robotiq_moveit_config/config/ur_servo.yaml b/src/ur_robotiq_moveit_config/config/ur_servo.yaml index 2e829ad..8e64d56 100644 --- a/src/ur_robotiq_moveit_config/config/ur_servo.yaml +++ b/src/ur_robotiq_moveit_config/config/ur_servo.yaml @@ -7,8 +7,8 @@ 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.05 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands. - rotational: 0.05 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands. + 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. # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic. joint: 0.01 # This is a fudge factor to account for any latency in the system, e.g. network latency or poor low-level @@ -17,7 +17,7 @@ scale: system_latency_compensation: 0.04 ## Properties of outgoing commands -publish_period: 0.004 # 1/Nominal publish rate [seconds] +publish_period: 0.01 # 1/Nominal publish rate [seconds] low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) # What type of topic does your robot driver expect? @@ -63,14 +63,14 @@ command_out_topic: /forward_position_controller/commands # Publish outgoing comm ## Collision checking for the entire robot body check_collisions: false # Check collisions? -# collision_check_rate: 40.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. +collision_check_rate: 40.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. # Two collision check algorithms are available: # "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually. # "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits -# collision_check_type: threshold_distance +collision_check_type: threshold_distance # Parameters for "threshold_distance"-type collision checking -# self_collision_proximity_threshold: 0.005 # Start decelerating when a self-collision is this far [m] -# scene_collision_proximity_threshold: 0.01 # Start decelerating when a scene collision is this far [m] +self_collision_proximity_threshold: 0.005 # Start decelerating when a self-collision is this far [m] +scene_collision_proximity_threshold: 0.01 # Start decelerating when a scene collision is this far [m] # Parameters for "stop_distance"-type collision checking -# collision_distance_safety_factor: 1.0 # Must be >= 1. A large safety factor is recommended to account for latency -# min_allowable_collision_distance: 0.005 # Stop if a collision is closer than this [m] +collision_distance_safety_factor: 1.0 # Must be >= 1. A large safety factor is recommended to account for latency +min_allowable_collision_distance: 0.005 # Stop if a collision is closer than this [m] diff --git a/src/ur_robotiq_moveit_config/srdf/ur_manipulator_macro.srdf.xacro b/src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro similarity index 89% rename from src/ur_robotiq_moveit_config/srdf/ur_manipulator_macro.srdf.xacro rename to src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro index 92985df..d0dc7a7 100644 --- a/src/ur_robotiq_moveit_config/srdf/ur_manipulator_macro.srdf.xacro +++ b/src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro @@ -5,11 +5,11 @@ --> - - - - - + + + + + @@ -17,17 +17,21 @@ + - - + + + + + @@ -58,17 +62,19 @@ - + - + - - - - + + - + + + + + diff --git a/src/ur_robotiq_servo/config/ps5-params.yaml b/src/ur_robotiq_servo/config/ps5-params.yaml new file mode 100644 index 0000000..46f47e1 --- /dev/null +++ b/src/ur_robotiq_servo/config/ps5-params.yaml @@ -0,0 +1,3 @@ +ps5_servo_node: + ros__parameters: + linear_speed_multiplier: 0.2 \ No newline at end of file diff --git a/src/ur_robotiq_servo/launch/ps5_servo.launch.py b/src/ur_robotiq_servo/launch/ps5_servo.launch.py index 4ece31f..5dfc743 100644 --- a/src/ur_robotiq_servo/launch/ps5_servo.launch.py +++ b/src/ur_robotiq_servo/launch/ps5_servo.launch.py @@ -8,24 +8,22 @@ def generate_launch_description(): config_directory = os.path.join( ament_index_python.packages.get_package_share_directory('ur_robotiq_servo'), 'config') - params = os.path.join(config_directory, 'joy-params.yaml') + joy_params = os.path.join(config_directory, 'joy-params.yaml') + ps5_params = os.path.join(config_directory, 'ps5-params.yaml') return LaunchDescription([ Node( package='joy', executable='joy_node', name='joy_node', output='screen', - parameters=[{ - params, - }], + parameters=[joy_params], ), Node( package='ur_robotiq_servo', executable='ps5_servo', name='ps5_servo_node', output='screen', - parameters=[{ - }], + parameters=[ps5_params], arguments=[], ), ]) \ 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 4c45d7c..fa259c6 100644 --- a/src/ur_robotiq_servo/ur_robotiq_servo/ps5_control.py +++ b/src/ur_robotiq_servo/ur_robotiq_servo/ps5_control.py @@ -13,16 +13,27 @@ class PS5ControllerNode(Node): 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 + # Parameters + self.linear_speed_multiplier = self.declare_parameter('linear_speed_multiplier', 1.0) + self.linear_speed_multiplier = self.get_parameter('linear_speed_multiplier').get_parameter_value().double_value + self.get_logger().info(f"Linear speed multiplier: {self.linear_speed_multiplier}") + + self.use_fake_hardware = self.declare_parameter('use_fake_hardware', False) + 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}") + # Subscriber and Publisher self.joy_sub = self.create_subscription( Joy, '/joy', self.joy_callback, 10) + self.twist_pub = self.create_publisher( TwistStamped, '/servo_node/delta_twist_cmds', 10) + self.joint_pub = self.create_publisher( JointJog, '/servo_node/delta_joint_cmds', @@ -66,20 +77,25 @@ class PS5ControllerNode(Node): twist_msg = TwistStamped() twist_msg.header.frame_id = 'tool0' twist_msg.header.stamp = self.get_clock().now().to_msg() - twist_msg.twist.linear.x = msg.axes[0] - twist_msg.twist.linear.y = msg.axes[1] - twist_msg.twist.linear.z = left_trigger - right_trigger + twist_msg.twist.linear.x = msg.axes[0] * self.linear_speed_multiplier + 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 self.twist_pub.publish(twist_msg) elif self.mode == 'joint': joint_msg = JointJog() joint_msg.header.frame_id = 'tool0' joint_msg.header.stamp = self.get_clock().now().to_msg() - joint_msg.joint_names = ["shoulder_pan_joint", - "shoulder_lift_joint", - "elbow_joint", - "wrist_1_joint", - "wrist_2_joint", - "wrist_3_joint"] + joint_msg.joint_names = [ + "shoulder_pan_joint", + "shoulder_lift_joint", + "elbow_joint", + "wrist_1_joint", + "wrist_2_joint", + "wrist_3_joint" + ] joint_msg.velocities = [msg.axes[0], msg.axes[1], msg.axes[2], @@ -88,6 +104,7 @@ class PS5ControllerNode(Node): (msg.buttons[13] - msg.buttons[14]) * 1.0] self.joint_pub.publish(joint_msg) + def main(args=None): rclpy.init(args=args) node = PS5ControllerNode() @@ -95,5 +112,6 @@ def main(args=None): node.destroy_node() rclpy.shutdown() + if __name__ == '__main__': main() \ No newline at end of file