Category: Software

The Crazyflie client has a quite long history, like a lot of things in the Crazyflie ecosystem it has been started when the Crazyflie was used alone using mainly manual control. It has evolved to follow new use-cases of the Crazyflie but it still has traces of its origins and some limitation are still there with us. Moreover, the Crazyflie client and lib are written in python, one the main goal was to make it easily cross-platform. Unfortunately making a cross platform graphical program that accesses hardware in Python has proven to be quite challenging and we feel Python is not the way to go anymore. In this blog post we would like to discuss a bit the current state of the Client and what we are looking at for the future.

Photo of a Crazyflie quadcopter in front of a laptop running the Crazyflie client
A Crazyflie connected to the CFclient

The Crazyflie client was originally design to be able to fly, inspect and work with one Crazyflie. It still serves this purpose quite well: with the client you can connect your Crazyflie, observe graphs of internal log values in real time, setup different decks and sets and store parameters. It is a very good tool to explore and work with the Crazyflie.

However, it is only working with one Crazyflie at a time will take over the radio, so another script cannot talk to the Crazyflie at the same time using the radio. Unless the script uses the ZMQ API that allows an external program to control client and so to control the Crazyflie while the client is connected and active. This functionality can be very useful but, since it is disabled by default, has not seen a lot of use.

The worst, for us, in the current client state is Python and PyQT distribution situation: we used to have an easy-to-use installer on windows, a Linux Snap package and plan for a Mac App. All these have been pretty much abandoned because they kept breaking down over time and the support weight for us was too big. So the only way to install the client is via Pip, the python package manager. This means that you already need a well setup Python environment to run the client. That is nice unless you have a Mac or a Raspberry pi: the latest MacOS broke part of the client that prevents it to run and there is no PyQT build for raspberry-pi available on PyPi. This is the kind of paper-cut that keeps happening at regular interval with distributing a Python application and that we keep having to look at.

So, we have been looking at ways to improve the situation. The Crazyflie client is more than 10 years old now, so a rewrite would not be such a crazy thing to consider. There are at least 2 angle of attack to make a new client better suited for the next 10 years of Crazyflie development:

Multi-platform and distribution

Making a multi-platform program is and will always be challenging. However, we have discovered that doing it in a dynamic interpreted language like python is even more so. The main challenge come from the fact that things tend to break on the user side depending of the user configuration: we all run a slightly different version of python, python evolve and package management evolves as well, so when things break it breaks at random depending of how up-to-date a particular system.

One solution would then be to switch to a compiled programming language. This increases the probability of finding problem at compile time and not at runtime, which means that we will hopefully be the first to know then Apple decided to change the location of one fundamental piece of library and so allow us to hopefully fix the problem before any user is impacted (assuming we can run CI on the latest version of MacOS early enough…).

So, as you might have guessed, our current idea is to write the client in Rust. We are currently looking at Tauri for the UI which is Web based. We still have ideas of also making a web-client so having a web-based IU on PC would simplify development of that.

We are not letting Python go away though, the idea is still to support Python, but to use it for what it is good at: a great language for developer and experimentation. Rust has great bindings to python so in this plan, the python lib is backed by the Rust lib.

Modularity

The other side of the current client limitation is the fact that it connects one Crazyflie taking over the radio. We actually love using the client to observe and poke the state of a Crazyflie so it would be really great to use it when writing a script or controlling a swarm with Crazyswarm. The current ZMQ implementation was designed to solve this issue, but it goes at it the wrong way around: the client becomes the gateway to the Crazyflie and must always be ON. It would be much nicer to be able to launch the client to connect and inspect a Crazyflie currently control by a script.

One design we are currently looking at would be to use use a protocol like Zenoh between the client and the lib. Basically, when connecting a Crazyflie, be it from a script or from the client, a server would be launched in the background that would handle the connection. All communication would pass through this server and so multiple programs would be able to communicate simultaneously with the Crazyflie.

This would allow us to build easily bridges to ROS to get the client to communicate with a Crazyflie currently connected in ROS. And since ROS2 is working on supporting Zenoh officially a bridge might not even be required.

As an added bonus, the Crazyflie server idea would greatly improve the situation when it comes to supporting Virtual Machines and WSL on Windows: it would move the USB connection handling to a Windows program and only require fast network connections which is something that works really well on WSL or VMs.

Conclusion

In this blog post I have tried to describe our current challenges and some way forward we see. The main message though is that we want to change things when it comes to the client, if you have wishes or ideas now is a good time to get in touch. Let’s make the next 10 years of Crazyflie client problem-free.

Today we have a guest blogpost by Simon D. Levy (Washington and Lee University) about using Haskell to warp parts of the Crazylife-firmware. We also have some announcements from Bitcraze itself at the bottom. Enjoy!

As Richard Feynman famously said, What I cannot create, I do not understand. My less ambitious version of this motto is What I can translate into another language, I might better understand.

In order to better understand the Crazyflie firmware, I first undertook to translate the C-based firmware into C++. By separating out the general-purpose algorithmic code (state estimator, closed-loop control) from the Crazyflie-specific / Hardware Abstraction Layer (HAL) component of the firmware, I ended up with a nice, header-only C++ library of algorithms that can work with both a simulator like Webots and on a Crazyflie 2.1 or Crazyflie Bolt flight controller (with the remaining Crazyflie firmware translated into the standard C++ .h/cpp format).

As a fan of functional languages like OCAML and Haskell (and Rust, which was strongly influenced by their approach), I wondered whether I couldn’t push this idea further, to get an even higher-level implementation of the algorithms. Having played a bit with Rust and encountered issues getting it to work efficiently with C++ (something that is thankfully now being addressed), I thought it might be work trying NASA Copilot, a Haskell extension that compiles Haskell into C with a fixed memory footprint (i.e., no malloc() or garbage-collection).

