Drone Spinning bug in ROS2 webots simulation

[hedaniel7]

Hello,

I observed the above spinning behavior when using your TinyMPC as the controller of a drone in a ROS2 node which is connected through subscriber/publisher to the Webots simulation.

(I did consider the conversion from quaternion to Rodriguez parameter here)
x y z r p y vx vy vz ox oy oz
x_init here is (0 0 0 0 0 0 0 0 0 0 0 0) (The position is updated with coordinates from the Webots simulation)
x_ref here is (0 0 1 0 0 0 0 0 0 0 0 0)

Commit to the code: AlboAlby00/CrazyflieControllers@16fd146
main interesting file is the ROS2 node which only receives coordinates of the drone and publishes motor values calculated with TinyMPC
https://github.com/AlboAlby00/CrazyflieControllers/blob/16fd146a583c1796024a8895326e26698181b92e/crazyflie_controllers/src/TinyMPC_controller.cpp

and terminal output: https://gist.github.com/hedaniel7/f0c1f88e444776e13216be25247bf32b

After ruling out that the order of the motor values u1, u2, u3, u4 might be in the wrong order and thus might cause a orientation failure and unless I didn't do anything wrong in my implementation,
I have the hypothesis that maybe something is wrong with calculated and outputted motor values?

I have this hypothesis also, because whenever I set the x_init to be statically the zero vector (0, 0, 0 ...) (not floating point values close to zero like 3.882e-18 for example set by the Webots simulation, but perfectly 0) and x_ref to be (0 0 0.9 ...) for example I get motor values all being zero.
https://gist.github.com/hedaniel7/bd5ba9b75e37e7673d879652ab72465d (Search for the title "Changing xref , when x0 is zero vector always")

But as soon as I set x_init to (0, 0, 0.01 ... ), then I get strong motor values and the drone lifts off
https://gist.github.com/hedaniel7/bd5ba9b75e37e7673d879652ab72465d (Search for the title "x0 << 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0;")

Somehow small changes in the values produce vastly different values

Did I do something obviously wrong or overlooked something in my implementation?

Steps to reproduce:

Tested on Ubuntu 20.04

• Install ROS2 Galactic
  sudo apt-get install ros-galactic-webots-ros2 (Installation of the webots simulation)
  git clone https://github.com/AlboAlby00/CrazyflieControllers/
cd CrazyflieControllers/
git checkout 16fd146a583c1796024a8895326e26698181b92e
colcon build
source install/setup.bash
ros2 launch crazyflie_controllers TinyMPC_controller_gdb.launch.py

I can understand if this is too much asked as a question and you choose not to reply.

Thank you very much for your time and effort

[xkhainguyen]

Hi there,

Thanks for reaching out. We didn't use third-party simulators for this project so it's hard to diagnose the problems here. You should be careful about the input (ie states) and output (ie motor thrusts) orders and units between the simulator and TinyMPC.

Also, we're aware that some people have made TinyMPC work on the Crazyflie in the RoSE (PyBullet) simulator with some care in mind. The first one was the conversion from the output of TinyMPC (normalized motor thrusts) and input to the simulator (total thrust and torques). The second one was the problem data which I already explained in #12. Initially, it stabilized but flew away instead of hovering for them.
We can connect you with them if you are interested.

[hedaniel7]

Hi,

Thank you very much for replying. Yes please, I would like to connect with them. I have sent you an email.