Bitcraze Forums

The Bitcraze web forum
https://forum.bitcraze.io/

flying a crazyflie using uav_trajectories

https://forum.bitcraze.io/viewtopic.php?t=4323

Page 1 of 1

flying a crazyflie using uav_trajectories

Posted: Wed Jun 10, 2020 1:58 pm
by 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

Re: flying a crazyflie using uav_trajectories

Posted: Thu Jun 11, 2020 6:44 am
by 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.

Re: flying a crazyflie using uav_trajectories

Posted: Thu Jun 11, 2020 7:16 am
by 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)

Re: flying a crazyflie using uav_trajectories

Posted: Fri Jun 12, 2020 6:56 am
by 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.

Re: flying a crazyflie using uav_trajectories

Posted: Fri Jun 12, 2020 7:26 am
by 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

Re: flying a crazyflie using uav_trajectories

Posted: Fri Jun 12, 2020 8:07 am
by kristoffer
Yes, that would be a problem. You can just set all the yaw data to 0

Re: flying a crazyflie using uav_trajectories

Posted: Fri Jun 12, 2020 11:57 am
by anomi
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

Posted: Wed Jun 24, 2020 9:02 am
by 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?
All times are UTC
Page 1 of 1

Powered by phpBB® Forum Software © phpBB Limited