From 27e12a3dc3468bc51103ffa6091a40f467b69df0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marvin=20Gro=C3=9Fe=20Besselmann?= Date: Fri, 12 Apr 2024 06:31:26 +0000 Subject: [PATCH 1/3] Added possibility to directly control the arm using an ArmVelocityCommand --- spot_wrapper/spot_arm.py | 44 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 44 insertions(+) diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index b0e0fd85..bdd60217 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -500,6 +500,50 @@ def hand_pose(self, data) -> typing.Tuple[bool, str]: return True, "Moved arm successfully" + def handle_arm_velocity( + self, arm_velocity_command: arm_command_pb2.ArmVelocityCommand.Request, cmd_duration: float = 0.15 + ) -> typing.Tuple[bool, str]: + """ + Set the velocity of the arm TCP + + Args: + arm_velocity_command: Protobuf message to set the arm velocity + cmd_duration: (optional) Time-to-live for the command in seconds. + + Returns: + Boolean success, string message + """ + + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + end_time = self._robot.time_sync.robot_timestamp_from_local_secs(time.time() + cmd_duration) + + arm_velocity_command2 = arm_command_pb2.ArmVelocityCommand.Request( + cylindrical_velocity=arm_velocity_command.cylindrical_velocity, + angular_velocity_of_hand_rt_odom_in_hand=arm_velocity_command.angular_velocity_of_hand_rt_odom_in_hand, + cartesian_velocity=arm_velocity_command.cartesian_velocity, + end_time=end_time, + ) + + robot_command = robot_command_pb2.RobotCommand() + robot_command.synchronized_command.arm_command.arm_velocity_command.CopyFrom(arm_velocity_command2) + + self._robot_command_client.robot_command( + command=robot_command, end_time_secs=time.time() + cmd_duration + ) + + except Exception as e: + print(e) + return ( + False, + f"An error occured while trying to move arm\n Exception: {e}", + ) + return True, "Moved arm successfully" + @staticmethod def block_until_gripper_command_completes( robot_command_client: RobotCommandClient, From 641e54c13130b38b50b196652626b98efbb18271 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marvin=20Gro=C3=9Fe=20Besselmann?= <35597356+MGBla@users.noreply.github.com> Date: Wed, 5 Jun 2024 14:42:32 +0200 Subject: [PATCH 2/3] Removed print Co-authored-by: Katie Hughes <157421702+khughes-bdai@users.noreply.github.com> --- spot_wrapper/spot_arm.py | 1 - 1 file changed, 1 deletion(-) diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index bdd60217..f70805f3 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -537,7 +537,6 @@ def handle_arm_velocity( ) except Exception as e: - print(e) return ( False, f"An error occured while trying to move arm\n Exception: {e}", From 0316a5fc9ff7e4ef10d948b2dce6858b964191d5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marvin=20Gro=C3=9Fe=20Besselmann?= <35597356+MGBla@users.noreply.github.com> Date: Wed, 5 Jun 2024 14:43:03 +0200 Subject: [PATCH 3/3] Added missing maximum acceleration Co-authored-by: Katie Hughes <157421702+khughes-bdai@users.noreply.github.com> --- spot_wrapper/spot_arm.py | 1 + 1 file changed, 1 insertion(+) diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index f70805f3..2b00b22e 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -526,6 +526,7 @@ def handle_arm_velocity( cylindrical_velocity=arm_velocity_command.cylindrical_velocity, angular_velocity_of_hand_rt_odom_in_hand=arm_velocity_command.angular_velocity_of_hand_rt_odom_in_hand, cartesian_velocity=arm_velocity_command.cartesian_velocity, + maximum_acceleration = arm_velocity_command.maximum_acceleration, end_time=end_time, )