My efforts paid off, resulting in a Haskell library of algorithms for closed-loop (PID) control and motor mixing that works with both my modified version of the Crazyflie firmware and with a simulator like Webots. For anyone wondering “why Haskell”, I refer you to this excellent discussion on the language’s advantages (purity, elegance), as well as this classic presentation on why the functional programming offers a better solution to complex tasks than object-oriented approach. For example, the code in the LambdaFlight core module cleanly reflects the dataflow (input/output) diagram for a standard flight controller:

Here is the Haskell code corresponding the component labeled Core in the diagram:

— Clock rate is 500Hz for Crazyflie, 100Hz for sim
dt = rateToPeriod clock_rate

— Make a list of PID controllers based on application order
pids = [positionPid resetPids inHoverMode dt,
      pitchRollAnglePid resetPids inHoverMode dt,
      pitchRollRatePid resetPids inHoverMode dt,
      altitudePid inHoverMode dt,
      climbRatePid inHoverMode dt,
      yawAnglePid dt,
      yawRatePid dt]

— Run PID controllers on open-loop (stick) demands to get demands for motor mixer
demands’ = foldl (\demand pid -> pid vehicleState demand) openLoopDemands pids

— Adjust thrust component for hover mode
thrust” = if inHoverMode then ((thrust demands’) * tscale + tbase) else tmin

— Run motor mixer on closed-loop demands to get motor spins, scaled for CF or sim
motors = quadCFMixer $ Demands thrust”
                   ((roll demands’) * prscale)
                   ((pitch demands’) * prscale)
                   ((yaw demands’) * yscale)

By adjusting the values of the clock rate and scaling coefficients (tscale, tbase, prscale, …), the same PID controllers (with same Kp, Ki, Kd constants) can be used for both the simulation and the actual Crazyflie.

Two pieces complete the picture.

First, how can a pure functional language like Haskell, lacking mutation/side-effects, support an algorithm like PID control, which requires keeping state variables (error integral, previous error) across iterations? The answer to this question comes from an ingenious part of the NASA Copilot framework, namely, streams. A stream (which is similar to a lazy list in Haskell), can come from an module of the program written in C (for example, the vehicle state obtained by state estimation) or from a previous value passed into the function. This feature allows Copilot to have functions that possess state while still being “pure” in the sense of being amenable to formal verification (mathematical proof of correctness; see this thread for a discussion). Although I don’t have the mathematical background to do formal verification proofs, the ability to prove the code correct is an extremely powerful feature of languages like Haskell and is what led NASA to develop the Copilot framework for its space missions.

Second, how can Haskell be combined with C/C++ without running into the performance issues typically encountered in calling code in one language from code in another? As I alluded to before, NASA Copilot solves this issue nicely, by compiling the Haskell code to C code with a fixed memory footprint. Declaring certain C/C++ variables to be global makes them accessible to the Haskell code as streams, and causes them to be compiled into the resulting C code, which can then be compiled and linked to the Crazyflie (or simulator) C/C++ code in the usual way (thanks to the Crazyflie Makefile’s use of the Kbuild system). Hence, to compile the whole project into a form suitable for flashing with make cload, requires just the following chain of commands (where make links creates links to some external libraries I wrote for the sensors):

make cf2_defconfig && make links && make copilot && make -j 32

My current work focuses on two directions: first, I am translating Crazyflie’s Kalman filter into Haskell — a more ambitious undertaking, but one that I feel more confident in undertaking now that the core control algorithms are completed. Second, I am looking for ways of modifying the most popular robotics simulators (Webots, Gazbeo) to work with dynamics (physics engine) code custom-written for quadcopters and other aerial vehicles (like the dynamics code I wrote for this simulator), as well as faster control loops.

Announcements

There are some announcements that are not part of this guest blogpost that we’d like to share.

  • The 350 mAh batteries are out of stock and unfortunately have a very long lead time at our manufacturer, so you can expect them to be back in stock early May. Please take a look this blogpost which also compares this battery to the Tattu alternative, if you can’t wait and need to have a similar battery now.
  • We have a Bitcraze developer meeting this Wednesday to talk about the supervisor safety functionalities in the crazyflie-firmware. Please keep an eye on the GH discussion thread for information on how to join.

There is a new release of the firmware, version 2024.2. The main change, and almost only change, in this release, is the Bluetooth stack that was updated from the Nordic’s semiconductor S110 to S130, which affects the firmware on the NRF51 on the Crazyflie. This was mainly done to be able to pass the listing requirement on Bluetooth SIG, but it will also have beneficial technical effects on the Crazyflie radio communication state of affairs.

The new stack and bootloader are distributed in the normal release .zip, which means that it can be updated from the client as normal. Please note that the latest lib and clients are required as we have had to implement new procedures to flash the bootloader and stack.

First of all, let’s define what is a soft device. Nordic semiconductor radio chips are awesome in the way that the radio hardware is fully documented, this means that we can implement our own radio protocols but we would also be able to implement our own Bluetooth stack (I have attempted that a long time ago with some success, the nRF5 radio hardware is really powerful and can be set up to do much of the work!). However, backing your own Bluetooth stack would require passing a full suite of validation at the Bluetooth SIG to prove the stack conform to specification. In order to avoid that, the usual strategy is to buy a Bluetooth device and to talk to it over UART. What Nordic did is to implement a “Softdevice”, a binary blob that runs in the same CPU as the user application and that talks to the application using software interrupt. This keeps the application completely separated from the Bluetooth stack and so means that we get the benefits of a pre-qualified Bluetooth stack that has already been tested and approved.

