From 61c25741bb8565b075f7de77eb327dc8cf952cbd Mon Sep 17 00:00:00 2001 From: Fiona Gillespie Date: Fri, 18 Oct 2024 13:48:29 -0400 Subject: [PATCH] [N/A] Expose height in Spot_Wrapper velocity command (#147) ## Change Overview This change enables dynamic height changes for teleop during the Drive-A-Spot demo. It adds an optional body_height parameter for the velocity_cmd function in Spot Wrapper. ## Testing Done This change has been tested on Spot. ## Instructions for Reviewers - Certify that this change is following [our dependency rules][1]. - Certify that the author has filled in the testing section with appropriate testing: * [Spot testing][2] for PRs touching Spot functionality, including the list of functions tested. * [Open AI testing][3] for PRs touching code that calls Open AI functions, including a list of tests done. For example, did they test dry, canned, AND online? * [ROS 2 testing][4] for PRs that use ROS 2 and that their ROS 2 tests use the `domain_coordinator` to prevent port conflicts. - For changes to docker, take a look at [docker testing][5] [1]: https://github.com/bdaiinstitute/bdai#dependency-rules [2]: https://github.com/bdaiinstitute/bdai/blob/main/hil_testing.md [3]: https://github.com/bdaiinstitute/bdai/blob/main/open_ai_testing.md [4]: https://github.com/bdaiinstitute/bdai/blob/main/ros_testing.md [5]: https://www.notion.so/theaiinstitute/Docker-Testing-9e6e03d4f0614103902f017a42ec0fb3 --- spot_wrapper/wrapper.py | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index eafb72c..3443c42 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -1299,7 +1299,7 @@ def get_mobility_params(self) -> spot_command_pb2.MobilityParams: return self._mobility_params def velocity_cmd( - self, v_x: float, v_y: float, v_rot: float, cmd_duration: float = 0.125 + self, v_x: float, v_y: float, v_rot: float, cmd_duration: float = 0.125, body_height: float = 0.0 ) -> typing.Tuple[bool, str]: """ @@ -1311,11 +1311,22 @@ def velocity_cmd( v_rot: Angular velocity around the Z axis in radians cmd_duration: (optional) Time-to-live for the command in seconds. Default is 125ms (assuming 10Hz command rate). + body_height: Offset of the body relative to nominal stand height, in metres Returns: Tuple of bool success and a string message """ end_time = time.time() + cmd_duration + if body_height: + current_mobility_params = self.get_mobility_params() + height_adjusted_params = RobotCommandBuilder.mobility_params( + body_height=body_height, + locomotion_hint=current_mobility_params.locomotion_hint, + stair_hint=current_mobility_params.stair_hint, + external_force_params=current_mobility_params.external_force_params, + stairs_mode=current_mobility_params.stairs_mode, + ) + self.set_mobility_params(height_adjusted_params) response = self._robot_command( RobotCommandBuilder.synchro_velocity_command(v_x=v_x, v_y=v_y, v_rot=v_rot, params=self._mobility_params), end_time_secs=end_time,