flying a crazyflie using uav_trajectories
flying a crazyflie using uav_trajectories
Hello
After going through all the steps to install tool uav : https://github.com/whoenig/uav_trajectories.
I managed to generate my bezier1 file, now I would like to know how I can fly my crazyflie with a csv file and autonomous_sequence_high_level
Thanks in advance
After going through all the steps to install tool uav : https://github.com/whoenig/uav_trajectories.
I managed to generate my bezier1 file, now I would like to know how I can fly my crazyflie with a csv file and autonomous_sequence_high_level
Thanks in advance
-
- Bitcraze
- Posts: 630
- Joined: Tue Jun 30, 2015 7:47 am
Re: flying a crazyflie using uav_trajectories
Hi!
The autonomous_sequence_high_level.py example is probably your best starting point, it uses polynomials though instead of bezier curves. It is possible to upload bezier curves to the high level commander as well but we don't have any example code for that.
I have not used the uav_trajectories tool my self and I'm not sure of the output format, but I think it is likely that it can output the polynomial format as well.
The autonomous_sequence_high_level.py example is probably your best starting point, it uses polynomials though instead of bezier curves. It is possible to upload bezier curves to the high level commander as well but we don't have any example code for that.
I have not used the uav_trajectories tool my self and I'm not sure of the output format, but I think it is likely that it can output the polynomial format as well.
Re: flying a crazyflie using uav_trajectories
Thank you for your reply
I generate the beziez file, i think is polynomial format, and after that i wrote a script to read each line of my csv file, i paste the result to autonomous_sequence_high_level.py.
but the problem i get this error when a execute the autonomous_sequence_high_level.py.
My script to display the Bezier file:
the autonomous_sequence_high_level.py
I generate the beziez file, i think is polynomial format, and after that i wrote a script to read each line of my csv file, i paste the result to autonomous_sequence_high_level.py.
but the problem i get this error when a execute the autonomous_sequence_high_level.py.
Code: Select all
Connecting to radio://0/80/2M
Connected to radio://0/80/2M
Uploading data
Traceback (most recent call last):
File "autonomous_sequence_high_level.py", line 222, in <module>
duration = upload_trajectory(cf, trajectory_id, figure8)
File "autonomous_sequence_high_level.py", line 194, in upload_trajectory
Uploader().upload(trajectory_mem)
File "autonomous_sequence_high_level.py", line 114, in upload
trajectory_mem.write_data(self._upload_done)
File "/home/ensea/.local/lib/python3.6/site-packages/cflib/crazyflie/mem.py", line 705, in write_data
data += struct.pack('<ffffffff', *poly4D.yaw.values)
struct.error: pack expected 8 items for packing (got 0)
Code: Select all
inp=open("bezier1.csv","r")
for line in inp:
print("["+line+", ],")
print("# noqa")
inp.close()
Code: Select all
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
# URI to the Crazyflie to connect to
uri = 'radio://0/80/2M'
# The trajectory to fly
# See https://github.com/whoenig/uav_trajectories for a tool to generate
# trajectories
# Duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7
figure8 = [
# noqa
[1.72041,0.0,0.45354900000000203,1.4156000000000037,0.0,0.45354900000000137,1.4156000000000024,0.0,0.4535490000000007,1.4156000000000009,0.0,0.4535490000000002,1.4155999999999997,0.0,0.2186845735500494,1.6468115571735393,0.0,0.14234955019294382,1.7003964313322577,0.0,0.08780304405130135,1.730746419595408,0.0,0.05076597769212654,1.7359074262104741
, ],
# noqa
[1.40258,0.0,0.05079959999999973,1.7359500000000054,0.0,0.020623892774285467,1.7401835875457183,0.0,0.0020958772321647,1.7276907495965643,0.0,-0.007098326149691678,1.6974211618160533,0.0,0.04642770923247397,1.5127286697738598,0.0,0.04948934440006808,1.4664989260894128,0.0,0.04241452445115912,1.4316076010282566,0.0,0.024564238709935182,1.4091893896135694
, ],
# noqa
[1.81632,0.0,0.02455950000000041,1.4091900000000037,0.0,0.0014388361920003034,1.3801540490057167,0.0,-0.03975636363345168,1.3720316749566839,0.0,-0.10041882501859267,1.3872881878854093,0.0,-0.3523352490943053,1.6542543194201456,0.0,-0.39077187757635445,1.7059186370631982,0.0,-0.4081921725479214,1.7341800661638844,0.0,-0.40637261108588163,1.7390780159761512
, ],
# noqa
[1.15497,0.0,-0.40635900000000136,1.7390800000000053,0.0,-0.4051942045052152,1.7421957295698607,0.0,-0.3962455869305774,1.7358649565612423,0.0,-0.37996746264531134,1.7200976705154962,0.0,-0.34064154852082773,1.6613532219634373,0.0,-0.33434655118642864,1.6188821078105882,0.0,-0.33052368696172085,1.5791613842281258,0.0,-0.3299255458842625,1.543258601366379
, ],
# noqa
[1.17625,0.0,-0.32992400000000066,1.5432700000000044,0.0,-0.32931309607857184,1.506712990178574,0.0,-0.3320454561229688,1.4741211431922632,0.0,-0.33891465527145126,1.4466255279641804,0.0,-0.45795162035176884,1.466009966289841,0.0,-0.5071180980427636,1.478700424632517,0.0,-0.5549201168094697,1.4918632077745593,0.0,-0.6007691497139778,1.506088752416706
, ],
# noqa
[0.824838,0.0,-0.6007490000000021,1.5060800000000047,0.0,-0.6328917585200013,1.5160547659340031,0.0,-0.6640703723867443,1.5265517841780623,0.0,-0.6940807001685951,1.537774586877349,0.0,-0.7195290866835702,1.5670777140548238,0.0,-0.7217980588922879,1.5783559673831655,0.0,-0.7209099103997235,1.5891923557998044,0.0,-0.7167672806294639,1.5996918685171442
, ],
# noqa
[1.03404,0.0,-0.7167670000000024,1.599700000000005,0.0,-0.7115733568360016,1.612862708776003,0.0,-0.7012647302085143,1.6254950853293135,0.0,-0.6856489729688651,1.6378019944611641,0.0,-0.610846890915947,1.6444070457736917,0.0,-0.5706149873208214,1.633814022410025,0.0,-0.5320136714026726,1.6205831586825339,0.0,-0.4944374993413341,1.604807263091334
, ],
# noqa
[1.07832,0.0,-0.4944340000000013,1.6048000000000051,0.0,-0.4552464647885722,1.588349150080003,0.0,-0.4171723742958976,1.5691310585602447,0.0,-0.3795243720723396,1.5472513330107367,0.0,-0.3323242069050696,1.4808651501395513,0.0,-0.3326950800864702,1.4572333530234098,0.0,-0.3380019602211653,1.435941546400985,0.0,-0.3482742267620078,1.4173385138778989
, ],
# noqa
[1.50767,0.0,-0.348285000000001,1.4173500000000039,0.0,-0.36265891039857207,1.3913506616128597,0.0,-0.3867514889371643,1.3706177206264327,0.0,-0.42065478721871163,1.3561137418432585,0.0,-0.5765503250446375,1.450722856275519,0.0,-0.6605810447249665,1.4692553559018622,0.0,-0.737790520340273,1.4868213220290727,0.0,-0.8103096605492475,1.5048018102810232
, ],
# noqa
[1.0071,0.0,-0.8103150000000029,1.5047800000000044,0.0,-0.8587596751714306,1.5167778556557174,0.0,-0.9051126327437319,1.5289539774124208,0.0,-0.9500094257317719,1.5417167095121977,0.0,-0.9945335900614083,1.5943253092111231,0.0,-0.9945992525500624,1.5992895439465735,0.0,-0.9899033319383517,1.6027387704008105,0.0,-0.9799862427537356,1.6046759722867474
, ],
# noqa
[1.14376,0.0,-0.9799830000000032,1.604670000000005,0.0,-0.9687166535508593,1.606865185889146,0.0,-0.9507125837215306,1.6071053050884287,0.0,-0.9252940272313683,1.6053899841868189,0.0,-0.8085728563040759,1.5231975705882281,0.0,-0.7952987129488094,1.492712777156091,0.0,-0.7898660710930852,1.4643457477903659,0.0,-0.7926668355927884,1.438658614644197
, ],
# noqa
[0.544044,0.0,-0.7926740000000024,1.4386500000000042,0.0,-0.7940076228292587,1.4264274297754314,0.0,-0.7972042245389489,1.4148100267700607,0.0,-0.8023059430017195,1.4038580270516772,0.0,-0.8245069528538593,1.398260766667467,0.0,-0.832662752269065,1.395405579984961,0.0,-0.8417816295981322,1.3939852244673818,0.0,-0.8518136897216835,1.3940122183030939
, ],
# noqa
[1.69781,0.0,-0.851812000000002,1.394010000000004,0.0,-0.883117190957144,1.3940937825577053,0.0,-0.9233140737679112,1.4082728658342223,0.0,-0.9708907125612308,1.43692829676543,0.0,-1.2987719936016069,1.5968187727359617,0.0,-1.3054573615188378,1.607415239657779,0.0,-1.2992953968142738,1.6122634363930195,0.0,-1.2827923542635773,1.6100167671973775
, ],
# noqa
[1.28011,0.0,-1.2827400000000042,1.6100100000000053,0.0,-1.2702722772185744,1.6083135963992747,0.0,-1.25191592085108,1.602581978344977,0.0,-1.228742102700686,1.592236249173642,0.0,-1.0614295529709485,1.4683795586406023,0.0,-1.0634850331374552,1.4575294016982703,0.0,-1.0687615371287555,1.448776337883575,0.0,-1.0780257228010761,1.4410645970763927
, ],
# noqa
[0.51015,0.0,-1.0780100000000032,1.4410500000000042,0.0,-1.0816977796078593,1.4379719516721456,0.0,-1.0860174473578446,1.4350577289040503,0.0,-1.0910170709847735,1.432240052419812,0.0,-1.1256314867823667,1.4082953762478219,0.0,-1.128693753815635,1.403544564188703,0.0,-1.1310194485284573,1.3983229395489005,0.0,-1.1325534395154824,1.3926285611744078
, ],
# noqa
[1.56718,0.0,-1.1325500000000028,1.3926300000000038,0.0,-1.1372391592780018,1.3751405846360027,0.0,-1.1343976034665855,1.3531971987485623,0.0,-1.1222808427185083,1.3267596214585937,0.0,-1.3488618352415238,1.4560629361511097,0.0,-1.4457878115858729,1.5245169839717276,0.0,-1.527878843582636,1.5845178832966123,0.0,-1.5588744179170142,1.6073529388653878
, ],
# noqa
[0.201968,0.0,-1.558850000000005,1.607340000000005,0.0,-1.562842561129146,1.6102812888365745,0.0,-1.5659863688048523,1.612605169819149,0.0,-1.5682038072577593,1.6142501748569646,0.0,-1.5699599859086637,1.615508397590891,0.0,-1.569960013439184,1.6155083932971006,0.0,-1.5699600332545045,1.6155083750936954,0.0,-1.5699600239692961,1.6155083387477018
, ],
# noqa
]
class Uploader:
def __init__(self):
self._is_done = False
def upload(self, trajectory_mem):
print('Uploading data')
trajectory_mem.write_data(self._upload_done)
while not self._is_done:
time.sleep(0.2)
def _upload_done(self, mem, addr):
print('Data uploaded')
self._is_done = True
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]
total_duration = 0
for row in trajectory:
duration = row[0]
x = Poly4D.Poly(row[1:9])
y = Poly4D.Poly(row[9:17])
z = Poly4D.Poly(row[17:25])
yaw = Poly4D.Poly(row[25:33])
trajectory_mem.poly4Ds.append(Poly4D(duration, x, y, z, yaw))
total_duration += duration
Uploader().upload(trajectory_mem)
cf.high_level_commander.define_trajectory(trajectory_id, 0,
len(trajectory_mem.poly4Ds))
return total_duration
def run_sequence(cf, trajectory_id, duration):
commander = cf.high_level_commander
commander.takeoff(1.0, 2.0)
time.sleep(3.0)
relative = True
commander.start_trajectory(trajectory_id, 1.0, relative)
time.sleep(duration)
commander.land(0.0, 2.0)
time.sleep(2)
commander.stop()
if __name__ == '__main__':
cflib.crtp.init_drivers(enable_debug_driver=False)
with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
cf = scf.cf
trajectory_id = 1
activate_high_level_commander(cf)
# activate_mellinger_controller(cf)
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)
-
- Bitcraze
- Posts: 630
- Joined: Tue Jun 30, 2015 7:47 am
Re: flying a crazyflie using uav_trajectories
Check the order of the columns in your csv file. I have a faint recollection of that the duration is the last column in the csv but the fist column in the python file.
Re: flying a crazyflie using uav_trajectories
I just checked my Csv file I found that the duration is in the first column like the python file.
The problem is that the Csv file contains 25 columns but on the other hand the python file needs 33 columns to fly, that's why he displayed this error:
struct.error: pack expected 8 items for packing (got 0)
The uav_trajectories tool generates points in my bezier file without the last 8 yaw values.
I think that's the problem
The problem is that the Csv file contains 25 columns but on the other hand the python file needs 33 columns to fly, that's why he displayed this error:
struct.error: pack expected 8 items for packing (got 0)
Code: Select all
# Duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7
I think that's the problem
-
- Bitcraze
- Posts: 630
- Joined: Tue Jun 30, 2015 7:47 am
Re: flying a crazyflie using uav_trajectories
Yes, that would be a problem. You can just set all the yaw data to 0
Re: flying a crazyflie using uav_trajectories
yes i did that before, I have the drone that flies but is not stable, and does not follow my trajector.
Re: flying a crazyflie using uav_trajectories
Hi Sorry for the late reply, but it seems that this is also something that we don't know exactly the answer to either. Have you found a solution in the mean time?