Implement Model Predictive Controller

[SteveMacenski]

MPC is the defacto standard these days. While DWB is closely related, it is not as extensive and configurable based on model types and base characteristics.

Something to spend some real time thinking about on a white board is whether that's a good general default for this project. MPC will likely be heavier than DWB and we need to balance generalization/algorithmic improvements with CPU/GPU intensive tasks.

Deep reinforcement learning is basically MPC taken full-bore but I think we're quite a bit of academic research away from making something on that front that will be directly usable on industrial hardware.

This goes along with #1278 and #1279 which I have intent and experience to implement, this ticket though I'm very much so open for someone else taking over.

[shivaang12]

@SteveMacenski How about Time Elastic Band?

[orduno]

(assuming we are talking about a receding horizon LQR)

MPC will likely be heavier than DWB

While it might actually be less expensive to compute a cycle by DWB/A (we would still have to see if the estimating (trajectory generator) and discarding (critics) of trajectories on DWB is faster than tweaked optimizations on MPC), MPC might perform better and require less cycles to get to the goal pose.

But, DWB, through the critics, provided a straight-forward way to define the behavior of the robot, for example, obstacle avoidance. ~Not sure yet how we would do that with MPC.~ With MPC, one option is to define optimization constraints that yield the desired behavior. We might still need to produce multiple alternatives on MPC and discard through critics.

[yathartha3]

@SteveMacenski How about Time Elastic Band?

@shivaang12 TEB has been ported for Dashing. It in in the TU Dortmund repo.

[yathartha3]

I'm very interested in this. However, can only start working on it after a month or so.

[SteveMacenski]

I'm also going to take a deeper technical dive into TEB. They claim it has some optimality to it and if that's true, it may be worth actually making TEB (or some variation on) the default controller over DWB. I don't, however, like the idea of "taking" TEB from its home and copying here. If we decide to go with TEB, we will need to come up with a maintenance plan or documentation about it (or git submodules). Especially because the project is still very active.

TEB has configuration guidelines for ackermann robots (and clearly works for the others). If we create a planner that supports ackermann and update TEB to check collisions using footprints rather than costs, we could claim full support for non-circular robots of types: holonomic, diff drive, and ackermann. While all in 2D, I'll take the W. Hopefully whatever changes are required to move to a gradient perception stack doesn't shoot us in the foot with any of this.

[croesmann]

TEB can actually be seen as a special realization of MPC:

• Variable horizon length
• Terminal equality condition (however, less restrictive since the horizon length is variable)
• System dynamics are described in terms of a flat system, so optimization is just performed in the state space (SE2 poses) and inputs are afterward reconstructed from the pose trajectory.
• Discretization using direct collocation via finite-differences.
• Point-to-point motion (the reference trajectory is static in each open-loop step, i.e. the current intermediate goal from the global plan) [a quasi path following mode is achieved by via-points]
• Minimum-time objective
• Hard-constraints are approximated by quadratic penalty functions (soft constraints) and weight adaptation -> leads to an unconstrained nonlinear least squares problem

So it is comparable with a point-to-point time-optimal model predictive control approach. Whereas the soft-constraint approximation and the dynamics description usually result in very fast computation times (compared to standard MPC), they are also introducing some restrictions:

• The soft-constraints do not scale well for very large optimization problems repsectively, the weights are hard to tune. Just thinking of autonomous truck parking, in which dimensions are much larger than for a small indoor robot.
• The geometric (/flat) system description just based on the pose trajectory makes introducing further constraints more difficult. E.g. for car-like robots limiting the steering rate introduces a very nested function which can result in an ill-defined optimization problem for some configurations. However, the current state without steering rate constraints works quite well, but requires the robot to steer relatively fast.

During the last year I implemented a generic MPC framework at my department which can realize variable-horizon time-optimal MPC but also standard quadratic-form receding-horizon MPC, etc. I also wrote an mpc local planner plugin that includes the features of the TEB but with full state space robot models and hard-constraints. Also other MPC configurations are possible. The package is still private, however, I thought about pushing it in the next days to ros1 and ros2. Of course, computation time is larger than for the TEB, but still very fast to my mind.

