servo not working with collsion checking

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
+```

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)
+lower_singularity_threshold:  17.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
+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?
+check_collisions: false # Check collisions?
-collision_check_rate: 10.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]
+# 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]
+# 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
+# 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]
+# min_allowable_collision_distance: 0.005 # Stop if a collision is closer than this [m]

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')