From 41a09bf0a08decc183e30920c31dc035b992fd40 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Thu, 26 Jun 2025 15:25:21 +0200 Subject: [PATCH 1/4] Add support for trajectory high-level command with relative_yaw Chooses between legacy and new command format based on drone CRTP version, ensuring backward compatibility while enabling relative_yaw flag. --- cflib/crazyflie/high_level_commander.py | 30 ++++++++++++++++++------- 1 file changed, 22 insertions(+), 8 deletions(-) diff --git a/cflib/crazyflie/high_level_commander.py b/cflib/crazyflie/high_level_commander.py index e52ad520e..271e86240 100644 --- a/cflib/crazyflie/high_level_commander.py +++ b/cflib/crazyflie/high_level_commander.py @@ -55,6 +55,7 @@ class HighLevelCommander(): COMMAND_LAND_2 = 8 COMMAND_SPIRAL = 11 COMMAND_GO_TO_2 = 12 + COMMAND_START_TRAJECTORY_2 = 13 ALL_GROUPS = 0 @@ -231,7 +232,7 @@ def spiral(self, angle, r0, rF, ascent, duration_s, sideways=False, clockwise=Fa duration_s)) def start_trajectory(self, trajectory_id, time_scale=1.0, relative=False, - reversed=False, group_mask=ALL_GROUPS): + relative_yaw=False, reversed=False, group_mask=ALL_GROUPS): """ starts executing a specified trajectory @@ -247,13 +248,26 @@ def start_trajectory(self, trajectory_id, time_scale=1.0, relative=False, :param group_mask: Mask for which CFs this should apply to :return: """ - self._send_packet(struct.pack(' Date: Thu, 26 Jun 2025 17:14:09 +0200 Subject: [PATCH 2/4] Refactor: Clarify naming of relative flag to indicate it applies to position Updated the naming of the relative flag used in trajectory commands to make it explicit that it applies to position. This improves clarity and aligns with the newly introduced relative yaw flag. --- cflib/crazyflie/high_level_commander.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/cflib/crazyflie/high_level_commander.py b/cflib/crazyflie/high_level_commander.py index 271e86240..a9904af64 100644 --- a/cflib/crazyflie/high_level_commander.py +++ b/cflib/crazyflie/high_level_commander.py @@ -231,7 +231,7 @@ def spiral(self, angle, r0, rF, ascent, duration_s, sideways=False, clockwise=Fa ascent, duration_s)) - def start_trajectory(self, trajectory_id, time_scale=1.0, relative=False, + def start_trajectory(self, trajectory_id, time_scale=1.0, relative_position=False, relative_yaw=False, reversed=False, group_mask=ALL_GROUPS): """ starts executing a specified trajectory @@ -241,8 +241,10 @@ def start_trajectory(self, trajectory_id, time_scale=1.0, relative=False, :param time_scale: Time factor; 1.0 = original speed; >1.0: slower; <1.0: faster - :param relative: Set to True, if trajectory should be shifted to + :param relative_position: Set to True, if trajectory should be shifted to current setpoint + :param relative_yaw: Set to True, if trajectory should be aligned to + current yaw :param reversed: Set to True, if trajectory should be executed in reverse :param group_mask: Mask for which CFs this should apply to @@ -255,7 +257,7 @@ def start_trajectory(self, trajectory_id, time_scale=1.0, relative=False, self._send_packet(struct.pack(' Date: Thu, 26 Jun 2025 18:22:36 +0200 Subject: [PATCH 3/4] Add --relative-yaw flag to demonstrate relative yaw functionality Enables running the trajectory with relative_yaw=True, rotating it based on the prior yaw command. --- .../autonomous_sequence_high_level.py | 23 ++++++++++++++----- 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/examples/autonomy/autonomous_sequence_high_level.py b/examples/autonomy/autonomous_sequence_high_level.py index afa995fae..556da5a42 100644 --- a/examples/autonomy/autonomous_sequence_high_level.py +++ b/examples/autonomy/autonomous_sequence_high_level.py @@ -6,7 +6,7 @@ # +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ # || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ # -# Copyright (C) 2018 Bitcraze AB +# Copyright (C) 2025 Bitcraze AB # # This program is free software; you can redistribute it and/or # modify it under the terms of the GNU General Public License @@ -28,6 +28,7 @@ It aims at documenting how to set the Crazyflie in position control mode and how to send setpoints using the high level commander. """ +import argparse import sys import time @@ -87,17 +88,17 @@ def upload_trajectory(cf, trajectory_id, trajectory): return total_duration -def run_sequence(cf, trajectory_id, duration): +def run_sequence(cf, trajectory_id, duration, relative_yaw=False): commander = cf.high_level_commander # Arm the Crazyflie cf.platform.send_arming_request(True) time.sleep(1.0) - commander.takeoff(1.0, 2.0) + takeoff_yaw = 3.14 / 2 if relative_yaw else 0.0 + commander.takeoff(1.0, 2.0, yaw=takeoff_yaw) time.sleep(3.0) - relative = True - commander.start_trajectory(trajectory_id, 1.0, relative) + commander.start_trajectory(trajectory_id, 1.0, relative=True, relative_yaw=relative_yaw) time.sleep(duration) commander.land(0.0, 2.0) time.sleep(2) @@ -105,6 +106,16 @@ def run_sequence(cf, trajectory_id, duration): if __name__ == '__main__': + parser = argparse.ArgumentParser(description='Crazyflie trajectory demo') + parser.add_argument('--relative-yaw', action='store_true', + help=( + 'Enable relative_yaw mode when running the trajectory. This demonstrates ' + 'how the trajectory can be rotated based on the orientation from the prior command.' + ) + ) + + args = parser.parse_args() + cflib.crtp.init_drivers() with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: @@ -115,4 +126,4 @@ def run_sequence(cf, trajectory_id, duration): duration = upload_trajectory(cf, trajectory_id, figure8) print('The sequence is {:.1f} seconds long'.format(duration)) reset_estimator(cf) - run_sequence(cf, trajectory_id, duration) + run_sequence(cf, trajectory_id, duration, relative_yaw=args.relative_yaw) From 30a9604b918e63d5900ddf333298b6e2d999849f Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Fri, 27 Jun 2025 08:37:42 +0200 Subject: [PATCH 4/4] Fix: Update example to use renamed relative position flag --- examples/autonomy/autonomous_sequence_high_level.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/autonomy/autonomous_sequence_high_level.py b/examples/autonomy/autonomous_sequence_high_level.py index 556da5a42..f4f33bd85 100644 --- a/examples/autonomy/autonomous_sequence_high_level.py +++ b/examples/autonomy/autonomous_sequence_high_level.py @@ -98,7 +98,7 @@ def run_sequence(cf, trajectory_id, duration, relative_yaw=False): takeoff_yaw = 3.14 / 2 if relative_yaw else 0.0 commander.takeoff(1.0, 2.0, yaw=takeoff_yaw) time.sleep(3.0) - commander.start_trajectory(trajectory_id, 1.0, relative=True, relative_yaw=relative_yaw) + commander.start_trajectory(trajectory_id, 1.0, relative_position=True, relative_yaw=relative_yaw) time.sleep(duration) commander.land(0.0, 2.0) time.sleep(2)