Add function to move gripper to a specific position (AndrejOrsula#70)

pymoveit2/gripper_command.py

@@ -107,6 +107,7 @@ def __init__(
         self.__close_gripper_command_goal = self.__init_gripper_command_goal(
             position=closed_gripper_joint_positions, max_effort=max_effort
         )
+        self.__max_effort = max_effort
 
         # Initialise internals for determining whether the gripper is open or closed
         self.__joint_state_mutex = threading.Lock()
@@ -180,6 +181,21 @@ def close(self, skip_if_noop: bool = False):
 
         self.__send_goal_async_gripper_command(self.__close_gripper_command_goal)
 
+    def move_to_position(self, position: float):
+        """
+        Move the gripper to a specific position.
+        - `position` - Desired position of the gripper.
+        """
+
+        if self.__ignore_new_calls_while_executing and self.__is_executing:
+            return
+        self.__is_motion_requested = True
+
+        gripper_cmd_goal = GripperCommandAction.Goal()
+        gripper_cmd_goal.command.position = position
+        gripper_cmd_goal.command.max_effort = self.__max_effort
+        self.__send_goal_async_gripper_command(gripper_cmd_goal)
+
     def reset_open(self, **kwargs):
         """
         Reset into open configuration by sending a dummy joint trajectory.

pymoveit2/gripper_interface.py

@@ -138,6 +138,23 @@ def close(self, skip_if_noop: bool = False):
 
         self._interface.close(self=self, skip_if_noop=skip_if_noop)
 
+    def move_to_position(self, position: float):
+        """
+        Move the gripper to a specific position.
+        - `position` - Desired position of the gripper.
+        """
+
+        if self._interface is None:
+            self.__determine_interface()
+        if self._interface is None:
+            self._node.get_logger().error(
+                "Unable to move the gripper to a position because the appropriate "
+                "interface cannot be determined."
+            )
+            return
+
+        self._interface.move_to_position(self=self, position=position)
+
     def reset_open(self, sync: bool = True):
         """
         Reset into open configuration by sending a dummy joint trajectory.

pymoveit2/moveit2_gripper.py

@@ -169,6 +169,15 @@ def close(self, skip_if_noop: bool = False):
                 joint_positions=self.__closed_gripper_joint_positions
             )
 
+    def move_to_position(self, position: float):
+        """
+        Move the gripper to a specific position.
+        - `position` - Desired position of the gripper
+        """
+
+        joint_positions = [position for _ in self.joint_names]
+        self.move_to_configuration(joint_positions=joint_positions)
+
     def reset_open(self, sync: bool = True):
         """
         Reset into open configuration by sending a dummy joint trajectory.