Currently, we have ported the bootloader and the nrf firmware to the new S130 stack. This opens a lot of potential benefits for the future:

  • The new stack should be at least 6 times faster than the old one. This makes implementing BLE communication for the Crazyflie to a PC/Chromebook much more appealing and should allow to make the existing mobile client more full features.
  • We are now able to update the bootloader, so we can make a new improved version of it in the future (ie. with safe link, swarm optimization, much faster Bluetooth boot loading….)
  • The new stack supports device and host mode. So things like pairing a gamepad with the Crazyflie becomes a technical possibility! (actual implementation is left as an exercise for the contributor ;-).

One very important thing to note is that working on the bootloader requires a debugger: if you flash a bugged radio bootloader you need a SWD debug probe to be able to fix your Crazyflie. The bootloader+softdevice flashing procedure is very safe, as long as you flash a working firmware.

This change should open quite new exciting possibilities. It will be interesting to see what we can achieve with BLE and updated bootloaders in the future. Please note that we had to make a change to both the Crazyflie python library and the CFclient in order to flash this new firmware so make sure that you update those as well to try out this new release.

Developer meeting

The next developer meeting will be on the 6th of March 2024, we will talk about the Crazyflie Supervisor subsystem… We have made some changes to the supervisor recently, and we will continue working more on it in the next couple of weeks mostly preparing for the arrival of the Brushless Crazyflie. We will talk about the current state and what we are working on. Follow the thread on Bitcraze Discussions to be up to date on how to join!

A while ago, we wrote a generic blog post about state estimation in the Crazyflie, mostly discussing different ways the Crazyflie can determine its attitude and/or position. At that time, we only had the Complementary filter and Extended Kalman filter (EKF). Over the years, we’ve made some great additions like the M-estimation-based robust Kalman filter (an enhancement of the EKF, see this blog post) and the Unscented Kalman filter.

However, we have noticed that some of our beginning users struggle with understanding the concept of Kalman filtering, depending on whether this has been covered in their curriculum. And for some more experienced users, it might be nice to have a recap of the basics as well, since this is a very important part of the Crazyflie’s capabilities of flight (and also for robotics in general). So, in this blog post, we will explain the principles of Kalman filtering and how it is applied within the Crazyflie firmware, which hopefully will provide a good base for anyone starting to delve into state estimation within the Crazyflie.

We will also have a developer meeting about Kalman filtering on the Crazyflie, so we hope you can join that as well if you have any questions about how it all works. Also we are planning to got to FOSdem this weekend so we hope to see you there too.

Main Principles of the Kalman Filter

Anybody remotely working with autonomous systems must, at one point, have heard of the Kalman filter, as it has existed since the 60s and even played a role in the Apollo program. Understanding its main principles is also important for anyone working with drones or robotics. There are plenty of resources available, and its Wikipedia page is filled with examples, so here we will focus mostly on the concept and principles and leave the bulk of the mathematics as an exercise for those who like to delve into that :).

So basically, there are several principles that apply to a Kalman filter:

  • It estimates a linear system that is driven by stochastic processes. The probability function that drives these stochastic processes should ideally be Gaussian.
  • It makes use of the Bayes’ rule, which is a general term in statistics that describes the probability of an event happening based on previous knowledge related to that event.
  • It assumes that the ‘to be estimated state’ can be described with a Markov model, which assumes that a sequence of the next possible event (or scenario) can be predicted by the current event. In other words, it does not need a full history of events to predict the next step(s), only the information from the event of one previous step.
  • A Kalman filter is described as a recursive filter, which means that it reuses (part of) its output as input for the next filtering step.

So the state estimate is usually a vector of different variables that the developer or user of the system likes to observe, for either control or prediction, something like position and velocity, for instance: [x, y, , ẏ, …]. One can describe a dynamics model that can predict the state in the next step using only the current time step’s state, like for instance: xt+1 = xt + t, yt+1 = yt + ẏt. This can also be nicely described in matrix form as well if you like linear algebra. To this model, you can also add predicted noise to make it more realistic, or the effect of the input commands to the system (like voltage to motors). We will not go into the latter in this blogpost.

The Concept of Kalman filters

Simplified block scheme of Kalman filtering

So, we will go through the process of explaining the steps of the Kalman filter now, which hopefully will be clear with the above picture. As mentioned before, we’d like to avoid formulas and are oversimplifying some parts to make it as clear as possible (hopefully…).

First, there is the predict phase, where the current state (estimate) and a dynamics model (also known as the state transition model) result in a predicted state. Also in the same phase, the predicted estimated covariance is calculated, which also uses the dynamics model plus an indication of the process noise model, indicating how much the dynamics model deviates from reality in predicting that state. In an ideal world and with an ideal model, this could be enough; however, no dynamics model is perfect, which is why the next phase is also very important.

Then it’s the update phase, where the filter estimate gets updated by a measurement of the real world through sensors. The measurement needs to go through a measurement model, which transforms the measurement into a measured state (also known as innovation or measurement pre-fit residual). Usually, a measurement is not a 1-1 depiction of one variable of the state, so the measurement model ensures that the measurement can properly be compared to the predicted state. This same measurement model, accompanied by the measurement noise model (which indicates how much the measurement differs from the real world), together with the predicted covariance, is used to calculate the innovation and Kalman gain.

The last part of the update phase is where the predictions are updated with the innovation. The Kalman gain is then used to update the predicted state to a new estimated state with the measured state. The same Kalman gain is also used to update the covariance, which can be used for the next time step.

An 1D example, height estimation

It’s always good to show the filter in some form of example, so let’s show you a simple one in terms of height estimation to demonstrate its implications.

1D example of height estimation

