Sorry for being unclear, I meant that it is easier to send the setpoints directly to the controller and skip the high level commander.
One thing to note is that you must continuously send setpoints (atleast 4-5/s) after take off, otherwise you will trigger a safety mechanism in the Crazyflie that will shut off the motors (assuming we lost contact with the ground station).
Script to retrace recorded trajectory. Note that the output from the record script should be pasted into this script.
Code: Select all
"""
Replay a trajectory
"""
import time
import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.log import LogConfig
from cflib.crazyflie.mem import MemoryElement
from cflib.crazyflie.mem import Poly4D
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from cflib.crazyflie.syncLogger import SyncLogger
from cflib.crazyflie.swarm import CachedCfFactory
from cflib.crazyflie.swarm import Swarm
# Output from recording is pasted here
trajectory = [
[5.400000, -0.046623, -1.483068, 0.762471, 82.846436],
[5.500000, -0.047361, -1.393556, 0.813164, 84.603775],
[5.600000, -0.059637, -1.363293, 0.820030, 84.172287],
[5.700000, -0.066751, -1.336190, 0.816958, 81.779724],
[5.800000, -0.069864, -1.308472, 0.821256, 75.042160],
[5.900000, -0.080665, -1.279595, 0.824106, 69.356316],
[6.000000, -0.080370, -1.251695, 0.828467, 65.894135],
[6.100000, -0.082539, -1.222107, 0.829532, 62.976616],
[6.200000, -0.082787, -1.189730, 0.829509, 60.504341],
[6.300000, -0.081539, -1.156130, 0.826049, 58.877171],
[6.400000, -0.077907, -1.121137, 0.819377, 58.178196],
[6.500000, -0.066879, -1.083180, 0.812653, 59.252960],
[6.600000, -0.053675, -1.043357, 0.809948, 61.132259],
[6.700000, -0.041428, -1.004326, 0.814144, 64.737968],
[6.800000, -0.029411, -0.968979, 0.821937, 69.534546],
[6.900000, -0.020582, -0.936722, 0.824898, 74.548950],
[7.000000, -0.014653, -0.904782, 0.823062, 76.927361],
[7.100000, -0.000151, -0.875139, 0.824938, 77.834595],
[7.200000, -0.001970, -0.839843, 0.810099, 79.992210],
[7.300000, 0.001109, -0.802715, 0.800625, 80.943268],
[7.400000, 0.001555, -0.764795, 0.793533, 79.931564],
[7.500000, 0.002797, -0.724406, 0.787105, 74.448074],
[7.600000, -0.000397, -0.688022, 0.782962, 64.401520],
[7.700000, 0.018339, -0.658532, 0.788613, 59.271442],
[7.800000, 0.029360, -0.590794, 0.817208, 55.675495],
[7.900000, 0.050684, -0.563149, 0.823424, 51.057396],
[8.000000, 0.079647, -0.540493, 0.825355, 49.538200],
[8.100000, 0.109896, -0.526162, 0.824579, 51.219215],
[8.200000, 0.142115, -0.519066, 0.816511, 54.697765],
[8.300000, 0.175884, -0.512573, 0.804499, 58.219784],
[8.400000, 0.203641, -0.478185, 0.808745, 64.146255],
[8.500000, 0.229068, -0.461136, 0.802763, 70.117943],
[8.600000, 0.248275, -0.441172, 0.800030, 76.210114],
[8.700000, 0.268882, -0.415872, 0.801275, 80.397133],
[8.800000, 0.280903, -0.385706, 0.800784, 85.776161],
[8.900000, 0.290677, -0.353264, 0.799559, 91.427910],
[9.000000, 0.296246, -0.321653, 0.793055, 97.325310],
[9.100000, 0.300180, -0.285427, 0.790114, 102.996185],
[9.200000, 0.297368, -0.256180, 0.792900, 108.499481],
[9.300000, 0.291420, -0.232984, 0.794600, 117.379250],
[9.400000, 0.285174, -0.217094, 0.796162, 123.318878],
[9.500000, 0.282441, -0.205182, 0.796854, 125.983597],
[9.601000, 0.281805, -0.196767, 0.797027, 130.582062],
[9.701000, 0.289993, -0.194549, 0.798496, 136.410538],
[9.801000, 0.302597, -0.194705, 0.799659, 139.695297],
[9.901000, 0.310636, -0.193411, 0.804258, 141.941925],
[10.002000, 0.315161, -0.191946, 0.808310, 144.142685],
[10.100000, 0.319253, -0.188847, 0.813771, 144.897217],
[10.202000, 0.321448, -0.185337, 0.817889, 144.746872],
[10.300000, 0.321749, -0.184425, 0.820635, 145.460281],
[10.400000, 0.322916, -0.182957, 0.821464, 144.902496],
[10.500000, 0.325141, -0.180800, 0.820596, 144.089630],
[10.600000, 0.324196, -0.177482, 0.820580, 143.311401],
[10.700000, 0.314568, -0.171069, 0.822845, 142.640778],
[10.800000, 0.301888, -0.174088, 0.814857, 143.367004],
[10.900000, 0.274989, -0.159648, 0.818286, 143.417664],
[11.000000, 0.238710, -0.135476, 0.818622, 147.435715],
[11.100000, 0.192609, -0.102869, 0.828788, 149.457138],
[11.202000, 0.121393, -0.086206, 0.857264, 150.869476],
[11.300000, 0.064492, -0.060692, 0.867345, 152.213165],
[11.402000, 0.009199, -0.035161, 0.877946, 152.123779],
[11.500000, -0.042365, -0.013294, 0.886184, 152.419479],
[11.600000, -0.084726, 0.008046, 0.896372, 152.041611],
[11.700000, -0.130026, 0.026541, 0.901281, 152.393127],
[11.800000, -0.162568, 0.040818, 0.906327, 153.610229],
[11.900000, -0.187910, 0.051822, 0.907942, 153.907425],
[12.000000, -0.208461, 0.058913, 0.908455, 154.950348],
[12.100000, -0.222268, 0.065125, 0.911239, 155.066055],
[12.200000, -0.230135, 0.069143, 0.911024, 155.248764],
[12.302000, -0.233711, 0.071800, 0.909383, 155.441055],
[12.400000, -0.234907, 0.074022, 0.907196, 155.764938],
[12.502000, -0.234188, 0.077555, 0.906003, 155.466400],
[12.600000, -0.233200, 0.078978, 0.904372, 155.537643],
[12.702000, -0.231589, 0.078735, 0.902779, 156.045410],
[12.800000, -0.228161, 0.078074, 0.902425, 155.926010],
[12.902000, -0.222553, 0.073920, 0.901006, 156.608032],
[13.000000, -0.215126, 0.067212, 0.899366, 157.604004],
[13.101000, -0.200705, 0.059126, 0.900632, 158.011826],
[13.201000, -0.184218, 0.052638, 0.903326, 157.605682],
[13.300000, -0.160767, 0.046944, 0.905363, 158.107254],
[13.400000, -0.132374, 0.039068, 0.901664, 159.609528],
[13.500000, -0.091360, 0.035059, 0.898739, 160.820358],
[13.600000, -0.042628, 0.029065, 0.895779, 162.295654],
[13.701000, 0.000598, 0.020691, 0.897583, 162.223877],
[13.801000, 0.047377, 0.013213, 0.898069, 161.282150],
[13.900000, 0.088747, 0.004695, 0.898055, 159.553955],
[14.001000, 0.127440, -0.001550, 0.900602, 155.896774],
[14.100000, 0.151998, 0.002863, 0.898810, 150.351532],
[14.200000, 0.168572, 0.020592, 0.891069, 141.137253],
[14.300000, 0.181331, 0.053950, 0.889098, 130.363144],
[14.400000, 0.185751, 0.089106, 0.891717, 122.277512],
[14.500000, 0.179911, 0.126473, 0.889913, 118.228111],
[14.600000, 0.168139, 0.168882, 0.884054, 118.967461],
[14.700000, 0.162218, 0.221941, 0.873269, 117.599953],
[14.800000, 0.152207, 0.276901, 0.867829, 117.023293],
[14.900000, 0.138454, 0.325850, 0.863850, 118.322395],
[15.000000, 0.126450, 0.373337, 0.866681, 120.539169],
[15.100000, 0.113888, 0.417951, 0.870066, 124.454376],
[15.200000, 0.096409, 0.463486, 0.872850, 128.330917],
[15.300000, 0.084632, 0.507698, 0.870167, 134.123459],
[15.400000, 0.075949, 0.551666, 0.868117, 139.664139],
[15.500000, 0.065718, 0.591515, 0.866254, 147.888779],
[15.600000, 0.057157, 0.625083, 0.862384, 156.325424],
[15.700000, 0.050263, 0.653542, 0.861792, 165.931412],
[15.800000, 0.044840, 0.672475, 0.869083, 174.995865],
[15.900000, 0.035987, 0.679698, 0.873232, -176.889206],
[16.000000, 0.031933, 0.681341, 0.877860, -170.199432],
[16.100000, 0.017795, 0.679226, 0.878745, -164.821014],
[16.200000, 0.008859, 0.672684, 0.879141, -158.626328],
[16.300000, -0.001810, 0.668458, 0.882887, -155.356888],
[16.400000, -0.013572, 0.661749, 0.890053, -153.303375],
[16.500000, -0.025374, 0.652693, 0.894960, -151.227417],
[16.600000, -0.037357, 0.646304, 0.896470, -149.784592],
[16.700000, -0.044247, 0.638797, 0.896294, -148.500870],
[16.800000, -0.054011, 0.627448, 0.897501, -147.173615],
[16.900000, -0.059676, 0.610932, 0.897475, -145.524857],
[17.000000, -0.076153, 0.595791, 0.894804, -143.534500],
[17.100000, -0.087931, 0.576382, 0.895319, -141.276001],
[17.200000, -0.101048, 0.556051, 0.899925, -140.048920],
[17.300000, -0.115180, 0.535109, 0.903920, -139.913788],
[17.400000, -0.128533, 0.515344, 0.908306, -141.996094],
[17.500000, -0.147858, 0.501066, 0.912917, -145.767319],
[17.600000, -0.171879, 0.497260, 0.914510, -149.234802],
[17.700000, -0.192439, 0.497547, 0.913693, -150.653290],
[17.801000, -0.208069, 0.503069, 0.912844, -150.336227],
[17.901000, -0.232794, 0.515735, 0.912051, -151.592957],
[18.000000, -0.253801, 0.528238, 0.911665, -151.858078],
[18.100000, -0.272340, 0.541164, 0.911718, -152.077774],
[18.200000, -0.293113, 0.555201, 0.910980, -153.201019],
[18.300000, -0.307615, 0.565918, 0.912698, -154.127853],
[18.400000, -0.321831, 0.571500, 0.909084, -154.144882],
[18.500000, -0.334011, 0.569999, 0.907208, -154.404739],
[18.600000, -0.343095, 0.565756, 0.913959, -152.992371],
[18.700000, -0.345451, 0.551376, 0.914896, -150.311935],
[18.800000, -0.341044, 0.537976, 0.918523, -148.353973],
[18.900000, -0.334784, 0.523252, 0.919487, -147.916214],
[19.000000, -0.325640, 0.507924, 0.916097, -147.409164],
[19.100000, -0.317081, 0.496668, 0.915371, -148.369537],
[19.200000, -0.307171, 0.486724, 0.915962, -149.271133],
[19.300000, -0.298579, 0.479550, 0.916771, -150.826462],
[19.400000, -0.292454, 0.473091, 0.918505, -152.418198],
[19.500000, -0.289246, 0.467184, 0.921805, -154.928223],
[19.600000, -0.283680, 0.458309, 0.927418, -156.329483],
[19.700000, -0.278370, 0.453534, 0.932164, -158.291855],
[19.800000, -0.275078, 0.452329, 0.937822, -160.249664],
[19.900000, -0.275693, 0.449275, 0.942925, -162.023880],
[20.000000, -0.281309, 0.443927, 0.944006, -162.342407],
[20.100000, -0.292613, 0.440254, 0.943939, -162.436584],
[20.200000, -0.305672, 0.436720, 0.941985, -162.284637],
[20.300000, -0.323377, 0.433207, 0.936215, -162.369873],
[20.400000, -0.341929, 0.429356, 0.928638, -162.314011],
[20.500000, -0.360774, 0.426056, 0.922264, -162.134308],
[20.600000, -0.377360, 0.423401, 0.918520, -161.751389],
[20.700000, -0.391521, 0.420324, 0.915428, -160.701645],
[20.800000, -0.402444, 0.417258, 0.911941, -158.409195],
[20.900000, -0.410922, 0.417973, 0.910166, -157.781342],
[21.000000, -0.419157, 0.420354, 0.908463, -156.921661],
[21.100000, -0.424450, 0.422737, 0.906261, -155.502808],
[21.200000, -0.429944, 0.427150, 0.902388, -153.458130],
[21.300000, -0.435902, 0.431193, 0.895661, -151.366211],
[21.400000, -0.440796, 0.433938, 0.890285, -149.417755],
[21.500000, -0.445341, 0.432129, 0.880908, -146.601395],
[21.600000, -0.442475, 0.449230, 0.899447, -144.710129],
[21.700000, -0.445497, 0.451509, 0.899618, -142.111404],
[21.800000, -0.444956, 0.450667, 0.900012, -139.143295],
[21.900000, -0.449435, 0.450733, 0.898642, -135.112747],
[22.000000, -0.456917, 0.455062, 0.900622, -131.801346],
[22.100000, -0.463418, 0.456522, 0.899665, -129.603836],
[22.200000, -0.466760, 0.457840, 0.895979, -127.527031],
[22.300000, -0.475486, 0.461157, 0.897568, -125.008247],
[22.400000, -0.487764, 0.464076, 0.897628, -122.349541],
[22.500000, -0.492463, 0.464052, 0.900481, -119.331963],
[22.600000, -0.498622, 0.465022, 0.902053, -117.793381],
[22.700000, -0.503581, 0.466047, 0.898336, -116.492683],
[22.800000, -0.507393, 0.468721, 0.894880, -115.528450],
[22.900000, -0.508886, 0.469871, 0.890539, -114.258926],
[23.000000, -0.510318, 0.469329, 0.888048, -112.877457],
[23.100000, -0.512463, 0.467951, 0.887714, -111.788788],
[23.200000, -0.517202, 0.465377, 0.892095, -111.043152],
[23.300000, -0.522087, 0.465298, 0.895361, -110.423584],
[23.400000, -0.525428, 0.467577, 0.899576, -110.283272],
[23.500000, -0.527589, 0.467972, 0.899472, -110.294510],
[23.600000, -0.527789, 0.467816, 0.899079, -110.239861],
[23.700000, -0.527635, 0.468650, 0.899528, -110.608620],
[23.800000, -0.525360, 0.470913, 0.899101, -111.264130],
[23.900000, -0.521762, 0.472963, 0.897648, -112.033623],
[24.000000, -0.518507, 0.473502, 0.896078, -112.839180],
[24.100000, -0.515085, 0.474178, 0.896094, -113.729500],
[24.200000, -0.511506, 0.474851, 0.896650, -115.006584],
[24.300000, -0.507173, 0.474946, 0.895814, -116.882454],
[24.400000, -0.502559, 0.474459, 0.896814, -119.984970],
[24.500000, -0.497302, 0.472021, 0.900273, -122.877502],
[24.600000, -0.491228, 0.468080, 0.903080, -125.034584],
[24.700000, -0.483110, 0.464161, 0.906682, -126.890823],
[24.800000, -0.476857, 0.462277, 0.912308, -129.374451],
[24.900000, -0.471988, 0.461673, 0.915945, -130.782654],
[25.000000, -0.464826, 0.463808, 0.920092, -132.726181],
[25.100000, -0.454295, 0.460184, 0.927616, -134.630920],
[25.200000, -0.446902, 0.459114, 0.934398, -138.021286],
[25.300000, -0.438180, 0.457808, 0.942661, -140.441345],
[25.400000, -0.428586, 0.455538, 0.950296, -141.654510],
[25.500000, -0.417578, 0.453981, 0.958315, -143.204773],
[25.600000, -0.411773, 0.454847, 0.964336, -145.615250],
[25.700000, -0.399030, 0.451764, 0.971577, -147.556000],
[25.800000, -0.390161, 0.449143, 0.975474, -149.770386],
[25.900000, -0.381391, 0.445979, 0.978363, -151.616348],
[26.000000, -0.375257, 0.444479, 0.979279, -155.008713],
[26.100000, -0.371485, 0.444311, 0.979954, -159.193130],
[26.200000, -0.372066, 0.443676, 0.977613, -161.477005],
[26.300000, -0.368032, 0.437399, 0.978775, -162.898911],
[26.400000, -0.371852, 0.437876, 0.976464, -165.982040],
[26.500000, -0.374992, 0.434435, 0.973554, -166.109589],
[26.600000, -0.379728, 0.433049, 0.971891, -165.771408],
[26.700000, -0.387189, 0.431129, 0.971397, -166.313248],
[26.800000, -0.395911, 0.430379, 0.970693, -166.715408],
[26.900000, -0.405527, 0.429396, 0.966257, -166.831711],
[27.000000, -0.413944, 0.427412, 0.962656, -166.568436],
[27.100000, -0.423543, 0.425709, 0.957094, -166.786774],
[27.200000, -0.433399, 0.422405, 0.951179, -167.028564],
[27.300000, -0.442000, 0.420086, 0.946355, -167.785492],
[27.400000, -0.449406, 0.415657, 0.939222, -167.902786],
[27.500000, -0.455403, 0.411057, 0.930943, -168.468842],
[27.600000, -0.460190, 0.404735, 0.919994, -168.239838],
[27.700000, -0.464305, 0.398163, 0.907833, -168.139709],
[27.800000, -0.467552, 0.392361, 0.896193, -168.436493],
[27.900000, -0.469289, 0.388015, 0.887649, -168.995651],
[28.000000, -0.469569, 0.383832, 0.881809, -169.550079],
[28.100000, -0.468701, 0.379538, 0.878221, -170.599197],
[28.200000, -0.467161, 0.375380, 0.875951, -171.601318],
[28.300000, -0.464608, 0.371546, 0.875124, -172.909729],
[28.400000, -0.461016, 0.367284, 0.875254, -174.162628],
[28.500000, -0.440110, 0.407415, 0.924563, -171.495010],
[28.600000, -0.435611, 0.410871, 0.931705, -170.983246],
[28.700000, -0.431164, 0.411971, 0.938610, -169.405228],
[28.800000, -0.430665, 0.412623, 0.944380, -166.658020],
[28.900000, -0.425064, 0.415045, 0.952103, -164.151688],
[29.000000, -0.427240, 0.421557, 0.961185, -165.475266],
[29.100000, -0.426900, 0.423370, 0.974769, -165.773697],
[29.200000, -0.424892, 0.420869, 0.990924, -165.443207],
[29.300000, -0.426775, 0.420017, 1.007705, -165.591965],
[29.400000, -0.433260, 0.420795, 1.024368, -166.218994],
[29.500000, -0.435997, 0.420950, 1.038688, -166.297455],
[29.600000, -0.445871, 0.421503, 1.049848, -166.942841],
[29.700000, -0.456370, 0.421962, 1.058090, -167.507675],
[29.800000, -0.462014, 0.420604, 1.060297, -167.147690],
[29.900000, -0.471665, 0.418350, 1.060835, -165.494812],
[30.000000, -0.483893, 0.416253, 1.061958, -164.633286],
[30.100000, -0.499154, 0.398175, 1.074498, -163.295273],
[30.200000, -0.512847, 0.395647, 1.080874, -162.584595],
[30.300000, -0.527056, 0.391161, 1.087653, -161.492188],
[30.400000, -0.542780, 0.388828, 1.093485, -161.208206],
[30.500000, -0.560633, 0.387485, 1.099806, -161.282547],
[30.600000, -0.575754, 0.386549, 1.104162, -161.065155],
[30.700000, -0.592633, 0.385481, 1.105130, -160.937469],
[30.800000, -0.608893, 0.384480, 1.104031, -161.165131],
[30.900000, -0.623569, 0.381321, 1.100413, -160.968369],
[31.000000, -0.637320, 0.379125, 1.092797, -160.987198],
[31.100000, -0.649553, 0.377852, 1.081211, -161.421616],
[31.200000, -0.659161, 0.375144, 1.067424, -160.989227],
[31.300000, -0.666067, 0.372101, 1.051442, -160.607620],
[31.400000, -0.669467, 0.367780, 1.034234, -159.550034],
[31.500000, -0.667677, 0.366218, 1.018367, -159.480240],
[31.600000, -0.660411, 0.365100, 1.004314, -160.076126],
[31.700000, -0.650226, 0.364043, 0.992483, -161.283722],
[31.800000, -0.637634, 0.362683, 0.983479, -162.214386],
[31.900000, -0.616668, 0.369934, 0.978507, -162.077881],
[32.000000, -0.592629, 0.372811, 0.968966, -160.220413],
[32.100000, -0.566684, 0.376912, 0.959580, -158.874298],
[32.200000, -0.538083, 0.379907, 0.950346, -156.683731],
[32.300000, -0.508783, 0.385710, 0.936716, -155.459686],
[32.400000, -0.478993, 0.390328, 0.920076, -153.598389],
[32.500000, -0.451700, 0.394269, 0.904986, -151.731827],
[32.600000, -0.430844, 0.398511, 0.887833, -150.976746],
[32.700000, -0.417489, 0.397769, 0.862780, -148.362473],
[32.800000, -0.412943, 0.399168, 0.826656, -149.118500],
[32.900000, -0.417921, 0.399357, 0.777750, -149.047836],
[33.000000, -0.429065, 0.395261, 0.733175, -153.046478],
[33.100000, -0.431353, 0.397026, 0.711700, -163.547089],
[33.200000, -0.428411, 0.395765, 0.705513, -164.345154],
[33.300000, -0.427238, 0.392024, 0.667885, -161.053085],
[33.400000, -0.439706, 0.386841, 0.625672, -133.894974],
[33.500000, -0.452514, 0.363602, 0.618029, -120.610451],
[33.600000, -0.473528, 0.353555, 0.615272, -143.069855],
[33.700000, -0.472243, 0.356287, 0.607969, -160.453415],
[33.800000, -0.454168, 0.354941, 0.601848, -154.408691],
[33.900000, -0.419579, 0.347439, 0.597786, -144.856598],
[34.000000, -0.373342, 0.335019, 0.592088, -142.925858],
]
def goto(scf, x, y, z, yaw):
scf.cf.commander.send_position_setpoint(x, y, z, yaw)
print(time.time(), x, y, z, yaw)
cflib.crtp.init_drivers(enable_debug_driver=False)
with SyncCrazyflie("radio://0/30") as scf:
scf.cf.param.set_value("ring.effect", 14)
scf.cf.param.set_value("ring.fadeColor", 0)
scf.cf.param.set_value("ring.fadeTime", 1.0)
# Take off
steps = 10
start_x = 0.0
start_y = -1.5
for i in range(steps):
x = (i * trajectory[0][1] + (steps - i) * start_x) / steps
y = (i * trajectory[0][2] + (steps - i) * start_y) / steps
z = i * trajectory[0][3] / steps
yaw = i * trajectory[0][4] / steps
goto(scf, x, y, z, yaw)
time.sleep(0.2)
for i in range(5):
goto(scf, trajectory[0][1], trajectory[0][2], trajectory[0][3], trajectory[0][4])
time.sleep(0.2)
offset_time = trajectory[0][0]
start_time = time.time()
last_led_time = 0
for row in trajectory:
next_time = row[0] - offset_time + start_time
delay = next_time - time.time()
if delay > 0:
time.sleep(delay)
x = row[1]
y = row[2]
z = row[3]
yaw = row[4]
goto(scf, x, y, z, yaw)
now_int = int(time.time())
if last_led_time != now_int:
col = 0x000000ff * (now_int & 1)
scf.cf.param.set_value("ring.fadeColor", col)
last_led_time = now_int
# Land for now
# steps = 10
# for i in range(steps):
# x = trajectory[-1][1]
# y = trajectory[-1][2]
# z = (steps - i) * trajectory[-1][3] / steps
# yaw = trajectory[-1][4]
# goto(scf, x, y, z, yaw)
# time.sleep(0.2)
# time.sleep(0.3)
scf.cf.commander.send_stop_setpoint()
for i in range(10):
scf.cf.param.set_value("ring.fadeColor", 0x00ff0000)
time.sleep(2)
scf.cf.param.set_value("ring.fadeColor", 0x0000ff00)
time.sleep(2)