Real robot works with servo

gripper needs different update loop and sometimes has too much current and then the whole robot goes into error recovery mode

README.md

@@ -88,7 +88,7 @@ After launching you need to change the controller to enable the servo mode.\
 Terminal 4:
 ```bash
 ros2 control switch_controllers --deactivate joint_trajectory_controller
-ros2 control switch_controllers --deactivate robotiq_gripper_joint_trajectory_controller
+ros2 control switch_controllers --deactivate robotiq_gripper_controller
 ros2 control switch_controllers --activate forward_position_controller
 ros2 control switch_controllers --activate forward_gripper_position_controller
 ```

src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro

@@ -436,8 +436,8 @@
             <axis xyz="0 0 1" />
         </joint>
 
-        <!-- Pseudo Link and Joint for gripper gap -->
+        <!-- Dummy link to facilitate the pseudo joint -->
-        <link name="${prefix}dummy_link">
+        <link name="dummy_link">
             <visual>
                 <geometry>
                     <box size="0.001 0.001 0.001"/>  <!-- Very small, effectively invisible -->
@@ -446,25 +446,23 @@
                     <color rgba="0.0 0.0 0.0 0.0"/>  <!-- Transparent color -->
                 </material>
             </visual>
-            <collision>
-              <geometry>
-                <sphere radius="0.001"/>  <!-- Negligible size -->
-              </geometry>
-            </collision>
         </link>
 
-        <joint name="${prefix}base_to_dummy" type="fixed">
+        <!-- Fixed joint to attach the dummy link to part of the gripper -->
-            <parent link="${prefix}robotiq_140_base_link"/>
+        <joint name="dummy_to_pad" type="fixed">
-            <child link="${prefix}dummy_link"/>
+            <parent link="dummy_link"/>
-            <origin xyz="0 0 0" rpy="0 0 0"/>  <!-- Adjust origin as needed -->
+            <child link="robotiq_140_left_inner_finger_pad"/>
+            <origin xyz="0 0 0" rpy="0 0 0"/>  <!-- Position it appropriately -->
         </joint>
 
-        <joint name="${prefix}gripper_gap" type="prismatic">
+        <!-- Pseudo prismatic joint for controlling the gripper gap using the dummy link -->
-            <parent link="${prefix}dummy_link"/>
+        <joint name="gripper_gap" type="prismatic">
-            <child link="${prefix}robotiq_140_right_inner_finger_pad"/>
+            <parent link="robotiq_140_base_link"/>  <!-- Adjust the parent link as necessary -->
-            <origin xyz="0 0 0" rpy="0 0 0"/>  <!-- Set origin appropriately -->
+            <child link="dummy_link"/>
-            <axis xyz="0 1 0"/>  <!-- Axis of movement; adjust according to actual alignment -->
+            <origin xyz="0 0 0" rpy="0 0 0"/>  <!-- Properly position this joint -->
+            <axis xyz="0 1 1"/>  <!-- Ensure the axis aligns with the intended motion -->
             <limit lower="0.0" upper="0.14" effort="235" velocity="0.15"/>
         </joint>
+
     </xacro:macro>
 </robot>

src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85_macro.urdf.xacro

@@ -304,32 +304,5 @@
             <origin xyz="-0.00563134 0.0 0.04718515" rpy="0 0 0" />
             <mimic joint="${prefix}finger_joint" />
         </joint>
-
-        <!-- Pseudo Link and Joint for gripper gap -->
-        <link name="${prefix}dummy_link">
-            <visual>
-                <geometry>
-                    <box size="0.001 0.001 0.001"/>  <!-- Very small, effectively invisible -->
-                </geometry>
-                <material name="transparent">
-                    <color rgba="0.0 0.0 0.0 0.0"/>  <!-- Transparent color -->
-                </material>
-            </visual>
-        </link>
-
-        <joint name="base_to_dummy" type="fixed">
-            <parent link="${prefix}robotiq_140_base_link"/>
-            <child link="dummy_link"/>
-            <origin xyz="0 0 0" rpy="0 0 0"/>  <!-- Adjust origin as needed -->
-        </joint>
-
-        <joint name="gripper_gap" type="prismatic">
-            <parent link="${prefix}dummy_link"/>
-            <child link="${prefix}robotiq_85_right_finger_tip_link"/>
-            <origin xyz="0 0 0" rpy="0 0 0"/>  <!-- Set origin appropriately -->
-            <axis xyz="0 1 0"/>  <!-- Axis of movement; adjust according to actual alignment -->
-            <limit lower="0.0" upper="0.085" effort="235" velocity="0.15"/>
-        </joint>
-
     </xacro:macro>
 </robot>

src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp

@@ -510,7 +510,7 @@ double RobotiqGripperHardwareInterface::convertRawToAngle(int raw_position) {
     if (std::isnan(max_angle_) || 0.0 >= max_angle_) {
         throw std::runtime_error("Invalid gripper angle limits: max_angle is not configured correctly.");
     }
-    double scaled_angle = (static_cast<double>(raw_position) / 255.0) * max_angle_;
+    double scaled_angle = ((230 - static_cast<double>(raw_position)) / 227.0) * max_angle_;
     return std::max(0.0, std::min(scaled_angle, max_angle_));
 }
 

src/ur_robotiq_description/launch/ur_robotiq_control.launch.py

@@ -217,13 +217,6 @@ def launch_setup(context, *args, **kwargs):
         package="controller_manager",
         executable="spawner",
         arguments=["robotiq_gripper_controller", "-c", "/controller_manager"],
