Add physics-based simulation

[whoenig]

No description provided.

[whoenig]

This change got bigger than I hoped, mostly because some refactoring was needed and I also included another useful visualization tool. Let me know if you want me to try to break this is smaller PRs for easier review.

I verified that the 3 firmware controllers work on simple trajectories as well as the previous mode that uses no physics-based simulation. Note that the control performance of PID and Brescianini aren't great. Future changes in both crazyflie-firmware and here are needed to allow the user to tune gains.

[knmcguire]

Hi! Sorry for the delay.

But great! I will try to find some time this week to look at it but it might be next week if that is okay?

[whoenig]

Sure, no rush!

[knmcguire]

Hi!

Before I give a thorough review, I noticed that there might be something off with the np simulation when I tried some parameters, what you can perhaps confirm if this is supposed to happen or not:

• If I put the max_dt to 0.1 in server.yaml with backend: np, the simulated crazyflies do not respond
• nice hover seems to be going well (also with 2), but with PID there is a very big overshoot (but you mentioned this might be a problem)
• With Figure8, the simulated crazyflie is unable to finish it in time. With two they don't even take off but just move on the ground. This is with mellinger.
• If the further the crazyflie with figure8 gets seems to be depended on the computer performance of the simulation. On high performance it seems to finish the first 'round', but on a lower performance it only makes it to one quarter of the figure 8.

Perhaps there is something off with the simulation time perhaps?

[whoenig]

1. max_dt just artificially limits the update rate. This logic wasn't changed in this PR, so it is possible that there is a bug in the limiting logic. I would argue that fixing this should be part of a separate change?
2. Multiple CFs are not supported yet, because of limitations in the firmware bindings (essentially, all CFs share the same controller state, which can result in strange behavior). I have a firmware change ready for Mellinger to fix that, but need to flight test before I PR.
3. PID controller doesn't work well for me either. I think we might need slightly different gains in simulation (tuning gains requires a firmware change for the bindings). It might also be interesting to see what happens with different physics backends to rule out issues in the logic on how the firmware controller is called/mapped.
4. For your simulation time issues: Are you testing using

ros2 launch crazyflie launch.py backend:=sim
ros2 run crazyflie_examples figure8 --ros-args -p use_sim_time:=True

? I think if the simulator itself is called with use_sim_time, there might be funny behavior.

I notice that this change is very big and perhaps it's too early to update the documentation, since more (firmware) changes are needed to properly support the multi UAV cases. Would it help if I split up the PR in smaller logical chunks (and hold off on the doc changes)?

[knmcguire]

Ah yes, if I put use_sim_time to true it indeed does it correctly. I didn't see the arg before.

I don't think smaller PRs are necessary. In concept this is already quite a big change and smaller chunks of PRs will not make it much more simpler probably. We just need a bit more time to review :)

But perhaps we can leave the doc changes for now indeed.

[whoenig]

I reverted some of the doc changes that were about the physics backend specifically.

I think this PR is difficult to review since it mixes refactoring and new features. Perhaps reviewing by commit is easier than looking at the whole thing.

[knmcguire]

So I mostly have very minor request, mostly comments. In general I agree with the current framework as it is. I mostly also wanted to go through it in detail so I'll understand how to implement the webots simulator :)

For the rest, let's indeed make separate issues for the max_dt not working for physics based simulation, and for making the bindings more separate so that multiple drones can be flown.

[knmcguire]

Some context here perhaps as not everybody knows who Tobias is :) Wondering where a good place would be to place these csv files you plotfitted these from...

[whoenig]

We have them in https://github.com/IMRCLab/crazyflie-system-id (a private repo). I am happy to make it public and refer to it, if the data itself is fine to share publicly?

[knmcguire]

I don't see why not actually! I'll ask tobias just to be sure but since the plots are already out. Perhaps you can already go ahead and refer to it in the text and make the repo public once I received the okay.

[knmcguire]

Just a bit of a comment as I am a bit OCDs for symmetry tough, as now we have:

• Crazyflie
  • Cpp
  • CFlib
  • Sim
    • None
    • np

While perhaps it might be nicer to do

• Crazyflie
  • Real
    • Cflib/cpp
  • Sim
    • None/NP

This should not be part of this PR but something I noticed and perhaps we can keep in mind for the future.

[knmcguire]

Also currently, we will be limiting ourselves to only simulations that have an python API. That is in general not a problem, since Webots and the pybullet based sim, but Gazebo will be out probably for the time being.

I'm okay with that as long as you are okay with the limitation as well, and I'm sure that when the time comes we can perhaps see if we can make a wrapper for that.

[whoenig]

I pushed some of the minor changes and left comments for the bigger ones.

Structure: The current one is even a bit more chaotic:
crazyflie pkg has teleop, py, and cpp backend and then crazyflie_sim has the simulation backend. I agree that this is not ideal, we should either separate more, or combine packages again. The only pro I see for separation is that packages can be easily used without weird dependencies (e.g., one can choose to just use the crazyflie_sim, if no physical hardware is present). One reason why we put things just in crazyflie was to use easy ros2 run commands, including for the CLI tools like reboot.

Sim Limitations: Yes, although this was already in the design. One could rewrite the whole simulator in a different language (and then use the C-firmware directly, rather than the Python bindings). I think Gazebo is still possible. If I remember correctly, most functions are done by writing (C++) plugins. Then we would write a plugin that exposes setting the rotor speeds and querying the robot state from Python (e.g., with zeroMQ).

[knmcguire]

Yeah that is true. It is not ideal but let's solve it for now with good documentation and update the overview to add a detailed schematics once the dust settles. We can keep this in mind for any future redesigns! I think it is more important to get to a good working base state first.

Gazebo is indeed only c++ plugins which is quite difficult to work with I felt, but I wrote some base of it to do exactly those things you said. We can also have the query communication go completely through ROS and have gazebo separate but I'm not sure if we would have nice control over the simulator steps. Once we come to the point where we want to implement gazebo, I'm sure that there are multiple ways to solve this.

btw, it seems that the CI is failing, but couldn't see the exact reason

[whoenig]

CI: some of the ROS2 tests in crazyflie_interfaces are failing (ValueError pep257_rosidl_generated_py). My guess is that somehow the ROS2 version in the docker image the CI uses changed. I couldn't find anything using a google search so far and this is definitely unrelated to this PR.

[knmcguire]

btw, congrats on submitting your first contribution @whoenig 😜

Must be a bug haha