OptiTrack and Crazyflie Python no ROS

Discussions related to positioning systems where the position is calculated outside the Crazyflie and sent to the device using radio, including MoCap systems
Post Reply
scott
Beginner
Posts: 2
Joined: Tue Feb 15, 2022 10:31 pm

OptiTrack and Crazyflie Python no ROS

Post by scott »

Hi all,

I'm a student and I've been tasked with integrating OptiTrack and Crazyflie 2.1 quadcopters for future classes, pretty much entirely using Python. We have Motive and the NatNet SDK set up to receive the position (xyz) and the quaternion (qx, qy, qz, qw). A special python script takes these values and transmits them via UDP to a specific ip address, where a client python file unpacks the data via socket and places it into a queue:

Code: Select all

def optitrack(queue: Queue, run_process: Value):
    logging.info('Beginning socket listener')
    with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as s:
        s.bind(('0.0.0.0', int(CLIENT_PORT)))
        logging.info(f'Starting socket listener')
        while run_process.value == 1:
            data = s.recvfrom(1024)[0]
            if not data:
                logging.info(f'No data received')
                break
            else:
                [a, b, c, d, e, f, g, h, i, j, k, l] = struct.unpack('ffffffffffff', data)
                x = -a
                y = c
                z = b
                qx = d
                qy = e
                qz = f
                qw = g
                roll = -h
                yaw = i
                pitch = -j
                bodyID = k
                framecount = l
                print(f'x = {x}, y = {y}, z = {z} \n qx = {qx}, qy = {qy}, qz = {qz}, qw = {qw} \n roll = {roll}, yaw = {yaw}, pitch = {pitch} \n bodyID = {bodyID}, framecount = {framecount}')
                if queue.empty():
                    queue.put((x, y, z, qx, qy, qz, qw))
This part works perfectly, I am able to receive the full pose and run this function in its own process to avoid any interference with the radio communication. I then have to send this information to the quadcopter:

Code: Select all

def send_pose(client, queue: Queue):
    logging.info('sending full pose')
    while client.is_connected:
        x, y, z, qx, qy, qz, qw = queue.get()
        client.cf.extpos.send_extpose(x, y, z, qx, qy, qz, qw)
        # time.sleep(5)
I run this function in its own thread to receive the queues and push the information to the quadcopter constantly. There is a mocap deck attached with 4 markers placed in a unique configuration. No other decks are attached. Running a simple hover test, the quadcopter crashes pretty much instantly. Some other things to note: I am using the PID controller via ('stabilizer.controller', 1) and the EKF via ('stabilizer.estimator', 2). My question is, in theory, is this the right way to approach it? My worry is that currently there is custom firmware installed for the class. Should I upload either the default firmware or even the crazyswarm firmware? I am for now only using one quadcopter, although it is possible more will be added in the future.

Thanks for the help!
kimberly
Bitcraze
Posts: 1050
Joined: Fri Jul 06, 2018 11:13 am

Re: OptiTrack and Crazyflie Python no ROS

Post by kimberly »

Hi!

Cool that you managed to get Vicon to work! But before you move further with that, the creators of crazyswarm have made a library for motioncapture systems already: https://github.com/IMRCLab/libmotioncapture which is also used in crazyswarm as well.

Probably what goes wrong in your case is that the coordinate system does not match the crazyflie's coordinate system. But I do advise to use crazyswarms' frameworks as they already encountered the issues you are facing and fixed that some how, to make it easier for yourself.

About your custom firmware, as long as it has branched from not too long ago, then it should work fine for both crazyswarm and the crazyflie python library. So from about 3-6 months ago back should be fine. If you have any question about this, please start a new thread in the forum as it is a different topic.
scott
Beginner
Posts: 2
Joined: Tue Feb 15, 2022 10:31 pm

Re: OptiTrack and Crazyflie Python no ROS

Post by scott »

Hi Kimberly, thank you for the response and the references.

I have successfully incorporated send_extpos(x, y, z) to allow for highly precise positional flight of the quadcopter, a great first step. I now want to send the full pose to avoid yaw drift. My thinking was to use send_extpose(x, y, z, qx, qy, qz, qw). I have highly reliable and accurate euler angles from the OptiTrack that align with the crazyflie coordinate system. I then attempt to convert these to quaternions to send for full pose:

Code: Select all

def get_quaternion_from_euler(roll, yaw, pitch):
    """
    Convert an Euler angle to a quaternion.

    Input
      :param roll: The roll (rotation around x-axis) angle in degrees.
      :param pitch: The pitch (rotation around y-axis) angle in degrees.
      :param yaw: The yaw (rotation around z-axis) angle in degrees.

    Output
      :return qx, qy, qz, qw: The orientation in quaternion [x,y,z,w] format
    """
    roll = np.deg2rad(roll)
    yaw = np.deg2rad(yaw)
    pitch = np.deg2rad(pitch)
    qx = np.sin(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) - np.cos(roll / 2) * np.sin(pitch / 2) * np.sin(yaw / 2)
    qy = np.cos(roll / 2) * np.sin(pitch / 2) * np.cos(yaw / 2) + np.sin(roll / 2) * np.cos(pitch / 2) * np.sin(yaw / 2)
    qz = np.cos(roll / 2) * np.cos(pitch / 2) * np.sin(yaw / 2) - np.sin(roll / 2) * np.sin(pitch / 2) * np.cos(yaw / 2)
    qw = np.cos(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) + np.sin(roll / 2) * np.sin(pitch / 2) * np.sin(yaw / 2)

    return [qx, qy, qz, qw]
The quadcopter then crashes when I input these quaternions into send_extpose().

Is there something wrong with the way I have converted my euler angles to quaternions? I've struggled to find a very successful implementation of send_extpose(), is it no longer the method of choice? Or is there a better way to send full pose (primarily yaw) to the quadcopter? I would prefer not to have to incorporate Crazyswarm or any other libraries/projects. I have all the default firmware plus a small amount of customization (introduction of a LQR activated via 'stabilizer.controller, 4').

If I can reliably convert my euler angles to quaternions that the quadcopter understands within its own reference frame, then my project will have been a success, so any help would be greatly appreciated.
kristoffer
Bitcraze
Posts: 630
Joined: Tue Jun 30, 2015 7:47 am

Re: OptiTrack and Crazyflie Python no ROS

Post by kristoffer »

You can use scipy to convert between various rotation formats https://docs.scipy.org/doc/scipy/refere ... ation.html
Post Reply