-        condition=UnlessCondition(use_fake_hardware),
-    )
-    robotiq_gripper_joint_trajectory_controller_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["robotiq_gripper_joint_trajectory_controller", "-c", "/controller_manager"],
-        condition=IfCondition(use_fake_hardware),
     )
 
     robotiq_activation_controller_spawner = Node(
@@ -347,7 +340,6 @@ def launch_setup(context, *args, **kwargs):
                          initial_joint_controller_spawner_stopped,
                          initial_joint_controller_spawner_started,
                          robotiq_gripper_controller_spawner,
-                         robotiq_gripper_joint_trajectory_controller_spawner,
                          robotiq_activation_controller_spawner,
                      ] + controller_spawners
 

src/ur_robotiq_moveit_config/config/moveit_controllers.yaml

@@ -31,8 +31,8 @@ joint_trajectory_controller:
 
 
 robotiq_gripper_controller:
-  action_ns: robotiq_2f_controllers
+  action_ns: gripper_cmd
-  type: RobotiqGripperCommandController
+  type: GripperCommand
   default: true
-  joint: gripper_gap
+  joints:
-  max_gap: 0.14
+    - gripper_gap

src/ur_robotiq_moveit_config/config/ur_servo.yaml

@@ -7,10 +7,10 @@ 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.00001  # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands.
+  linear:  0.4  # 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.
+  rotational:  0.8 # 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
+  joint: 0.5
 # This is a fudge factor to account for any latency in the system, e.g. network latency or poor low-level
 # controller performance. It essentially increases the timestep when calculating the target pose, to move the target
 # pose farther away. [seconds]
@@ -38,6 +38,7 @@ low_pass_filter_coeff: 2.0  # Larger --> trust the filtered data more, trust the
 ## MoveIt properties
 move_group_name:  ur_manipulator  # Often 'manipulator' or 'arm'
 planning_frame: world  # The MoveIt planning frame. Often 'base_link' or 'world'
+is_primary_planning_scene_monitor: false
 
 ## Other frames
 ee_frame_name: tool0  # The name of the end effector link, used to return the EE pose

src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro

@@ -27,14 +27,15 @@
         <joint name="${prefix}right_outer_finger_joint"/>
         <joint name="${prefix}right_inner_finger_joint"/>
         <joint name="${prefix}right_inner_finger_pad_joint"/>
-        <joint name="${prefix}base_to_dummy"/>
     </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="open" group="${prefix}gripper">
         <joint name="${prefix}gripper_gap" value="0.14"/>
+        <joint name="${prefix}finger_joint" value="0.0"/>
     </group_state>
     <group_state name="closed" group="${prefix}gripper">
         <joint name="${prefix}gripper_gap" value="0.0"/>
+        <joint name="${prefix}finger_joint" value="0.695"/>
     </group_state>
     <group_state name="${prefix}home" group="${prefix}${name}_manipulator">
       <joint name="${prefix}elbow_joint" value="0" />

src/ur_robotiq_servo/config/joy-params.yaml

@@ -1,7 +1,7 @@
 joy_node:
     ros__parameters:
         device_id: 0
-        device_name: "PS5 Controller"
+        device_name: "Wireless Controller"
         deadzone: 0.5
         autorepeat_rate: 20.0
         sticky_buttons: false

src/ur_robotiq_servo/config/ps5-params.yaml

@@ -1,6 +1,6 @@
 ps5_servo_node:
     ros__parameters:
-      linear_speed_multiplier: 0.2
+      linear_speed_multiplier: 0.5
-      gripper_position_multiplier: 0.02
+      gripper_position_multiplier: 0.004
       gripper_lower_limit: 0.0
       gripper_upper_limit: 0.14

src/ur_robotiq_servo/ur_robotiq_servo/ps5_control.py

@@ -97,13 +97,13 @@ class PS5ControllerNode(Node):
     def joy_callback(self, msg):
         # Check for button press to toggle mode
         # Assuming button 2 (e.g., Triangle on PS5) for toggling mode
-        current_button_state = msg.buttons[16]
+        current_button_state = msg.buttons[8]
         if current_button_state == 1 and self.last_button_state == 0:
             self.mode = 'joint' if self.mode == 'twist' else 'twist'
             self.get_logger().info(f'Mode switched to: {self.mode}')
         self.last_button_state = current_button_state
 
-        left_trigger = (msg.axes[4] - 1) / - 2.0
+        left_trigger = (msg.axes[2] - 1) / - 2.0
         right_trigger = (msg.axes[5] - 1) / - 2.0
 
         # Process control input based on current mode
@@ -115,8 +115,8 @@ class PS5ControllerNode(Node):
             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.y = msg.axes[4]
-            twist_msg.twist.angular.z = (msg.buttons[9] - msg.buttons[10]) * 1.0
+            twist_msg.twist.angular.z = (msg.buttons[4] - msg.buttons[5]) * 1.0
             self.twist_pub.publish(twist_msg)
         elif self.mode == 'joint':
             joint_msg = JointJog()
@@ -132,10 +132,10 @@ class PS5ControllerNode(Node):
             ]
             joint_msg.velocities = [msg.axes[0],
                                     msg.axes[1],
-                                    msg.axes[2],
                                     msg.axes[3],
-                                    (msg.buttons[11] - msg.buttons[12]) * 1.0,
+                                    msg.axes[4],
-                                    (msg.buttons[13] - msg.buttons[14]) * 1.0]
+                                    msg.axes[6],
+                                    msg.axes[7]]
             self.joint_pub.publish(joint_msg)
 
         # Gripper controller