flying a crazyflie using uav_trajectories

[anomi]

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

[kristoffer]

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.

[anomi]

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.

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)

My script to display the Bezier file:

Code: Select all

inp=open("bezier1.csv","r")
for line in inp:
	print("["+line+", ],")
	print("# noqa")
inp.close()

the autonomous_sequence_high_level.py

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)

[kristoffer]

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.

[anomi]

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)

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

The uav_trajectories tool generates points in my bezier file without the last 8 yaw values.

I think that's the problem

[kristoffer]

Yes, that would be a problem. You can just set all the yaw data to 0

[anomi]

yes i did that before, I have the drone that flies but is not stable, and does not follow my trajector.

[kimberly]

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?