Bring MPC to new rpq_quadrotor_control

[foehnx]

@kohlerj a first idea:

I propose to make a pure virtual class for as controller_base, then implement children of that class as position_controller and model_predictive_controller.

There are some differences between the two which we have to cover in a clean implementation:

• a unified run() function. right now the interfaces need different arguments:
  • position_controller.run(state_estimate, reference, parameters)
  • model_predictive_controller.run(state_estimate, reference_pose)
  • OR model_predictive_controller.run(state_estimate, reference_trajectory)
• position_controller takes parameters as argument in each function call, while model_predictive_controller sets the parameters once and has them as a member variable. I propose to set the parameters once. What's the reason to pass them every time? (It's a lot of memory access in the MPC)

Therefore, I propose a solution by introducing the controller_base templated on the parameter struct, as we used to do it. This could have a run function as: controller_base.run(state_estimate, trajectory) where, the trajectory can have one array element for poses (and always for the position_controller) and multiple elements for the MPC whenever needed. I would not pass the parameters for the controller down from the autopilot, but pass the node handles in the constructor, so that each class from controller_base can read the parameters by itself from the ROS parameter environment (easier to implement). The parameters of the controller should not be handled by the autopilot as they have no function in this class.

[kohlerj]

Hey,

From what I understand, correct me if I am wrong, the only difference btw the position controller and the mpc controller is the fact that the latter needs the whole future trajectory whereas the former only need one point. If SO, then we could adapt the actual position controller to take the tail of the actual trajectory (a one element one in case of a simple reference) and pop the first element as reference. The mpc controller would use the whole trajectory.

Also I think passing the parameters at each time is not that bad, overhead is minimal since passed by ref. Rather, it gives you the opportunity to dynamically modify the behavior of the controller, let's say if you're simply moving to your start point before executing a more aggressive trajectory.

[foehnx]

Agree on passing the whole trajectory tail! Regarding the parameters: Due to the interface of the solver I would have to a) write the cost matrices every loop or b) check the whole matrix (or at least diagonal) every time for change, which is a waste of time. Could we have a bool to indicate a change on function call? something like run(state_estimate, reference, parameters, bool changed = true)? The interface for position_controller would remain the same but we could exploit it for the MPC.

[foehnx]

Additional: Could we template the whole class AutoPilot on <typename Controller, typename Parameter>? Otherwise we would have to include rpg_mpc in the rpg_quadrotor_control or make it a dependency because of the need of controller and parameter class at compile time. In my proposed way we could instantiate templated on position_controller in the autopilot.cpp and add an instance of it templated on MPC to the rpg_mpc repository (just one file/line defining the instance).

[mfaessle]

@foehnph When merging #60 and before closing this issue, could you please make a small comment with what you actually implemented out of all the things discussed above? Thanks

[foehnx]

Status comment for merging #60

• [x] Autopilot is templated on typename Controller, typename Parameter
• [x] Standard Autopilot still uses position controller and its parameters.
• [x] MPC repository instantiates Autopilot based on MPC controller and its parameters.
• [x] unified run() method uses reference trajectory instead of reference pose (Trajectory Point).
• [x] therefore Trajectory has constructor taking one TrajectoryPoint as argument to quickly generate a single reference for the position controller.
• [x] Autopilot now picks a lookahead on the reference trajectory if in executeTrajectory().
• [x] MPC can take a Trajectory with any amount >=1 of TrajectoryPoints as reference, robustly tracking static (pose) or dynamic (trajectory) references.

[mfaessle]

is the lookahead always used? also if the mpc is not used? Because then the default parameter should be 0.0.

[foehnx]

right now, yes it is always used. we can change that at a later point because functionality does not depend on it. for now I will merge.

[mfaessle]

isn't this a lot of unnecessary overhead for computing the reference trajectory when using the position controller?

[foehnx]

well... the whole tracking is not very optimized right now. you also looked through the whole trajectory to find the current state at each loop... so no, it is not many additional iterations, if that's what you ment.

shall I add hints and links to MPC in the readme and the wiki once it's public?

[mfaessle]

I mean in "executeTrajectory" you are now always composing a trajectory of lookahead duration (to be fed to the controller's "run" function) which is not necessary for the position controller since it only uses one trajectory points and therefore the autopilot's default "predictive_control_lookahead" parameter should be 0.0 for use with the position controller or do I not understand that correctly?

[foehnx]

you do understand this correctly, but it since position controller only takes the first point in the reference, passing it a whole lookahead is not a problem. Regarding computational overhead: as I mentioned before, we're searching the whole trajectory up to the point where time_from_start >= time in every loop... this is in most situations more overhead then just adding the additional lookahead. So in terms of optimization, we should first take care of the tracking algorithm and make in only search forward from the last iteration point. I have some ideas regarding this by making an new Tracker class... but this is future work.

[mfaessle]

got it, I will not bother you any longer ;)

[foehnx]

no problem, happy to hear opinions on work you did mainly xD

[mfaessle]

@foehnph I guess this can be closed... How about advertising the MPC repo in the readme here and also describe and link to it in this repo's wiki?

[mfaessle]

@foehnph What's the status here?