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.