From e4cefa1777d6947b0b8e65fd31c95d4c07b18da5 Mon Sep 17 00:00:00 2001 From: kabirkedia Date: Wed, 3 Sep 2025 16:09:25 -0400 Subject: [PATCH 1/5] add timesync endpoint to the spot_wrapper for arm velocity control --- spot_wrapper/spot_arm.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index 3a6727c0..23816536 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -539,14 +539,17 @@ def handle_arm_velocity( 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_command.end_time.CopyFrom(end_time) + end_time = time.time() + cmd_duration + end_time_proto = self._robot.time_sync.robot_timestamp_from_local_secs(end_time) + arm_velocity_command.end_time.CopyFrom(end_time_proto) robot_command = robot_command_pb2.RobotCommand() robot_command.synchronized_command.arm_command.arm_velocity_command.CopyFrom(arm_velocity_command) self._robot_command_client.robot_command( - command=robot_command, end_time_secs=time.time() + cmd_duration + command=robot_command, + end_time_secs=end_time, + timesync_endpoint=self._robot.time_sync.endpoint, ) except Exception as e: From 9cf7b9c3f978fd7467f88650a070bca8feb83780 Mon Sep 17 00:00:00 2001 From: kabirkedia Date: Wed, 3 Sep 2025 17:29:42 -0400 Subject: [PATCH 2/5] add timestamp from ros message --- spot_wrapper/spot_arm.py | 2 +- spot_wrapper/wrapper.py | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index 23816536..558265b2 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -539,7 +539,7 @@ def handle_arm_velocity( self._logger.info(msg) return False, msg else: - end_time = time.time() + cmd_duration + end_time = arm_velocity_command.end_time.seconds + arm_velocity_command.end_time.nanos/1e9 + cmd_duration end_time_proto = self._robot.time_sync.robot_timestamp_from_local_secs(end_time) arm_velocity_command.end_time.CopyFrom(end_time_proto) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 25f0e6a4..2015d4c9 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -1289,6 +1289,7 @@ def velocity_cmd( v_x: float, v_y: float, v_rot: float, + timestamp: float, cmd_duration: float = 0.125, body_height: float = 0.0, use_obstacle_params: bool = False, @@ -1309,7 +1310,7 @@ def velocity_cmd( Returns: Tuple of bool success and a string message """ - end_time = time.time() + cmd_duration + end_time = timestamp + cmd_duration if body_height: current_mobility_params = self.get_mobility_params() height_adjusted_params = RobotCommandBuilder.mobility_params( From 75c898edb0acd38ae27eff9390fb839054e42259 Mon Sep 17 00:00:00 2001 From: kabirkedia Date: Thu, 4 Sep 2025 13:34:01 -0400 Subject: [PATCH 3/5] make timestampe optional --- spot_wrapper/wrapper.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 2015d4c9..031a01c9 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -1289,7 +1289,7 @@ def velocity_cmd( v_x: float, v_y: float, v_rot: float, - timestamp: float, + timestamp: float = time.time(), cmd_duration: float = 0.125, body_height: float = 0.0, use_obstacle_params: bool = False, @@ -1302,6 +1302,7 @@ def velocity_cmd( v_x: Velocity in the X direction in meters v_y: Velocity in the Y direction in meters v_rot: Angular velocity around the Z axis in radians + timestamp: (optional) Time at which the command is sent, in seconds since the epoch. Default is now. 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 From 3a850bd13f48d3b757cdcb08cbb427f945c30ebf Mon Sep 17 00:00:00 2001 From: kabirkedia Date: Tue, 7 Oct 2025 16:39:26 -0400 Subject: [PATCH 4/5] ran precommit --- spot_wrapper/spot_arm.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index 4f5d7f27..fcc816cc 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -546,7 +546,7 @@ def handle_arm_velocity( robot_command = robot_command_pb2.RobotCommand() robot_command.synchronized_command.arm_command.arm_velocity_command.CopyFrom(arm_velocity_command) self._robot_command_client.robot_command( - command=robot_command, + command=robot_command, end_time_secs=end_time, timesync_endpoint=self._robot.time_sync.endpoint, ) From 0e403b21932dd8ae974507169129486351b3c16d Mon Sep 17 00:00:00 2001 From: kabirkedia Date: Wed, 8 Oct 2025 14:28:24 -0400 Subject: [PATCH 5/5] resolve the CI Error --- spot_wrapper/wrapper.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index a471124c..92d62ccf 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -1290,7 +1290,7 @@ def velocity_cmd( v_x: float, v_y: float, v_rot: float, - timestamp: float = now_sec(), + timestamp: float | None = None, cmd_duration: float = 0.125, body_height: float = 0.0, use_obstacle_params: bool = False, @@ -1312,7 +1312,8 @@ def velocity_cmd( Returns: Tuple of bool success and a string message """ - end_time = timestamp + cmd_duration + start_time = now_sec() if timestamp is None else timestamp + end_time = start_time + cmd_duration if body_height: current_mobility_params = self.get_mobility_params() height_adjusted_params = RobotCommandBuilder.mobility_params(