From dc6fd6e45bb5ef991c647cf22bccd1a65e9bbddd Mon Sep 17 00:00:00 2001 From: Laura Lee Date: Wed, 16 Oct 2024 19:55:57 +0000 Subject: [PATCH] Add non blocking for hand_pose --- spot_wrapper/spot_arm.py | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index 6e78a37..e4acdb8 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -447,9 +447,10 @@ def hand_pose( qy: float, qz: float, qw: float, - duration: float = 0.125, + duration: float = 0.0, ref_frame: str = "body", ensure_power_on_and_stand: bool = True, + blocking: bool = True, ) -> typing.Tuple[bool, str]: """ Set the pose of the hand @@ -483,11 +484,12 @@ def hand_pose( # Rotation as a quaternion. rotation = geometry_pb2.Quaternion(w=qw, x=qx, y=qy, z=qz) - traj_duration = seconds_to_duration(duration) - # Build the SE(3) pose of the desired hand position in the moving body frame. hand_pose = geometry_pb2.SE3Pose(position=position, rotation=rotation) - hand_pose_traj_point = trajectory_pb2.SE3TrajectoryPoint(pose=hand_pose, time_since_reference=traj_duration) + hand_pose_traj_point = trajectory_pb2.SE3TrajectoryPoint(pose=hand_pose) + if duration != 0.0: + traj_duration = seconds_to_duration(duration) + hand_pose_traj_point = trajectory_pb2.SE3TrajectoryPoint(pose=hand_pose, time_since_reference=traj_duration) hand_trajectory = trajectory_pb2.SE3Trajectory(points=[hand_pose_traj_point]) arm_cartesian_command = arm_command_pb2.ArmCartesianCommand.Request( @@ -504,7 +506,8 @@ def hand_pose( # Send the request cmd_id = self._robot_command_client.robot_command(robot_command) - self.wait_for_arm_command_to_complete(cmd_id) + if blocking: + self.wait_for_arm_command_to_complete(cmd_id) except Exception as e: return (