Including webots simulator into Crazyswarm2

[knmcguire]

Hi! I want to get started to include webots into Crazyswarm2! I went through the code of the current implementation and had some ideas or questions of how to handle this, and it might be better to start refractoring and clean up things first. This is perhaps a bit too vague to start issues for now so that is why I started a discussion here.

Crazyflie Server

So I was thinking to have a thread per topic, so that is perhaps easier to discuss?

These will consist off:

• The difference between the crazyflie_server.py in the crazyflie and crazyflie_sim folder.
• Refactoring necessary in the CF firmware
• How to implement the backend within webots.
• And more to be added if we come up with anything

[knmcguire]

Hmmm I can also handle this within this issue: #117. But since I have multiple topics discussed, we could use the thread feature of this?

[knmcguire]

We can for instance update the issue with outcomes of these discussions to prevent clutter

[knmcguire]

First question: The crazyflie_server.py in crazyflie_sim was written from originally the script in crazyflie package. Is it ultimately the idea that these two codes are merged into one? There is quite some overlap.

[whoenig]

No, I think the serve very different purposes and have different dependencies. It might be possible to put some common functions in some utils file, iff we put both in the same package.

[knmcguire]

I guess that would be possible right? Such we can move it to the crazyflie package that we call itcrazyflie_sim_server.py, and make a separate class for the utils. That can make more sense?

But at least this way, since we want to keep it separate, I don't have to refactor the original crazyflie_server.py before including the new simulator so that works out.

[knmcguire]

2. For the Mellinger some changes were merged to make it more suitable for bindings, with multiple instances. Mellinger: support to change gains in bindings bitcraze/crazyflie-firmware#1180

But this creates some if statements here:

crazyswarm2/crazyflie_sim/crazyflie_sim/crazyflie_sil.py

Lines 271 to 274 in bc0489c

	if self.controller_name != "mellinger":
	self.controller(self.control, self.setpoint, self.sensors, self.state, tick)
	else:
	self.controller(self.mellinger_control, self.control, self.setpoint, self.sensors, self.state, tick)

I guess it would be ideal to unify the framework, which means that we also have to make data structures for all the other controllers as well. Also, I believe that @krichardsson had some ideas of improving the structure in general, but maybe that might be too much work for now.

[whoenig]

Yes, ultimately, we need these sort of firmware changes for each controller that needs proper multi-robot SIL support. I tried that a while ago for PID, but this controller is spread out it so many code files that it's actually difficult to put the state in one struct.

[knmcguire]

I think this has been a wish of Kristoffer for a while to refractor this... Maybe for now I'll just focus on the mellinger for the simulator but I've made an ticket for the crazyflie-firmware to unify the controllers in this way: bitcraze/crazyflie-firmware#1198

[knmcguire]

In my previous attempt to integrating the webots simulator, the driver file had too much functionality in one, but I can still use it as some inspiration, so let's see how to do that.

We can get measurements similar in this part and set the motors with these parts

But... calling the simulator at a fixed timestep needs to be figured out. Currently the driver also have the ROS calling in there, but that is ofcourse handled now from the crazyflie_server side... so in terms of how to implement it I'm afraid that I probably can not use the ROS_driver example of webots 🤔

[knmcguire]

That is what I was hoping for. But it seems to be a bit more complicated than that. The supervisor is just another instance in webots with an external controller, which is ros2_supervisor or whatever we write it to be. Then the external controller needs to be connected to the port that represents the controller of the supervisor in webots: https://cyberbotics.com/doc/guide/running-extern-robot-controllers

In the ROS2 driver/example, this connection is made by making a URDF file (like here in the Mavic file) where the webots_ros2_driver is making the connection by being initialized with that robot description + controller file after being initialized in Launch. But that means, is that webots_ros2_driver owns the instance of that controller and not our script, and then we have to deal with the interROS2 thread communication.

So, if we start our own supervisor instance, we also need to establish the connection to the external controller in Webots ourselves.

Anyway, that is as far as I got so far. I'm going to be reading more webots doc today to understand how to do this, because it sounds better to try to avoid ROS2 making the external controller connection as we will have more control. Do you agree?

[knmcguire]

Oke.. maybe it might not be super complicated. As I understand, we have to make sure that the environmental variable WEBOTS_CONTROLLER_URL needs to exist in the same environment as where the sim control mode is being handled.

[knmcguire]

I'll give it a go to see if I have control over at least the supervisor, but the external controllers for each crazyflie is perhaps going to be a bit more trickier as these controllers need to run with a different environment variable 🤔. But let's take it one step at the time.

[whoenig]

So I don't think that we need a separate plugin for each Crazyflie. Instead, we should be able to use the webots Python API to set the desired motor forces directly. One example is here: https://github.com/cyberbotics/webots_ros2/blob/a296551cbe3d5e293e1ac36ef2f1c8611ec0c0e6/webots_ros2_driver/webots/lib/controller/python/controller/motor.py.

