From 401a09a2baadbed6d93e3247833a3763ed89b2ef Mon Sep 17 00:00:00 2001
From: Fiona Gillespie <fgillespie@theaiinstitute.com>
Date: Thu, 17 Oct 2024 17:54:20 +0000
Subject: [PATCH] [N/A] Expose height in Spot_Wrapper velocity command

---
 spot_wrapper/wrapper.py | 25 +++++++++++++++++++------
 1 file changed, 19 insertions(+), 6 deletions(-)

diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py
index eafb72c..b2d1187 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,16 +1311,29 @@ 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
-        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,
-            timesync_endpoint=self._robot.time_sync.endpoint,
-        )
+
+        if body_height:
+            # If there is an offset body_height, use it to pose the robot
+            response = self._robot_command(
+                RobotCommandBuilder.synchro_velocity_command(v_x=v_x, v_y=v_y, v_rot=v_rot, body_height=body_height),
+                end_time_secs=end_time,
+                timesync_endpoint=self._robot.time_sync.endpoint,
+            )
+        else:
+            # Otherwise just use the mobility 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,
+                timesync_endpoint=self._robot.time_sync.endpoint,
+            )
         self.last_velocity_command_time = end_time
         return response[0], response[1]