I am reusing some classes from the teb_local_planner, like PoseSE2, obstacles, robot footprint models, distance computations. This could be related to your last paragraph, as this is all required by gradient-based optimization and is not yet part of navigation or navigation2. I thought about moving these classes to a separate ros repository which TEB and MPC planners depend on. This is also the reason why I am holding back the ros2 release of TEB right now.

Instead of copying the TEB (which I actually don't like as well), what about adding a dependency on it to navigation2 meta package? We can make sure to push the new TEB release quite soon after every new ROS release, as the number of dependencies is low. I could also add a further maintainer. What do you think? Edit: okay, the circular dependency could be a problem as bloom releases all projects at once...

[SteveMacenski]

Alright, there's alot there. We may want to take some of this offline into emails for brevity and come back here with a summary, especially if you have some of this work in private branches you don't want to excessively discuss in this forum. My email's on my github page if you want to address a response there.

My current plan is to mess around with creating prototype 2.5D C-space planners. On my chopping blocks a simplified Hybrid-A, RRT, and optimization planners to build feasible paths with the "typical" planning methodology that the costmap_2d inflation layer removes. This will give us the basic ability to plan time-variant plans with orientation information for different drive types and base footprints. I'm not convinced on their own any of those 3 will actually be tractable, so optimizations will need to be made but I want to start with the gambit to work from (and as an exercise to myself, who hasn't done motion planning now in about 3 years).

So with that in mind, the trajectories a planner would get are no longer just 2D waypoints, but with target orientations. Is that something TEB can handle or could be made to handle? One key reason for this is the irritating behavior differential drive robots have when a path requires them to spin in place and something like TEB/DWB will make the robot rotate and drive forward for a "hooking" maneuver. This maneuver can often be quite fast and does not look safe at times. If we can track orientation, than that information for a spin-around should be embedded in the plans. There are many (I'd say even most) situations where if I need to spin > 90 degrees, I want to have it happen in place so it doesn't take up too much space in narrow places (like warehouse aisles).

---

Other than compute, what are the reasons to stay with TEB over this new MPC, should it be available?

I wouldn't be copying TEB over. Frankly, I don't want to maintain it. Adding it as a dep for navigation2 is possible, but I like having everything in one place. An option is I add TEB as a git submodule so a recursive clone of Nav2 will grab teb as well and place it inline. I don't think we need to add all plugins to the main repo, but the default ones should be clearly called out in some way. I may be over-thinking it and that isn't necessary however.

but requires the robot to steer relatively fast.

That is a point I've seen before and it straight up scared the crap out of me. That was the last time I ran TEB on my larger robots. Would your MPC project resolve that?

the weights are hard to tune

I haven't tried, but DWB isn't exactly easy either.

Something that concerns me slightly about TEB is that its a controller, and deals with dynamic obstacles, and goes through via points, and ..., and ... I don't think you've hit the point of feature creep, but for a new user, its alot to swallow all at once. And for an experienced user, there are tutorials on the Wiki for these things, but all marked experimental which doesn't inspire confidence. Assuming that these things aren't just experimental but ready for prime-time, I think that all could be handled through documentation improvements. The last concern I've run into before is that if you don't run TEB fast enough, it acts very unstably and as far as I can tell, there's not some computable minimum update interval to be found on the general case about the minimum update speed to be effective.

Side note: Do you find taking diffs in the costmaps detects and tracks dynamic obstacles effectively as in this tutorial? I haven't looked at this code, but I tried a similar thing at another company and the results were not promising in deployed testing.

More thematically: do you as the author believe that TEB is objectively a better default controller for navigation? If this was the default, what areas do you think would be better / worse off by that decision?

Edit: I think for navigation to directly depend on it, we'd need to add 2 maintainers: Tully and me. Tully so that if I disappear he can assign a new navigation stack maintainer, and me so that I can keep a ROS2 master branch up to date with interfaces and changes. I wouldn't plan on (or seriously, ever) dealing with algorithmic or non-pedantic updates.

[SteveMacenski]

Covered in the scope of https://github.com/ros-planning/navigation2/issues/1710