search

gtfactslab / CrazySim

A Crazyflie simulator for testing CFLib Python code, ROS 2 nodes through Crazyswarm2, custom crazyflie-firmware modules, or perform a flight demo on the crazyflie-python-client.
GNU General Public License v3.0
72 stars 15 forks source link

Improve MPC performance #15

Closed gy2256 closed 5 months ago

gy2256 commented 5 months ago

Hello,

thanks for the great work!

Have you considered improving the MPC performance by linearizing the dynamics w.r.t. hovering such that the optimization problem is easier to solve?

More specifically, I'm referring the dynamics model that is defined here:

https://github.com/llanesc/crazyflie_mpc/blob/e37eae23f99f8edfb303f1e3c2dd99685de5e9ca/crazyflie_mpc/quadrotor_simplified_model.py#L30-L81

llanesc commented 5 months ago

Hi @gy2256, I have done some linear MPC before, but I decided to push this as Nonlinear MPC because it's purely a demonstration. I think the main limitation is having to transmit control commands through the radio. The main contribution of this repo is the simulation of crazyflie-firmware with Gazebo and the capability of using CFLib/Crazyswarm2/crazyflie-python-client. I'm thinking of actually packaging the ros2_ws into a separate repo so that people that just want the simulator can use this repo.

gy2256 commented 5 months ago

Thanks for the reply!

The reason I brought this up is that I saw you have implemented two control loops for broadcasting the attitude command and solving the MPC.

https://github.com/llanesc/crazyflie_mpc/blob/e37eae23f99f8edfb303f1e3c2dd99685de5e9ca/crazyflie_mpc/crazyflie_multiagent_mpc.py#L115-L116.

Please correct me if I'm understanding it incorrectly. Are the nonlinear dynamics making the problem harder to solve and thus taking longer? I did some testing with the real hardware (with a different controller), and it seemed like the control/sampling callback function for Crazyflie needs to have a minimum of 30Hz to ensure stable flight. Therefore, you have the self._main_loop running at a faster rate (50Hz) than the self._mpc_solver_loop (10Hz). I was thinking that if the dynamics is linear, then the solver is probably able to match the update frequency of the self._main_loop.

Yeah, I think it makes sense to have a separate repo for ROS implementation as there are many implementation details involved.

llanesc commented 5 months ago

From my experience, you can solve Nonlinear Quadrotor MPC with simple constraints at about 100Hz. The video of 16 cfs actually had the prior implementation with just a single solve and publish loop. I wanted to see if this type of approach of having a separate timer for the solver and publisher improved performance and to be more consistent on the publishing rate when you run this for more than one drone and on lower computer hardware like a laptop. Basically MPC solves the optimization problem up to some horizon and the control publishing timer will publish the stored control from the prior MPC control queue.

gy2256 commented 5 months ago

Thanks for the explanation!