At the end, all this ROS2 stuff in webots is just a wrapper around the existing webots Python API. I think it makes most sense to use that API directly, because it will be much more efficient that way.

[knmcguire]

Yes, providing the fact that the supervisor can control the crazyflie instances in the webots directly through the external controller. Let's for now assume that it will, as that would be the approach that requires the least change of the current framework.

In my previous attempt I was able to create the crazyflie instance in webots like this:

crazyswarm2/crazyflie_simulation/crazyflie_simulation/crazyflie_webots_spawner.py

Line 37 in df3899c

children_field.importMFNodeFromString(-1, 'Crazyflie { translation '+str(location[0])+' '+ str(location[1])+' '+str(location[2])+' name "'+ name +'" controller "<extern>"}')

. Hopefully there exists an API that can get that instance of the control!

[knmcguire]

FYI, I'm currently working on my own fork of crazyswarm2 to hack around and try out ideas. After I found a solution I'll make a draft PR here

[knmcguire]

Alright. I've made some good steps with the simulation: https://github.com/knmcguire/crazyswarm2/tree/add-webots-sim-2

I have quite some control with the supervisor node, I can spawn the crazyflies and observe their position. But... I can not access the internals of the crazyflie instances, which includes sensors and motors. The documentation says:

However, even a Supervisor cannot access the internal information and measurements of devices mounted on a different robot, such as camera images and distance sensor measurements.😞

So yeah... there needs to be a controller per Crazyflie instance in order to access the motor control

[whoenig]

That looks great already - thanks for the update!

[knmcguire]

It seemed so easy from these instructions: https://cyberbotics.com/doc/guide/running-extern-robot-controllers#single-simulation-and-multiple-local-extern-robot-controllers

But no, I got the error: "Error: only one Robot instance can be created per controller process.". And when I asked the webots guys for clarification, it is that I can only run one python file per controller robot.

So either I've got to solve it with ROS messages anyway, or perhaps try the receiver node: https://cyberbotics.com/doc/reference/receiver?tab-os=linux&tab-language=python

[knmcguire]

or.. perhaps python threads might be enough here.

[knmcguire]

nope... the importer detects the controller as still being run from the same process. so this won't work either. Might need to rethink the strategy and start again from scratch

[knmcguire]

I've made a draft PR ! #179

Still want to intergrate the multiprocessing library to make it even cleaner and find out what is going on with the control

This is the schematic of that PR

[knmcguire]

So a bit of bad news. Currently I am not able to tune the Crazyflie with the pythonbindings with the standard timestep of Webots. The numpy backend runs much faster and that is able to handle it, but the bindings for the controller behave very different if it is running at a slower rate. Technically webots can run at 1 ms... but then it is so slow that it is hardly worth it. This is an issue we will need to investigate first.

Before I got it to work in webots with the pythonbindings, but I had to scale the output of the controller waaaaay down, and even scale the roll pitch yaw commands even more to make it flyable. So then using SIL loses it's purpose I'm afraid as the simulation is so far off the actual flight.

So I'm going to do it a bit differently, since the backend structure allows to send through setpoints to the simulator as well. So I'll extend the simple simulated crazyflie controller with no bindings to handle setpoints ( only velocity and position?), which acts on setpoints that the server is feeding to it. The SIL can ofcourse do high level commands and generate setpoints based on that since that is something that I don't like to rewrite ofcourse :)

This is the approach I was thinking about, but I'll first work on the crazyflie_simulation repo to write this controller. So the (3rd) PR will come in the weeks after that.

Sorry again for the delay! But I hope it will come out all nicely in the end 🙂

[knmcguire]

I kind of noticed when writing an 'onboard' controller for the webots crazyflie: the behavior of the controller directly embedded into the webots robot node, behaves differently than when I 'externalize' that same controller through a TCP connection (like I was trying to do before). I couldn't investigate any further in this due to the upcoming quartely meeting but I just wanted to put it here to remind myself to create a minimal example of this such that I can notify the Webots maintiners on this.

[knmcguire]

I'm starting with a third attempt of intergrating the webots. Now, staying closer to webots intented implementation, and that the webots backend sends out topics instead. It will allow me to switch between python bindings on the controller itself and outside like in the current framework more easily.

Very early works started on my personal one but I will make a draft PR once it is at least flyable. https://github.com/knmcguire/crazyswarm2/tree/add-webots-sim-3

[knmcguire]

So... we have some issues with intergrating webots properly in this, which has to do with timing issues in webots and the python bindings, which makes it difficult to have good syncing.

To allow ourselves to have more time to take a good look at the python bindings, I proposed together with @whoenig to have an 'external sim' backend instead, that does not start a simulator but sends out topics, which can be read in by an external simulator. This way, we can allow ourselves time to fix the issues with the python bindings and such. I've made an issue for this and will push a PR soon: #252

Also, this will show the modularity of Crazyswarm 2 even more.