You see here a Crazyflie flying, and currently it has its height estimated at zt and its velocity at żt. It goes to the predict phase and predicts the next height to be at zt+1,predict, which is a simple model of just zt + żt. Then for the innovation and updating phase, a measurement (from a range sensor) rz is used for the filter, which is translated to zt+1, meas. In this case, the measurement model is very simple when flying over a flat surface, as it probably is only a translation addition of the sensor to the middle of the Crazyflie, or perhaps a compensation for a roll or pitch rotation.

In the background, the covariances are updated and the Kalman gain is calculated, and based on zt+1,predict and zt+1, meas, the next state zt+1 is calculated. As you probably noticed, there was a discrepancy between the predicted height and measured height, which could be due to the fact that the dynamics model couldn’t correctly predict the height. Perhaps a PID gain was higher than expected or the Crazyflie had upgraded motors that made it climb faster on takeoff. As you can see here, the filter put the estimated height closer to zt+1 to the measurement than the predicted height. The measurement noise model incorporated into the covariances indicates that the height sensor is more accurate than the height coming from the dynamics model. This would very well be the case for an infrared height sensor like the one on the Flow Deck; however, if it were an ultrasound-based sensor or barometer instead (which are much noisier), then the predicted height would be closer to the one predicted by the dynamics model.

Also, it’s good to note that the dynamics model does not currently include the motor input, but it could have done so as well. In that case, it would have been better able to predict the jump it missed now.

A 2D example, horizontal position

A 2D example in x and y position

Let’s take it up a notch and add an extra dimension. You see here now that there is a 2D solution of the Crazyflie moving horizontally. It is at position xt, yt and has a velocity of t, ẏt at that moment in time. The dynamics model estimates the Crazyflie to end up in the general direction of the velocity factor, so it is a simple addition of the current position and velocity vector. If the Crazyflie has a flow sensor (like on the Flow Deck), flow fx, fy can be detected and translated by the measurement model to a measured velocity (part of the state filter) by combining it with a height measurement and camera characteristics.

However, the measurement in the form of the measured flow fx, fy estimates that there is much more flow detected in the x-direction than in the y-direction. This can be due to a sudden wind gust in the y-direction, which the dynamics model couldn’t accurately predict, or the fact that there weren’t as many features on the surface in the y-direction, making it more difficult for the flow sensor to measure the flow in that direction. Since this is not something that both models can account for, the filter will, based on the Kalman gain and covariances, put the estimate somewhere in between. However, this is of course dependent on the estimated covariances of both the outcome of the measurement and dynamic models.

In case of non-linearity

It would be much simpler if the world’s processes could be described with linear systems and have Gaussian distributions. However, the world is complex, so that is rarely the case. We can make parts of the world more abstract in simulation, and Kalman filters can handle that, but when dealing with real flying vehicles, such as the Crazyflie, which is considered a highly nonlinear system, it needs to be described by a nonlinear dynamics model. Additionally, the measurements of sensors in more complex and 3D situations usually don’t have a one-to-one linear relationship with the variables in the state. Can you still use the Kalman filter then, considering the earlier mentioned principles?

Luckily certain assumptions can be made that can still make Kalman filters useful in the sense of non-linearity.

  • Extended Kalman Filter (EKF): If there is non-linearity in either the dynamics model, measurement models or both, at each prediction and update step, these models are linearized around the current state variables by calculating the Jacobian, which is a collection of first-order partial derivative calculations of the model and the state variables.
  • Unscented Kalman Filter (UKF): An unscented Kalman filter deals with linearities by selecting sigma points selected around the mean of the state estimate, which are backpropagated through the non-linear dynamics model.

However, there is also the case of non-Gaussian processes in both dynamics and measurements, and in that case a complementary filter or particle filter would be best suited. The Crazyflie contains a complementary filter (which does not estimate x and y), an extended Kalman filter and an experimental unscented Kalman filter. Check out the state-estimation documentation for more information.

So…. where is the code?

This is all fine and dandy, however… where can you find all of this in the code of the Crazyflie firmware? Here is an overview of where you can find it exactly in the sense of the most used filter of them all, namely the Extended Kalman Filter.

There are several assumptions made and adjustments made to the regular EKF implementation to make it suitable for flight on the Crazyflie. For those details I’d like to refer to the papers on where this implementation is based on, which can be found in the EKF documentation. Also for a more precise explanation of Kalman filter, please check out the lecture slides of Stanford University on Linear dynamical systems or the Linköping university’s course slides on Sensor Fusion.

Update: From the comments we also got notified of an nice EKF tutorial where you write the filter from scratch (github) from Prof. Simon D. Levy from Washington and Lee university. Practice makes perfect!

Next Developer meeting and FOSdem

As you would have guessed, our next developer meeting will be about the Kalman filters in the Crazyflie. Keep an eye on this Discussion thread for more details on the meeting.

Also Kimberly and Arnaud will be attending FOSdem this weekend in Brussels, Belgium. We are hoping to organize an open-source robotics BOF/meetup there, so please let us know if you are planning to go as well!

A few years ago, we wrote a blogpost about the Commander framework, where we explained how the setpoint structure worked, which drives the controller of the Crazyflie, which is an essential part of the stabilization module. Basically, without these, there would not be any autonomy on the Crazyflie, let alone manual flight.

In the blogpost, we already shed some light on where different setpoints can come from in the commander framework, either from the Crazyflie python library (externally with the Crazyradio), the high level commander (onboard) or the App layer (onboard).

General framework of the stabilization structure of the crazyflie with setpoint handling. * This part is takes place on the computer through the CFlib for python, so there is also communication protocol in between. It is left out of this schematics for easier understanding.

However, we notice that there is sometimes confusion regarding these different functionalities and what exactly sends which setpoints and how. These details might not be crucial when using just one Crazyflie, but become more significant when managing multiple drones. Understanding how often your computer needs to send setpoints or not becomes crucial in such scenarios. Therefore, this blog post aims to provide a clearer explanation of this aspect.

