servo working without collision checking for twist and joints

gripper not included in servo

README.md

@@ -25,6 +25,8 @@ To download this repository and all of its included submodules, follow these ste
    ```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 <path/to/UR-Robotiq> 
      ```
 
    * 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
 ```

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

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

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

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

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.
+  linear:  0.00001  # 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.
+  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]
+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: 1.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_moveit_config/srdf/ur_robotiq_macro.srdf.xacro

@@ -5,11 +5,11 @@
 -->
 <robot xmlns:xacro="http://wiki.ros.org/xacro" name="$(arg name)">
   <xacro:macro name="ur_robotiq_srdf" params="name prefix">
-    <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
+    <!--GROUPS Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
-    <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
+    <!--LINKS When a link is specified, the parent joint of that link (if it exists) is automatically included-->
-    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
+    <!--JOINTS When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
-    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
+    <!--CHAINS When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
-    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
+    <!--SUBGROUPS Groups can also be formed by referencing to already defined group names-->
     <group name="${prefix}${name}_manipulator">
         <chain base_link="${prefix}base_link" tip_link="${prefix}tool0"/>
     </group>
@@ -17,17 +17,21 @@
         <joint name="${prefix}robotiq_base_joint"/>
         <joint name="${prefix}robotiq_140_base_joint"/>
         <joint name="${prefix}finger_joint"/>
+        <joint name="${prefix}left_inner_knuckle_joint"/>
         <joint name="${prefix}left_outer_finger_joint"/>
         <joint name="${prefix}left_inner_finger_joint"/>
         <joint name="${prefix}left_inner_finger_pad_joint"/>
-        <joint name="${prefix}left_inner_knuckle_joint"/>
         <joint name="${prefix}right_inner_knuckle_joint"/>
         <joint name="${prefix}right_outer_knuckle_joint"/>
         <joint name="${prefix}right_outer_finger_joint"/>
         <joint name="${prefix}right_inner_finger_joint"/>
         <joint name="${prefix}right_inner_finger_pad_joint"/>
     </group>
-    <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
+    <group name="${prefix}${name}_manipulator_combined">
+        <group name="${prefix}${name}_manipulator"/>
+        <group name="${prefix}gripper"/>
+    </group>
+    <!--GROUP STATES Purpose Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
     <group_state name="${prefix}close" group="${prefix}gripper">
         <joint name="${prefix}finger_joint" value="0.7"/>
     </group_state>
@@ -58,17 +62,19 @@
       <joint name="${prefix}wrist_2_joint" value="-1.6" />
       <joint name="${prefix}wrist_3_joint" value="-0.11" />
     </group_state>
-    <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
+    <!--END EFFECTOR Purpose Represent information about an end effector.-->
     <end_effector name="${prefix}gripper" parent_link="${prefix}tool0" group="${prefix}gripper" parent_group="${prefix}${name}_manipulator"/>
-    <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
+    <!--VIRTUAL JOINT Purpose this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
     <virtual_joint name="world_joint" type="fixed" parent_frame="world" child_link="${prefix}base_link"/>
-    <!--PASSIVE JOINT: Purpose: this element is used to mark joints that are not actuated-->
+    <!--PASSIVE JOINT Purpose this element is used to mark joints that are not actuated-->
-    <passive_joint name="${prefix}right_inner_finger_joint"/>
-    <passive_joint name="${prefix}right_outer_knuckle_joint"/>
-    <passive_joint name="${prefix}right_inner_knuckle_joint"/>
     <passive_joint name="${prefix}left_inner_knuckle_joint"/>
+    <passive_joint name="${prefix}left_outer_finger_joint"/>
     <passive_joint name="${prefix}left_inner_finger_joint"/>
-    <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
+    <passive_joint name="${prefix}right_inner_knuckle_joint"/>
+    <passive_joint name="${prefix}right_outer_knuckle_joint"/>
+    <passive_joint name="${prefix}right_outer_finger_joint"/>
+    <passive_joint name="${prefix}right_inner_finger_joint"/>
+    <!--DISABLE COLLISIONS By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
     <disable_collisions link1="${prefix}base_link_inertia" link2="${prefix}shoulder_link" reason="Adjacent"/>
     <disable_collisions link1="${prefix}base_link_inertia" link2="${prefix}upper_arm_link" reason="Never"/>
     <disable_collisions link1="${prefix}base_link_inertia" link2="${prefix}wrist_1_link" reason="Never"/>

src/ur_robotiq_servo/config/ps5-params.yaml

@@ -0,0 +1,3 @@
+ps5_servo_node:
+    ros__parameters:
+      linear_speed_multiplier: 0.2

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=[{
+            parameters=[joy_params],
-                params,
-            }],
         ),
         Node(
             package='ur_robotiq_servo',
             executable='ps5_servo',
             name='ps5_servo_node',
             output='screen',
-            parameters=[{
+            parameters=[ps5_params],
-            }],
             arguments=[],
         ),
     ])

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.x = msg.axes[0] * self.linear_speed_multiplier
-            twist_msg.twist.linear.y = msg.axes[1]
+            twist_msg.twist.linear.y = -msg.axes[1] * self.linear_speed_multiplier
-            twist_msg.twist.linear.z = left_trigger - right_trigger
+            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",
+            joint_msg.joint_names = [
+                "shoulder_pan_joint",
                 "shoulder_lift_joint",
                 "elbow_joint",
                 "wrist_1_joint",
                 "wrist_2_joint",
-                                     "wrist_3_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()