Low Level Controls - What's best?

[willsharpless]

Hi,

First, thanks for writing this software, its great. I would like to use it as a platform for controls research. I understand it has been written for high-level control but was hoping to try actuating ~lower-level controls.

Ideally, I would like to control [roll, pitch, yaw_dot, thrust] or [roll_dot, pitch_dot, yaw_dot, thrust]. It seems (open to suggestion) that cmdVel / cmd_vel_legacy is the simplest way to do this, but I found that it is half-implemented. It looks like you all have thought about this but after uncommenting, modifying and executing, I found it doesn't work with backend:=sim, throwing not-yet-implemented-in-SIL errors.

The crazyflie_SIL code is a bit complex to me: I am confused about the order of operations e.g. for self.setState, self.controller and self.control_t. If there were a little explanation somewhere that would be much appreciated. I am a bit too busy to parse all this out but maybe implementing an SIL cmdVel is simple?

Above all, I mostly would just like to get these controls working in sim and on hardware, however possible. What do you all think is the best route for that?

Here is a python MWE:

#!/usr/bin/env python

from pathlib import Path

from crazyflie_py import Crazyswarm
from crazyflie_py.uav_trajectory import Trajectory
import numpy as np

def main():

    swarm = Crazyswarm()
    timeHelper = swarm.timeHelper
    allcfs = swarm.allcfs

    ## HL
    allcfs.takeoff(targetHeight=1.0, duration=5.0)
    timeHelper.sleep(4.0)
    for cf in allcfs.crazyflies:
        cf.goTo([0.0, 0.0, 1.0], 0.0, 5.)
    timeHelper.sleep(5.) 

    ## LL
    for j  in range(1256):
        for cf in allcfs.crazyflies:

            ## With cmdFullState (works)
            # zpos = 1. + 0.5 * np.sin(j / 100)
            # cf.cmdFullState([0., 0., zpos], [0., 0., 0.], [0., 0., 0.], 0., [0., 0., 0.])

            ## With cmdVel (doesnt)
            thrust = 7.5 + 5. * np.sin(j / 100)
            cf.cmdVel(0., 0., 0., thrust)

        timeHelper.sleep(0.01)

    ## Transfer back to HL
    cf.notifySetpointsStop(10)
    for cf in allcfs.crazyflies:
        cf.goTo([0.0, 0.0, 1.0], 0.0, 5.)
    timeHelper.sleep(5) 

    allcfs.land(targetHeight=0.06, duration=5.0)
    timeHelper.sleep(3.0)

if __name__ == '__main__':
    main()

which primarily would (?) rely on uncommenting / defining these methods:

in crazyflie.py,

def cmdVel(self, roll, pitch, yawrate, thrust):
        msg = Twist()
        msg.linear.x = pitch
        msg.linear.y = roll
        msg.angular.z = yawrate
        msg.linear.z = thrust
        self.cmdVelPublisher.publish(msg)

in crazyflie_server.py,

def _cmd_vel_legacy_changed(self, msg, uri=""):
    roll = msg.linear.y
    pitch = -msg.linear.x
    yawrate = msg.angular.z
    # thrust = int(min(max(msg.linear.z, 0, 0), 60000))
    thrust = int(min(max(msg.linear.z * 40000/9.81, 0, 0), 60000)) # assumes thrust in ~[2.5, 14.5]
    self.swarm._cfs[uri].cf.commander.send_setpoint(
        roll, pitch, yawrate, thrust)