From 789d0dc6341c98bece1510b4f778e0efab2bef09 Mon Sep 17 00:00:00 2001 From: hmllr Date: Tue, 7 Mar 2023 15:31:04 +0100 Subject: [PATCH 1/3] circling square demo with colors First turn off LEDs, then take off as first thing in swarm cmd to avoid delays --- examples/autonomy/circling_square_demo.py | 233 ++++++++++++++++++++++ 1 file changed, 233 insertions(+) create mode 100644 examples/autonomy/circling_square_demo.py diff --git a/examples/autonomy/circling_square_demo.py b/examples/autonomy/circling_square_demo.py new file mode 100644 index 000000000..e1487578a --- /dev/null +++ b/examples/autonomy/circling_square_demo.py @@ -0,0 +1,233 @@ +# -*- coding: utf-8 -*- +# +# || ____ _ __ +# +------+ / __ )(_) /_______________ _____ ___ +# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2018 Bitcraze AB +# +# This program is free software; you can redistribute it and/or +# modify it under the terms of the GNU General Public License +# as published by the Free Software Foundation; either version 2 +# of the License, or (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . +""" +Simple "show" example that connects to 8 crazyflies (check the addresses at the top +and update it to your crazyflies addresses) and uses the high level commander with bezier curves +to send trajectories to fly a circle (while the 8 drones are positioned in a square). +To spice it up, the LEDs are changing color - the color move factor defines how fast and in which direction. + +This example is intended to work with any positioning system (including LPS). +It aims at documenting how to set the Crazyflie in position control mode +and how to send setpoints using the high level commander. +""" +import sys +import time + +import numpy as np +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.high_level_commander import HighLevelCommander +from cflib.crazyflie.log import LogConfig +from cflib.crazyflie.mem import CompressedSegment +from cflib.crazyflie.mem import CompressedStart +from cflib.crazyflie.mem import MemoryElement +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.crazyflie.syncLogger import SyncLogger +from cflib.utils import uri_helper +from cflib.crazyflie.swarm import CachedCfFactory +from cflib.crazyflie.swarm import Swarm + +URI1 = 'radio://0/60/2M/E7E7E7E710' +URI2 = 'radio://0/60/2M/E7E7E7E711' +URI3 = 'radio://0/60/2M/E7E7E7E712' +URI4 = 'radio://0/60/2M/E7E7E7E713' +URI5 = 'radio://0/60/2M/E7E7E7E714' +URI6 = 'radio://0/60/2M/E7E7E7E715' +URI7 = 'radio://0/60/2M/E7E7E7E716' +URI8 = 'radio://0/60/2M/E7E7E7E717' + +# The trajectory to fly +a = 0.55 # where the Beizer curve control point should be https://spencermortensen.com/articles/bezier-circle/ +h = 1.0 # [m] how high we should fly +t = 2.0 # seconds per step, one circle has 4 steps +r1 = 1.0 # [m] the radius at which the first half of the drones are +r2 = np.sqrt(2.0) # [m] the radius at which every second other drone is +loops = 2 # how many loops we should fly +color_move_factor = 3 # magic factor which defines how fast the colors move + +def rotate_beizer_node(xl, yl, alpha): + x_rot = [] + y_rot = [] + for x,y in zip(xl,yl): + x_rot.append(x*np.cos(alpha) - y*np.sin(alpha)) + y_rot.append(x*np.sin(alpha) + y*np.cos(alpha)) + return x_rot, y_rot + + +def wait_for_position_estimator(scf): + print('Waiting for estimator to find position...') + + log_config = LogConfig(name='Kalman Variance', period_in_ms=500) + log_config.add_variable('kalman.varPX', 'float') + log_config.add_variable('kalman.varPY', 'float') + log_config.add_variable('kalman.varPZ', 'float') + + var_y_history = [1000] * 10 + var_x_history = [1000] * 10 + var_z_history = [1000] * 10 + + threshold = 0.001 + + with SyncLogger(scf, log_config) as logger: + for log_entry in logger: + data = log_entry[1] + + var_x_history.append(data['kalman.varPX']) + var_x_history.pop(0) + var_y_history.append(data['kalman.varPY']) + var_y_history.pop(0) + var_z_history.append(data['kalman.varPZ']) + var_z_history.pop(0) + + min_x = min(var_x_history) + max_x = max(var_x_history) + min_y = min(var_y_history) + max_y = max(var_y_history) + min_z = min(var_z_history) + max_z = max(var_z_history) + + # print("{} {} {}". + # format(max_x - min_x, max_y - min_y, max_z - min_z)) + + if (max_x - min_x) < threshold and ( + max_y - min_y) < threshold and ( + max_z - min_z) < threshold: + break + + +def reset_estimator(cf): + cf.param.set_value('kalman.resetEstimation', '1') + time.sleep(0.1) + cf.param.set_value('kalman.resetEstimation', '0') + + wait_for_position_estimator(cf) + + +def activate_high_level_commander(cf): + cf.param.set_value('commander.enHighLevel', '1') + + +def activate_mellinger_controller(cf): + cf.param.set_value('stabilizer.controller', '2') + + +def upload_trajectory(cf, trajectory_id, trajectory): + trajectory_mem = cf.mem.get_mems(MemoryElement.TYPE_TRAJ)[0] + + trajectory_mem.trajectory = trajectory + + upload_result = trajectory_mem.write_data_sync() + if not upload_result: + print('Upload failed, aborting!') + sys.exit(1) + cf.high_level_commander.define_trajectory( + trajectory_id, + 0, + len(trajectory), + type=HighLevelCommander.TRAJECTORY_TYPE_POLY4D_COMPRESSED) + + total_duration = 0 + # Skip the start element + for segment in trajectory[1:]: + total_duration += segment.duration + + return total_duration + +def turn_off_leds(scf): + # Set solid color effect + scf.cf.param.set_value('ring.effect', '7') + # Set the RGB values + scf.cf.param.set_value('ring.solidRed', '0') + scf.cf.param.set_value('ring.solidGreen', '0') + scf.cf.param.set_value('ring.solidBlue', '0') + +def run_sequence(scf, alpha, r): + commander = scf.cf.high_level_commander + trajectory_id = 1 + duration = 4*t + commander.takeoff(h, 2.0) + time.sleep(3.0) + x_start, y_start = rotate_beizer_node([r],[0.0], alpha) + commander.go_to(x_start[0], y_start[0], h, 0.0, 2.0) + time.sleep(3.0) + relative = False + start_time_leds = time.time() + for i in range(loops): + commander.start_trajectory(trajectory_id, 1.0, relative) + start_time = time.time() + end_time = start_time + duration + while True: + color_angle = alpha + ((time.time() - start_time_leds)/duration)*2.0*3.14*color_move_factor + scf.cf.param.set_value('ring.solidRed', np.cos(color_angle)*127 + 127) + scf.cf.param.set_value('ring.solidGreen', 128 - np.cos(color_angle)*127) + scf.cf.param.set_value('ring.solidBlue', '0') + if time.time() > end_time: + break + time.sleep(0.2) + commander.land(0.0, 2.0) + scf.cf.param.set_value('ring.solidRed', '0') + scf.cf.param.set_value('ring.solidGreen', '0') + scf.cf.param.set_value('ring.solidBlue', '0') + time.sleep(20) # sleep long enough to be sure to have turned off leds + commander.stop() + +def create_trajectory(alpha, r): + x_start, y_start = rotate_beizer_node([r],[0.0], alpha) + beizer_point_1_x, beizer_point_1_y = rotate_beizer_node([r, r*a, 0.0], [r*a, r, r], alpha) + beizer_point_2_x, beizer_point_2_y = rotate_beizer_node([-r*a, -r, -r], [r, r*a, 0.0], alpha) + beizer_point_3_x, beizer_point_3_y = rotate_beizer_node([-r, -r*a, 0.0], [-r*a, -r, -r], alpha) + beizer_point_4_x, beizer_point_4_y = rotate_beizer_node([r*a, r, r], [-r, -r*a, 0.0], alpha) + trajectory = [ + CompressedStart(x_start[0], y_start[0], h, 0.0), + CompressedSegment(t, beizer_point_1_x, beizer_point_1_y, [h], []), + CompressedSegment(t, beizer_point_2_x, beizer_point_2_y, [h], []), + CompressedSegment(t, beizer_point_3_x, beizer_point_3_y, [h], []), + CompressedSegment(t, beizer_point_4_x, beizer_point_4_y, [h], []), + ] + return trajectory + +def upload_trajectories(scf, alpha, r): + trajectory_id = 1 + trajectory = create_trajectory(alpha, r) + upload_trajectory(scf.cf, trajectory_id, trajectory) + + +if __name__ == '__main__': + cflib.crtp.init_drivers() + uris = [URI1, URI2, URI3, URI4, URI5, URI6, URI7, URI8] + # uris = [URI1, URI5] + position_params = { + URI1:[0.0,r1], + URI2:[3.14/4,r2], + URI3:[3.14/2,r1], + URI4:[3.14/4*3,r2], + URI5:[3.14,r1], + URI6:[3.14/4*5,r2], + URI7:[3.14/4*6,r1], + URI8:[3.14/4*7,r2]} + + factory = CachedCfFactory(rw_cache='./cache') + with Swarm(uris, factory=factory) as swarm: + swarm.parallel_safe(turn_off_leds) + swarm.parallel_safe(upload_trajectories, args_dict=position_params) + time.sleep(5) + swarm.parallel_safe(run_sequence, args_dict=position_params) From f12e36b4dd734b23d60dcdb48bb68b73c0affacd Mon Sep 17 00:00:00 2001 From: hmllr Date: Wed, 22 Mar 2023 10:08:38 +0100 Subject: [PATCH 2/3] style guide fixes --- examples/autonomy/circling_square_demo.py | 59 ++++++++++++----------- 1 file changed, 31 insertions(+), 28 deletions(-) diff --git a/examples/autonomy/circling_square_demo.py b/examples/autonomy/circling_square_demo.py index e1487578a..3eb7fd74f 100644 --- a/examples/autonomy/circling_square_demo.py +++ b/examples/autonomy/circling_square_demo.py @@ -33,18 +33,16 @@ import time import numpy as np + import cflib.crtp -from cflib.crazyflie import Crazyflie from cflib.crazyflie.high_level_commander import HighLevelCommander from cflib.crazyflie.log import LogConfig from cflib.crazyflie.mem import CompressedSegment from cflib.crazyflie.mem import CompressedStart from cflib.crazyflie.mem import MemoryElement -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.crazyflie.syncLogger import SyncLogger -from cflib.utils import uri_helper from cflib.crazyflie.swarm import CachedCfFactory from cflib.crazyflie.swarm import Swarm +from cflib.crazyflie.syncLogger import SyncLogger URI1 = 'radio://0/60/2M/E7E7E7E710' URI2 = 'radio://0/60/2M/E7E7E7E711' @@ -56,18 +54,19 @@ URI8 = 'radio://0/60/2M/E7E7E7E717' # The trajectory to fly -a = 0.55 # where the Beizer curve control point should be https://spencermortensen.com/articles/bezier-circle/ -h = 1.0 # [m] how high we should fly -t = 2.0 # seconds per step, one circle has 4 steps -r1 = 1.0 # [m] the radius at which the first half of the drones are -r2 = np.sqrt(2.0) # [m] the radius at which every second other drone is -loops = 2 # how many loops we should fly -color_move_factor = 3 # magic factor which defines how fast the colors move +a = 0.55 # where the Beizer curve control point should be https://spencermortensen.com/articles/bezier-circle/ +h = 1.0 # [m] how high we should fly +t = 2.0 # seconds per step, one circle has 4 steps +r1 = 1.0 # [m] the radius at which the first half of the drones are +r2 = np.sqrt(2.0) # [m] the radius at which every second other drone is +loops = 2 # how many loops we should fly +color_move_factor = 3 # magic factor which defines how fast the colors move + def rotate_beizer_node(xl, yl, alpha): x_rot = [] y_rot = [] - for x,y in zip(xl,yl): + for x, y in zip(xl, yl): x_rot.append(x*np.cos(alpha) - y*np.sin(alpha)) y_rot.append(x*np.sin(alpha) + y*np.cos(alpha)) return x_rot, y_rot @@ -152,6 +151,7 @@ def upload_trajectory(cf, trajectory_id, trajectory): return total_duration + def turn_off_leds(scf): # Set solid color effect scf.cf.param.set_value('ring.effect', '7') @@ -160,13 +160,14 @@ def turn_off_leds(scf): scf.cf.param.set_value('ring.solidGreen', '0') scf.cf.param.set_value('ring.solidBlue', '0') + def run_sequence(scf, alpha, r): commander = scf.cf.high_level_commander trajectory_id = 1 duration = 4*t commander.takeoff(h, 2.0) time.sleep(3.0) - x_start, y_start = rotate_beizer_node([r],[0.0], alpha) + x_start, y_start = rotate_beizer_node([r], [0.0], alpha) commander.go_to(x_start[0], y_start[0], h, 0.0, 2.0) time.sleep(3.0) relative = False @@ -187,24 +188,26 @@ def run_sequence(scf, alpha, r): scf.cf.param.set_value('ring.solidRed', '0') scf.cf.param.set_value('ring.solidGreen', '0') scf.cf.param.set_value('ring.solidBlue', '0') - time.sleep(20) # sleep long enough to be sure to have turned off leds + time.sleep(20) # sleep long enough to be sure to have turned off leds commander.stop() + def create_trajectory(alpha, r): - x_start, y_start = rotate_beizer_node([r],[0.0], alpha) + x_start, y_start = rotate_beizer_node([r], [0.0], alpha) beizer_point_1_x, beizer_point_1_y = rotate_beizer_node([r, r*a, 0.0], [r*a, r, r], alpha) beizer_point_2_x, beizer_point_2_y = rotate_beizer_node([-r*a, -r, -r], [r, r*a, 0.0], alpha) beizer_point_3_x, beizer_point_3_y = rotate_beizer_node([-r, -r*a, 0.0], [-r*a, -r, -r], alpha) beizer_point_4_x, beizer_point_4_y = rotate_beizer_node([r*a, r, r], [-r, -r*a, 0.0], alpha) trajectory = [ - CompressedStart(x_start[0], y_start[0], h, 0.0), - CompressedSegment(t, beizer_point_1_x, beizer_point_1_y, [h], []), - CompressedSegment(t, beizer_point_2_x, beizer_point_2_y, [h], []), - CompressedSegment(t, beizer_point_3_x, beizer_point_3_y, [h], []), - CompressedSegment(t, beizer_point_4_x, beizer_point_4_y, [h], []), + CompressedStart(x_start[0], y_start[0], h, 0.0), + CompressedSegment(t, beizer_point_1_x, beizer_point_1_y, [h], []), + CompressedSegment(t, beizer_point_2_x, beizer_point_2_y, [h], []), + CompressedSegment(t, beizer_point_3_x, beizer_point_3_y, [h], []), + CompressedSegment(t, beizer_point_4_x, beizer_point_4_y, [h], []), ] return trajectory + def upload_trajectories(scf, alpha, r): trajectory_id = 1 trajectory = create_trajectory(alpha, r) @@ -216,14 +219,14 @@ def upload_trajectories(scf, alpha, r): uris = [URI1, URI2, URI3, URI4, URI5, URI6, URI7, URI8] # uris = [URI1, URI5] position_params = { - URI1:[0.0,r1], - URI2:[3.14/4,r2], - URI3:[3.14/2,r1], - URI4:[3.14/4*3,r2], - URI5:[3.14,r1], - URI6:[3.14/4*5,r2], - URI7:[3.14/4*6,r1], - URI8:[3.14/4*7,r2]} + URI1: [0.0, r1], + URI2: [3.14/4, r2], + URI3: [3.14/2, r1], + URI4: [3.14/4*3, r2], + URI5: [3.14, r1], + URI6: [3.14/4*5, r2], + URI7: [3.14/4*6, r1], + URI8: [3.14/4*7, r2]} factory = CachedCfFactory(rw_cache='./cache') with Swarm(uris, factory=factory) as swarm: From ded4281370274ac02bd6c4640e264ae2c0005174 Mon Sep 17 00:00:00 2001 From: hmllr Date: Thu, 23 Mar 2023 11:22:24 +0100 Subject: [PATCH 3/3] Update circling_square_demo.py change 3.14 to pi and copyrightyear --- examples/autonomy/circling_square_demo.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/examples/autonomy/circling_square_demo.py b/examples/autonomy/circling_square_demo.py index 3eb7fd74f..88f2ed495 100644 --- a/examples/autonomy/circling_square_demo.py +++ b/examples/autonomy/circling_square_demo.py @@ -6,7 +6,7 @@ # +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ # || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ # -# Copyright (C) 2018 Bitcraze AB +# Copyright (C) 2023 Bitcraze AB # # This program is free software; you can redistribute it and/or # modify it under the terms of the GNU General Public License @@ -177,7 +177,7 @@ def run_sequence(scf, alpha, r): start_time = time.time() end_time = start_time + duration while True: - color_angle = alpha + ((time.time() - start_time_leds)/duration)*2.0*3.14*color_move_factor + color_angle = alpha + ((time.time() - start_time_leds)/duration)*2.0*np.pi*color_move_factor scf.cf.param.set_value('ring.solidRed', np.cos(color_angle)*127 + 127) scf.cf.param.set_value('ring.solidGreen', 128 - np.cos(color_angle)*127) scf.cf.param.set_value('ring.solidBlue', '0') @@ -220,13 +220,13 @@ def upload_trajectories(scf, alpha, r): # uris = [URI1, URI5] position_params = { URI1: [0.0, r1], - URI2: [3.14/4, r2], - URI3: [3.14/2, r1], - URI4: [3.14/4*3, r2], - URI5: [3.14, r1], - URI6: [3.14/4*5, r2], - URI7: [3.14/4*6, r1], - URI8: [3.14/4*7, r2]} + URI2: [np.pi/4, r2], + URI3: [np.pi/2, r1], + URI4: [np.pi/4*3, r2], + URI5: [np.pi, r1], + URI6: [np.pi/4*5, r2], + URI7: [np.pi/4*6, r1], + URI8: [np.pi/4*7, r2]} factory = CachedCfFactory(rw_cache='./cache') with Swarm(uris, factory=factory) as swarm: