Merge pull request #5 from alliander-opensource/real_robot_movement_t…

…weaks

gripper and movement speed improvements

rcdt_franka/config/servo_params.yaml

@@ -11,8 +11,8 @@ use_gazebo: true # Whether the robot is started in a Gazebo simulation environme
 command_in_type: "unitless" # "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.4  # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands.
-  rotational:  0.8 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands.
+  linear:  0.3  # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands.
+  rotational:  0.6 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands.
   # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic.
   joint: 0.5
 

rcdt_franka/nodes/joy_to_gripper_node.py

@@ -18,10 +18,14 @@ def __init__(self):
         super().__init__("fr3_gripper_client")
         self.move_client = ActionClient(self, Move, "/fr3_gripper/move")
         self.grasp_client = ActionClient(self, Grasp, "/fr3_gripper/grasp")
+        self.is_gripped = False
 
     def open_gripper(self) -> None:
+        if not self.is_gripped:
+            return
         goal = Move.Goal()
         goal.width = 0.08
+        goal.speed = 0.03
 
         if not self.move_client.wait_for_server(timeout_sec=2):
             self.get_logger().warn("Gripper move client not available.")
@@ -30,12 +34,16 @@ def open_gripper(self) -> None:
         result: Move.Impl.GetResultService.Response = self.move_client.send_goal(goal)
         if not result.result.success:
             self.get_logger().warn("Opening gripper did not succeed.")
+        self.is_gripped = False
 
     def close_gripper(self) -> None:
+        if self.is_gripped:
+            return
         goal = Grasp.Goal()
         goal.width = 0.0
         goal.epsilon.inner = goal.epsilon.outer = 0.08
         goal.force = 100.0
+        goal.speed = 0.03
 
         if not self.grasp_client.wait_for_server(timeout_sec=2):
             self.get_logger().warn("Gripper grasp client not available.")
@@ -44,6 +52,7 @@ def close_gripper(self) -> None:
         result: Grasp.Impl.GetResultService.Response = self.grasp_client.send_goal(goal)
         if not result.result.success:
             self.get_logger().warn("Closing gripper did not succeed.")
+        self.is_gripped = True
 
 
 class JoyToGripper(Node):