Sending set-points directly from the CFlib

Let’s start at the lower level from the computer. It is possible to send various types of setpoints directly from a Python script using the Crazyflie Python library (cflib for short). This capability extends to tasks such as manual control:

send_setpoint(roll, pitch, yawrate, thrust)

or for hover control (velocity control):

send_hover_setpoint(vx, vy, yawrate, zdistance)

You can check the automatic generated API documentation for more setpoint sending options.

If you use these functions in a script, the principle is quite basic: the Crazyradio sends exactly 1 packet with this setpoint over the air to the Crazyflie, and it will act upon that. There are no secret threads opening in the background, and nothing magical happens on the Crazyflie either. However, the challenge here is that if your script doesn’t send an updated setpoint within a certain amount of time (default of 2 seconds), a timeout will occur, and the Crazyflie will drop out of the sky. Therefore, you need to send a setpoint at regular intervals, like in a for loop, to keep the Crazyflie flying. This is something you need to take care of in the script.

Example scripts in the CFlib that are sending setpoints directly:

Setpoint handling through Motion Commander Class

Another way to handle the regular sending of setpoints automatically in the CFLib is through the Motion Commander class. By initializing a Motion Commander object (usually using a context manager), a thread is started with takeoff that will continuously send (velocity) setpoints at a fixed rate. These setpoints can then be updated by the following functions, for instance, moving forward with blocking:

forward(distance)

or a giving body fixed velocity setpoint updates (that returns immediately):

start_linear_motion(vx, vy, vz, rate_yaw)

You can check the Motion Commander’s API-generated documentation for more functions that can be utilized. As there is a background thread consistently sending setpoints to the Crazyflie, no timeout will occur, and you only need to use one of these functions for the ‘behavior update’. This thread will be closed as soon as the Crazyflie lands again.

Here are example scripts in the CFlib that use the motion commander class:

Setpoint handling through the high level commander

Prior to this, all logical and setpoint handling occurred on the PC side. Whether sending setpoints directly or using the Motion Commander class, there was a continuous stream of setpoint packets sent through the air for every movement the Crazyflie made. However, what if the Crazyflie misses one of these packets? Or how does this stream handle communication with many Crazyflies, especially in swarms where bandwidth becomes a critical factor?

This challenge led the developers at the Crazyswarm project (now Crazyswarm2) to implement more planning autonomy directly on the Crazyflie itself, in the form of the high-level commander. With the High-Level Commander, you can simply send one higher-level command to the Crazyflie, and the intermediate substeps (setpoints) are generated on the Crazyflie itself. This can be achieved with a regular takeoff:

take_off(height)

or go to a certain position in space:

go_to(x, y)

This can be accomplished using either the PositionHLCommander, which can be used as a context manager similar to the Motion Commander (without the Python threading), or by directly employing the functions of the High-Level Commander. You can refer to the automated API documentation for the available functions of the PositionHLCommander class or the High-Level Commander class.

Here are examples in the CFlib using either of these classes:

Notes on location of autonomy and discrepancies

Considering the various options available in the Crazyflie Python library, it’s essential to realize that these setpoint-setting choices, whether direct or through the High-Level Commander, can also be configured through the app layer onboard the Crazyflie itself. You can find examples of these app layer configurations in the Crazyflie firmware repository.

It’s important to note some discrepancies regarding the Motion Commander class, which was designed with the Flow Deck (relative positioning) in mind. Consequently, it lacks a ‘go to this position’ equivalent. For such tasks, you may need to use the lower-level send_position_setpoint() function of the regular Commander class (see this ticket.) The same applies to the High-Level Commander, which was primarily designed for absolute positioning systems and lacks a ‘go forward with x m/s‘ equivalent. Currently, there isn’t a possibility to achieve these functionalities at a lower level from the Crazyflie Python library as this functionality needs to be implemented in the Crazyflie firmware first (see this ticket). It would be beneficial to align these functionalities on both the CFlib and High-Level Commander sides at some point in the future.

Hope this helps a bit to explain the commander frame work in more detail and where the real autonomy lies of the Crazyflie when you use different commander classes. If you have any questions on what the Crazyflie can do with these, we advise you to ask your questions on discussions.bitcraze.io and we will try to point you in the right direction and give examples!

Before we start settling down and preparing for Christmas, it’s time for another release! The last one was before the summer in July, and we’ve had quite a few changes on the development master branch that we’d like to share. You can now download the latest Cfclient through pip and install the newest firmware on the Crazyflie to 2023.11 via the CFclient.

Latest changes in CFclient and Cflib

The most significant change in the CFclient is that we have finally transitioned from QT5 to QT6 for the GUI graphics. Additionally, we have addressed some issues with the toolboxes. Finally, we have added an information box to indicate the state of the supervisor, such as whether the Crazyflie is considered tumbled, flying, or if a restart is required because it is locked.

Cfclient when the crazyflie is tumbled with supervisor info

For the backend, namely the Crazyflie Python library, some important changes have been implemented. Along with fixes to the parameter and logging framework, full-state setpoints have been introduced. This feature has existed in firmware for a while due to the Crazyswarm1 project (now Crazyswarm2), but it wasn’t implemented in the cflib until now. Additionally, it’s now necessary to use `notify_setpoint_stop` in cases of switching between high-level setpoints and regular position setpoints. There is also a generic motion capture example now based on the libmotioncapture library.

Note that even though the CFclient has been converted to QT6, there are several examples in the Cflib folder that have not been updated yet. This will be fixed soon, and a ticket has been created for it. Additionally, in the Bitcraze-VM, there have been some reported issues with QT6 (see this ticket).

