From 14126b5b7d2df91921d9d04ad9d589cc841912f2 Mon Sep 17 00:00:00 2001 From: Niko Date: Thu, 4 Apr 2024 09:46:04 +0200 Subject: [PATCH] servo not working with collsion checking --- README.md | 16 +++++++++-- .../config/ur_servo.yaml | 28 +++++++++---------- .../ur_robotiq_servo/ps5_control.py | 6 ++-- 3 files changed, 31 insertions(+), 19 deletions(-) diff --git a/README.md b/README.md index 61d3799..76a40f8 100644 --- a/README.md +++ b/README.md @@ -62,11 +62,23 @@ ros2 launch ur_robotiq_moveit_config ur_robotiq_moveit.launch.py use_fake_hardwa ``` ### use_fake_hardware:=true -Terminal 1.: +Terminal 1: ```bash ros2 launch ur_robotiq_description ur_robotiq_control.launch.py robot_ip:=aaa.bbb.ccc.ddd use_fake_hardware:=true launch_rviz:=false initial_joint_controller:=joint_trajectory_controller ``` -Terminal 2.: +Terminal 2: ```bash ros2 launch ur_robotiq_moveit_config ur_robotiq_moveit.launch.py use_fake_hardware:=true ``` + +### servoing +Terminal 3: +```bash +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 +``` \ 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 7327069..2e829ad 100644 --- a/src/ur_robotiq_moveit_config/config/ur_servo.yaml +++ b/src/ur_robotiq_moveit_config/config/ur_servo.yaml @@ -22,26 +22,26 @@ low_latency_mode: false # Set this to true to publish as soon as an incoming Tw # What type of topic does your robot driver expect? # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory -command_out_type: trajectory_msgs/JointTrajectory +command_out_type: std_msgs/Float64MultiArray # What to publish? Can save some bandwidth as most robots only require positions or velocities publish_joint_positions: true -publish_joint_velocities: true +publish_joint_velocities: false publish_joint_accelerations: false ## Plugins for smoothing outgoing commands smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" ## Incoming Joint State properties -low_pass_filter_coeff: 10.0 # Larger --> trust the filtered data more, trust the measurements less. +low_pass_filter_coeff: 2.0 # Larger --> trust the filtered data more, trust the measurements less. ## MoveIt properties move_group_name: ur_manipulator # Often 'manipulator' or 'arm' -planning_frame: base_link # The MoveIt planning frame. Often 'base_link' or 'world' +planning_frame: world # The MoveIt planning frame. Often 'base_link' or 'world' ## Other frames ee_frame_name: tool0 # The name of the end effector link, used to return the EE pose -robot_link_command_frame: tool0 # commands must be given in the frame of a robot link. Usually either the base or end effector +robot_link_command_frame: base_link # commands must be given in the frame of a robot link. Usually either the base or end effector ## Stopping behaviour incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command @@ -50,8 +50,8 @@ incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a ne num_outgoing_halt_msgs_to_publish: 4 ## Configure handling of singularities and joint limits -lower_singularity_threshold: 50.0 # Start decelerating when the condition number hits this (close to singularity) -hard_stop_singularity_threshold: 100.0 # Stop when the condition number hits this +lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity) +hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this joint_limit_margin: 0.05 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. ## Topic names @@ -62,15 +62,15 @@ status_topic: ~/status # Publish status to this topic command_out_topic: /forward_position_controller/commands # Publish outgoing commands here ## Collision checking for the entire robot body -check_collisions: true # Check collisions? -collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. +check_collisions: false # Check collisions? +# 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: 100.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_servo/ur_robotiq_servo/ps5_control.py b/src/ur_robotiq_servo/ur_robotiq_servo/ps5_control.py index fc8b52f..4c45d7c 100644 --- a/src/ur_robotiq_servo/ur_robotiq_servo/ps5_control.py +++ b/src/ur_robotiq_servo/ur_robotiq_servo/ps5_control.py @@ -18,15 +18,15 @@ class PS5ControllerNode(Node): Joy, '/joy', self.joy_callback, - 1) + 10) self.twist_pub = self.create_publisher( TwistStamped, '/servo_node/delta_twist_cmds', - 1) + 10) self.joint_pub = self.create_publisher( JointJog, '/servo_node/delta_joint_cmds', - 1) + 10) # Services self.servo_client = self.create_client(Trigger, '/servo_node/start_servo')