Add support for trajectory high-level command with relative_yaw

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
 
@@ -230,8 +231,8 @@ 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,
-                         reversed=False, group_mask=ALL_GROUPS):
+    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
 
@@ -240,20 +241,35 @@ 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
         :return:
         """
-        self._send_packet(struct.pack('<BBBBBf',
-                                      self.COMMAND_START_TRAJECTORY,
-                                      group_mask,
-                                      relative,
-                                      reversed,
-                                      trajectory_id,
-                                      time_scale))
+        if self._cf.platform.get_protocol_version() < 10:
+            if relative_yaw:
+                print('Warning: Relative yaw is not supported in protocol '
+                      'version < 9, update your Crazyflie\'s firmware')
+            self._send_packet(struct.pack('<BBBBBf',
+                                          self.COMMAND_START_TRAJECTORY,
+                                          group_mask,
+                                          relative_position,
+                                          reversed,
+                                          trajectory_id,
+                                          time_scale))
+        else:
+            self._send_packet(struct.pack('<BBBBBBf',
+                                          self.COMMAND_START_TRAJECTORY_2,
+                                          group_mask,
+                                          relative_position,
+                                          relative_yaw,
+                                          reversed,
+                                          trajectory_id,
+                                          time_scale))
 
     def define_trajectory(self, trajectory_id, offset, n_pieces, type=TRAJECTORY_TYPE_POLY4D):
         """

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,24 +88,34 @@ 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_position=True, relative_yaw=relative_yaw)
     time.sleep(duration)
     commander.land(0.0, 2.0)
     time.sleep(2)
     commander.stop()
 
 
 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)