Latest changes in the firmware

The firmware has undergone some important changes too. On the STM side of things, the hybrid TDOA mode has been merged (check out this recent blog post). This feature is still considered experimental, so please refer to the documentation for the right settings. Additionally, support for the supervisor information box in the CFclient has been added. To utilize it, both the firmware and CFclient need to be updated. There is also a new example demonstrating communication between gap8 and cpx. Last but not least, it is now possible to create Python bindings for portions of the Kalman filter, mainly for the Loco positioning system. On the other hand, the NRF firmware has no added functionalities except for some build changes and fixes.

Crazyradio2 + LPS tools

We’ve also made some improvements in other firmware or tools. Starting with the Crazyradio2, which includes fixes for broadcasting (important for you Crazyswarm2 folks!). We also aimed to make a new release of LPS tools since we heard that people were experiencing issues with USB devices. Unfortunately, there are some problems with the GitHub release actions, so that will likely be delayed. For anyone facing USB issues, you can install the LPS tools from source with Python following the ReadMe’s instructions.

Release details and Remaining issues

So here are the details of all that is released:

Some things still require attention that are a bit affected by this release, but we haven’t had the time to fix it yet:

  • Fix issues with LPS tools and release (see this ticket)
  • CFclient seems to be broken on the bitcraze-VM (see this ticket)
  • CFlib examples with QT-based GUI are still on QT5 (see this ticket)
  • The newest CFclient seems to need additional packages in some cases ( see this and this ticket)

Please let us know at https://discussions.bitcraze.io if you are having more problems.

Developer meeting this Wednesday

As we already announced last week in the Monday blog post, we will be having a developer meeting this Wednesday (6th Dec, 3 pm CET) regarding the Flow deck (refer to this discussion thread for joining information). Since we usually don’t fill up the entire hour, the last part of the developer meeting is available for some generic support questions face-to-face (online), including questions about the release!

When we originally wrote the TDoA3 implementation for the Loco Positioning System back in 2017 we had the idea of adding functionality to also enable the Crazyflies to send UWB packets in some situations, AKA TDoA3 Hybrid mode. We did not have the time to implement that idea back then, but through the years there have been some interest in the functionality and recently I finally got around to do it as a Fun Friday project. Annoying enough it was not that complicated and only took a couple of hours, I should have done it earlier!

We wrote a bit about the hybrid mode in an earlier blog post and there is also a github issue with some discussions on the topic. The short version of the functionality is that a Crazyflie at selected times switches from only passively receiving UWB packets from the anchors, to also actively transmitting packets and doing Two Way Ranging (TWR) with the peers in the network.

One use case is for a Crazyflie to simply participate in the TWR traffic to give it ranging information for improved position estimation. This can for instance be useful when flying outside the convex hull where TDoA positioning degrades rapidly while TWR works pretty well.

Another funky use case is to extend a Loco positioning system by using TWR to fly outside the convex hull and land somewhere. At this point the Crazyflie switches role and acts as an anchor instead by including its position in the transmitted packets and enabling other Crazyflies to use the transmissions for TDoA or TWR position estimation.

It is also possible to go even more dynamic and transmit the estimated position while flying and thus act as a flying anchor. There are complications when doing this with multiple Crazyflies as they use information from each other and the estimated positions probably will diverge if errors are not handled in a proper way, but at least there is now a framework where this type of functionality could be added. See the references to research in the area in the previous blog post.

The implementation is very experimental and has not been merged to master yet, but if you are interested you can find it in the krichardsson/hybrid-mode branch (PR #1330). There are a few new parameters that changes the behavior such as turning on/off transmissions, using TDoA or TWR data for position estimation and what to include in transmitted packets. Please see the implementation and documentation for details. Also note that the hybrid mode functionality is not compiled by default and must be enabled in the build configuration to be available.

Lately we have been thinking about and using the Rust programming language more at Bitcraze. In this blog post we will talk a bit about our current use, current experiments and potentially future use and how it will affect our ecosystem.

Rust is a system programming language that has good performance, is reliable and productive. Practically it means that it can be used to run small and fast code (well suited for embedded systems for example), be quite fun to write, and be reasonably sure that if it compiles, it works.

On servers

Over the year we have written and maintained a server system to handle a lot of things related to production and sales. This system is the one generating shipping quote when you order in our store, telling us that there is an order, printing packing lists and shipping labels for the order as well as keeping track of stock and telling us when it is time to order a new batch of product.

This system is used every day and has been invaluable to how we work at Bitcraze. It is mostly implemented as NodeJS micro services.

We have started writing new functionality for it in Rust instead of in a new monolithic service. This has been a great experience, not always easy, but the bonus is that once it compiles there has been almost no run-time error. This has allowed us to gain experience with Rust in an environment that is well documented: servers on PC.

In test rigs

Every manufactured product must be tested: there is no guarantee a board will work when it exits the re-flow oven. This test usually happens in a test-rig that measures and affects various signals on the board (look under your favorite Bitcraze deck and you will see test-points: round pads designed to enter in contact with test probes). Attached to this test-rig is a computer running our test software. We have used a Python-implemented test software for all our products so far and this system started showing its age by being harder and harder to work with and, most importantly, hard to deploy on computers in the factory.

For Crazyradio 2.0, we decided to completely re-write our test software, in Rust of course :-). The design of the test framework is very inspired by OpenHTF: the framework provides the basic architecture of the test and the executor, tests are implemented in Rust and implement all the required test phases. Test statuses are streamed to a web browser as well as to our server (to one of the newer parts of our server system written in Rust). There are two big advantages of using Rust in this application: making sure the test software works reliably and without errors saves a lot of time during manufacturing and helps make sure no bad board leaves the factory. Rust is also awesome to deploy and distribute: the software written on our Linux machine can be compiled for Windows/Mac/Linux on any architecture, no more Python environment to set up!

