diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index ba313a1..fcc816c 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -539,13 +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(now_sec() + cmd_duration) - arm_velocity_command.end_time.CopyFrom(end_time) + end_time = now_sec() + 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=now_sec() + cmd_duration) + self._robot_command_client.robot_command( + command=robot_command, + end_time_secs=end_time, + timesync_endpoint=self._robot.time_sync.endpoint, + ) except Exception as e: return ( diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 066bdaf..92d62cc 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -1290,6 +1290,7 @@ def velocity_cmd( v_x: float, v_y: float, v_rot: float, + timestamp: float | None = None, cmd_duration: float = 0.125, body_height: float = 0.0, use_obstacle_params: bool = False, @@ -1302,6 +1303,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 @@ -1310,7 +1312,8 @@ def velocity_cmd( Returns: Tuple of bool success and a string message """ - end_time = now_sec() + 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(