From afd30fa01724ba890599ae6143586d6694ff2319 Mon Sep 17 00:00:00 2001
From: Yifei Cheng <ycheng517@gmail.com>
Date: Mon, 3 Jun 2024 04:03:05 -0400
Subject: [PATCH] Add function to move gripper to a specific position (#70)

---
 pymoveit2/gripper_command.py   | 16 ++++++++++++++++
 pymoveit2/gripper_interface.py | 17 +++++++++++++++++
 pymoveit2/moveit2_gripper.py   |  9 +++++++++
 3 files changed, 42 insertions(+)

diff --git a/pymoveit2/gripper_command.py b/pymoveit2/gripper_command.py
index 0ee92ab..52a968e 100644
--- a/pymoveit2/gripper_command.py
+++ b/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.
diff --git a/pymoveit2/gripper_interface.py b/pymoveit2/gripper_interface.py
index 0503805..2fa4380 100644
--- a/pymoveit2/gripper_interface.py
+++ b/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.
diff --git a/pymoveit2/moveit2_gripper.py b/pymoveit2/moveit2_gripper.py
index 35fcdc1..74d1529 100644
--- a/pymoveit2/moveit2_gripper.py
+++ b/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.