As for the deployment we actually choose to deploy the test software on a Raspberry pie managed by Balena cloud. This means that we can remotely update the test rig software and we are always sure that the right version is running in production. Rust has allowed that to be painless: we develop on our amd64 PCs and it compiles out-of-the-box and works on the ARM64 Raspberry Pi.

In embedded systems

Now we are coming to our more experimental use of Rust, until now on fun-Fridays project but soon on prototypes. We have been playing with Rust on embedded for quite a while: I have re-written the Crazyflie2’s stm32 boot-loader in Rust, we have experimented with Rust on a couple of our ESP32-based prototypes. Embedded systems are never as easy as programming on PC and the way Rust libs are organized to guarantee good usage of the peripheral does not always yield good error messages from the compiler. But, for sure, it does not feel good and it feels very scary to come back to C: the Rust compiler checks so many things that it makes programming fun, with C, any small mistake will bite hard a couple of weeks later.

We have just started working seriously on a new deck (more about it in a future blog post ;-) and we have started in Rust. We do still take that as an experiment: we keep our options open to coming back to C if there is any hiccup. But so far it looks quite good.

In the Crazyflie lib?

That is a future plan, that we have not started to work on seriously at all, but that we are planning for the future. We are planning to write a new version of the Crazyflie lib in Rust with binding to other languages.

According to our experience so far, Rust is safe, fun to write, and very easy to distribute to all the systems we currently support with the Python lib and more. On top of Windows/Mac/Linux, Rust would enable support of our official lib on the web, in embedded systems (ie. ESP32), as well as on iPhone and Android.

The plan would be to have the low level of the lib, ie. communication with Crazyradio and the Crazyflie and subsystems drivers, implemented in Rust. Then binding to Python, C++, Ros, Javascript, … can be made to allow usage of the lib in these languages. This would have the advantage of allowing every current user to use the official lib without having to re-implement their own special-purpose version. On the Python side, nothing would change, in the sense that a Rust-implemented lib can be installed with “pip install cflib” …

Conclusion

This blog post is a request for comments: if you are a user of the Crazyflie and have strong opinions for or against Rust we would like to hear about it. We want to make it clear that we are not planning on porting the Crazyflie firmware to Rust: the Crazyflie is designed as a development platform and we are aware that Rust is not yet as used or well-known as C or Python. However, the firmware running on a deck CPU or in the bottom of the lib would benefit a lot from Rust’s advantages and do not need to be modified so often outside Bitcraze (it is of course always open-source and we encourage contributions :-D).

We will keep you updated if we make more progress on the new deck and the lib, in the meantime we will keep having fun experimenting :-).

The python client is based on Qt and we have been using version 5 so far. Qt5 has been replaced by Qt6 quite some time ago and as Qt5 is not really maintained any more, we have been looking at switching to Qt6 for a long time. Finally we have taken the step, this blog post will outline what has changed.

The switch to PyQt6 is not that complicated for the majority of the client code base, apart from some minor changes in various classes, the biggest update is that enumerations are handled differently. If you check out the changes in the pull request you will see that imports have changed for obvious reasons

# PyQt5
from PyQt5.QtCore import Qt
from PyQt5.QtWidgets import QMessageBox
# ...
# PyQt6
from PyQt6.QtCore import Qt
from PyQt6.QtWidgets import QMessageBox
# ...Code language: Python (python)

Enumerations are not available directly on the Qt type anymore, instead they must be named using their fully qualified names. For instance

# PyQt5
Qt.DisplayRole
# PyQt6
Qt.ItemDataRole.DisplayRoleCode language: Python (python)

And some other minor changes.

The Qualisys tab

The main blocker for doing this change has been that the Qualisys tab was using a library that used to be available in PyQt5 but that has not been added to PyQt6, we ended up removing the Qualisys tab to be able to move on. The Qualisys tab was originally contributed by Qualisys for demo purposes and it had two nice areas of functionality that we would like to re-implement:

  1. Motion capture positioning. It was very easy to connect a Qualisys mocap system and feed the position information to the Crazyflie for automated flight.
  2. Trajectory support. It was possible to fly some simple trajectories, for instance a circle which is nice for testing and demos.

The idea is to re-implement the mocap connectivity using the libmotioncapture library from Crazyswarm2, as an extra bonus this would support all major mocap systems. For the trajectory part, we would like to add this as a new tab that can be used by any positioning systems, mocap, Loco, Lighthouse or flow deck. If you are interested in helping out with this, let us know!

The state of the code

The changes for PyQt6 have been merged into the master branch. It seems to work but we have not tested all functionality yet, please let us know if you run into any problems or weird behavior.

When you have pulled in the code from github you will also have to re-install all dependencies by running

pip install -e .

Enjoy!

Today, Vivek Adajania from Learning Systems and Robotics lab write about a project for a safe motion planning of Crazyflie swarm that was published at ICRA 2023. Enjoy!

Motivation

Quadrotor swarms offer significant potential in applications like search and rescue, environmental mapping, and payload transport due to their flexibility and robustness compared to single quadrotors. The core challenge in these applications is collision-free and kinematically feasible trajectory planning. As the quadrotors share space, they must safely manoeuvre around each other and avoid collisions with static obstacles. Existing solutions [1] [2], while effective for generating collision-free trajectories, often struggle in densely cluttered scenarios due to simplifying approximations.

Background

There are two literature groups in the domain of optimization-based quadrotor swarm motion planning: centralized and distributed approaches. In a centralized setup, a central computer solves a joint optimization problem that computes trajectories for all quadrotors at once. These approaches have broad solution space but quickly become computationally intractable as the number of quadrotors increases. On the other hand, the distributed approach involves each quadrotor independently solving its optimization problem and incorporating trajectories shared by the neighbouring quadrotors. This strategy offers improved scalability, yet existing distributed approaches struggle in cluttered environments.

Fig. Centralized and distributed planning approach to quadrotor swarm motion planning. The arrows indicate the flow of communication.

In this work, we adopt a distributed planning strategy. The independent optimization problem that needs to be solved by each of the quadrotors in the distributed setup is a non-convex quadratically constrained quadratic program (QCQP). This nature of the problem stems from non-convex and quadratic collision avoidance constraints and kinematic constraints.

Existing distributed approaches rely on sequential convex programming (SCP) that performs conservative approximations to obtain a quadratic program (QP). First, linearization of the collision avoidance constraints to obtain affine hyperplane constraints. Second, axis-wise decoupling of the kinematic constraints to obtain affine box constraints. We obtain a QP but with small feasible sets.

Fig. Conservative approximations made by Sequential Convex Programming (SCP) based approaches.

Proposed Approach

In contrast, our proposed approach obtains a QP without relying on the previously mentioned approximations. The first ingredient is the polar reformulation of collision avoidance and kinematic constraints. An example of the 2D polar reformulation of collision avoidance constraints is shown below:

Fig. Example illustration of polar reformulation of 2D collision avoidance constraints.

The second ingredient is to relax the reformulated constraints as l-2 penalties into the cost function and apply Alternating Minimization. Alternating Minimization results in subproblems that are convex QPs, and some have closed-form solutions, thus obtaining a QP form without relying on linearization; further details can be found in our paper [3]. We can also use and reformulate alternative collision avoidance constraints, barrier function (BF) constraints

where hij is the Euclidean distance between quadrotor i and quadrotor j, and the parameter γ controls how fast the quadrotor i is allowed to approach the boundary of quadrotor j.  

Results

We experimentally demonstrate our approach on a 12 Crazyflie 2.0 swarm testbed in challenging scenes: obstacle-free, obstacle-rich, shared workspace with a human. The experimental video is provided below:

In the simulation, we compare our approach against two SCP approaches: SCP (Continuous) [2] enforces constraints across the entire horizon, while SCP (On-demand) [1] enforces only on the first predicted collision. Our (Axiswise) includes box kinematic constraints, while Our (Quadratic) preserves the original quadratic constraints.

From our simulation results, we see that SCP (On-demand) has a lower compute time than SCP (Continuous), as SCP (On-demand) enforces fewer constraints. But, this compute time trend comes at the expense of success rate. On the contrary, our approaches achieve a high success rate with low compute times. Ours (Quadratic) has a slightly higher success rate than Ours (Axiswise) as it has access to large kinematic bounds.

Fig. Simulation results from 100 start-goal configurations with swarm sizes ranging from 10 to 50 in a cluttered environment with 16 cylindrical static obstacles.

Fig. Simulation results from 100 start-goal configurations with swarm sizes ranging from 10 to 50 and three different γvalues in a cluttered environment with 16 cylindrical static obstacles.

On average, our approaches achieved a 72% success rate improvement, a 36% reduction in mission time, and 42x faster per-agent computation time—our approach trades-off mission time with inter-agent clearance and distance to obstacles via BF constraints.

Outlook

In this work, we presented an online and scalable trajectory planning algorithm for quadrotor swarms in cluttered environments that do not rely on the linearization of collision avoidance constraints and axis-wise decoupling of kinematic constraints. We do so by reformulating the quadratic constraints to a  polar form and applying alternating minimization to the resulting problem. Consequently, our planner achieves high scalability and low computation times than existing approaches. We also show that we can reformulate barrier function constraints to introduce safety behaviours in the swarm. One of the future works is to extend the approach to navigate the swarm in a complex 3D environment.

References

[1] Luis, Carlos E., Marijan Vukosavljev, and Angela P. Schoellig. “Online trajectory generation with distributed model predictive control for multi-robot motion planning.” IEEE Robotics and Automation Letters 5.2 (2020): 604-611.

[2] E. Soria, F. Schiano and D. Floreano, “Distributed Predictive Drone Swarms in Cluttered Environments,” in IEEE Robotics and Automation Letters, vol. 7, no. 1, pp. 73-80, Jan. 2022, doi: 10.1109/LRA.2021.3118091.

[3] V. K. Adajania, S. Zhou, A. K. Singh and A. P. Schoellig, “AMSwarm: An Alternating Minimization Approach for Safe Motion Planning of Quadrotor Swarms in Cluttered Environments,” 2023 IEEE International Conference on Robotics and Automation (ICRA), London, United Kingdom, 2023, pp. 1421-1427, doi: 10.1109/ICRA48891.2023.10161063.

Links

The authors are with the Learning Systems and Robotics Lab at the University of Toronto and the Technical University of Munich. The authors are also affiliated with the Vector Institute for Artificial Intelligence and the University of Toronto Robotics Institute (RI) in Canada and the Munich Institute of Robotics and Machine Intelligence (MIRMI) in Germany.

Feel free to contact us with any questions or ideas: vivek.adajania@robotics.utias.utoronto.ca. Please cite this as:

@INPROCEEDINGS{
adajania2023amswarm, 
author={Adajania, Vivek K. and Zhou, Siqi and Singh, Arun Kumar and Schoellig, Angela P.}, 
booktitle={2023 IEEE International Conference on Robotics and Automation (ICRA)}, 
title={AMSwarm: An Alternating Minimization Approach for Safe Motion Planning of Quadrotor Swarms in Cluttered Environments}, 
year={2023}, 
pages={1421-1427}, 
doi={10.1109/ICRA48891.2023.10161063} 
}