Model predictive control (MPC) is a popular process control method that has a general definition related to satisfying constraints given a well-defined model of the system inputs and behavior. The general definition can be found elsewhere, but in the context of robot motion planning specifically, it corrects a robot's trajectory each timestep in response to the initial definition of constraints in both the environment and the robot's own motion, as well as any changes or inaccuracies in these constraints throughout the simulation.

An oversimplification of the method itself is:

Define the motion mechanics of the robot--basically, how do the actual inputs into the robot's machinery (electric signals, pushing a button, pulling a level, etc.) map to their actual result (how far does the robot actually move, how much does it accelerate)
Define the constraints of the environment--the spaces that the robot is allowed to move in, obstacles (static or dynamic), etc.
Define the ideal trajectory of the robot, which may or may not change throughout the simulation depending on whether or not the environmental constraints are static or if there are other actors in the scene
Every timestep, compute the ideal set of inputs for some horizon (some number of timesteps ahead) given the robot's current configuration, and then tell the robot to do the first of those set of inputs.
The result is that the robot's local trajectories are decently well-optimized, allowing for it to respond better to local changes in constraints compared to some precomputed model.

MPC has traditionally been limited to the fact that the process is online; it was designed to redo the calculation over the next horizon every timestep, which has really only been feasible in the last 10 years in the consumer world since consumer PCs were generally not strong enough to do this back then, especially in cases of complex constraints (solving differential equations and other computationally expensive processes).

Nowadays, with stronger consumer hardware, it has become much more popular in general because it is pretty robust to dynamic constraints, which would be most constraints in the real world, especially humans that the robot needs to interact with or avoid.

Here, you can see a simple graph of an MPC process. MPC tries to predict the ideal inputs that would result in the ideal trajectory over the horizon. However, unlike many other not-so-realtime models, it does not follow through with the decision for the entire horizon. It recomputes as often as possible. It can be combined with the Kalman filter to also correct for inaccuracies in the robot motion itself (e.g. bad calibration).

My Project

My final project is supposed to be on HRI between humans and robots that are tethered together by a string, leash, or some other connector. Due to the nature of dealing with a human, especially one providing continuous direct feedback on the robot's motion (especially by pulling), I needed to explore motion planning methods handling dynamic constraints. MPC seemed like a good idea to look into.

[doi 10.1109_ICRA.2016.7487274] Neunert, Michael; de Crousaz, Cedric; Furrer, Fadri; Kamel, Mina -- [IEEE 2016 IEEE International Conference on Robotics and Automation (ICRA) - Stockho.pdf

I was especially fascinated with this particular project, since it's still the only example I've seen of trying to get a robot to respond to being secured to a tether--the drone in the beginning of the video is attached to a pole by a rope.

I wanted to try to replicate it and test it on a dynamic tether, possibly as a final project. At the very least, for this project, I wanted to get MPC working in a game engine (Unreal 4, specifically), since that is how I'll need to do my final project.

This is the citation for this work, and the PDF if you want to go through it:

Neunert, Michael, et al. "Fast nonlinear model predictive control for unified trajectory optimization and tracking." 2016 IEEE international conference on robotics and automation (ICRA). IEEE, 2016.

In the end, I underestimated the complexity of implementing MPC... in particular, optimization is difficult, and there are not many existing implementations that work for my needs. In the end, my results are mostly stress testing a particular MPC implementation and figuring out how to get it working in UE4 with some trickery. I also tried it in a few different constraint environments to see where it breaks.

Now, I'll go through the process of getting this working and what specifically I ended up being able to do.

MPC Libraries

Step 1: See if anyone else has tried to implement MPC and shared the code. In order to focus on the application itself, I didn't want to reinvent the wheel.

There are actually quite a few, but most of them have issues I'll get to in a bit (search: https://github.com/topics/model-predictive-control).
Most of the libraries I found were in C++ or Python.
- C++ makes sense because it is arguably one of the best-optimized general object-oriented languages in existence (that people actually use), and since MPC relies on good optimization, I imagine that C++ is one of the only good ways to get MPC working in a lot of applications. Also, I believe ROS uses C++ & C.
- Python makes sense for ease of use. It's pretty terribly optimized, though, which is why most precompiled Python libraries were made in C++/C and simply exposed to Python.
You might notice in the above Github link that there are 2 popular libraries for building MPC models:
- Operator Splitting QP Solver (OSQP): https://github.com/oxfordcontrol/osqp
- Control Toolbox: https://github.com/ethz-adrl/control-toolbox
- The implementation that I ended up using uses OSQP as the optimizer backend.
- There are also some other less-known (and probably less optimized since they heavily use Python) implementations such as doMPC: https://github.com/do-mpc/do-mpc . I'm providing these links in case someone wants to try them out.

Now, for actual MPC implementations.
- The best-looking one is this (https://github.com/JunshengFu/Model-Predictive-Control). It even has this 3D car demo and a nice visualization.
  - The biggest problem is one that plagues 3D graphics as well--it only works well on Linux. In my experience, it's a lot of work to try to build projects like this on Windows, even with CMake or prebuilt binaries, because there are bound to be compatibility problems or hours of troubleshooting random errors that show up as a result of the compilation process gone wrong. You also often end up needing to built really basic libraries as well just to make CMake happy (like Boost for C++ or CUDA....)
  - So, I didn't use it. My experience with APIs like NVIDIA's and Intel's deterred me from trying to compile a Linux binary for Windows.
  - If you're wondering why I can't just use Linux for the entire game engine implementation, it's because neither Unity or UE4 work well on Linux, especially for VR applications (which my tethered robot dog is meant for). People in game development rarely use anything but Windows for the game development part itself, except indie devs (who are usually forced to switch back to Windows once they get a job at a major studio anyway).
- This MPC lab at UC Berkeley provides many different libraries for general use (https://github.com/MPC-Berkeley). However, you'll notice that most of them were made for Linux or ROS.
- This implementation looked decent (https://github.com/forgi86/pyMPC). I tried it, but was deterred by lack of examples related to vehicles and poor visualization. This one is similar (https://github.com/AtsushiSakai/PyAdvancedControl).

Library I Used

Ultimately, I ended up using this one called Multi-Purpose MPC (https://github.com/matssteinweg/Multi-Purpose-MPC).

It uses OSQP, as mentioned above. As I'll get into later, this actually made life a bit harder because this library cannot be installed with easy_install, so I could not use the handy embedded UE4 Python implementation I normally use.
The visualization is nice. It's essentially a 2D version of the car demo above
It's well-documented and well-explained...for the most part (we'll get to this...).
The code is very simple. As a quick intro to the structure, it is just a handful of Python files that interact with each other:
- map.py defines a grid describing the structure of the environment, its obstacles, and ideal/reference path. It describes occupancy with a simple binary system. 0 means the surface is navigable, 1 means it's occupied.
- MPC.py handles the computation of the vehicle's next step, using OSQP.
- spatial_bicycle_models.py defines the simple bicycle model used to move the vehicle in this demo. It is similar to the popular unicycle model used in differential drive (to the right), but a bicycle cannot turn in place like the unicycle--it has a length between its front and rear wheels defining how it must turn.
  - Both models (and most vehicle models meant for simulation) basically use the mechanics of the vehicle to eventually result in 2 parameters used to define where the simulated vehicle should go: a velocity vector (v) and a turn angle (phi). Many model acceleration as well.
- simulation.py defines the simulation parameters
  - size of vehicle
  - motion constraints of vehicle (max inputs, max velocity/acceleration/rotation)
  - which "map" to use (a static image defining the base environment--basically a race track)
  - where the obstacles are
  - finally, a loop moving the vehicle according to the MPC controller.
    - I made a version of this, as described below, called simulationUE4.py, which uses UE4 for this step instead.

The car is the yellow block. The red path is the predicted horizon of the vehicle. The orange path is where is actually went. The gray path in the background is the precomputed reference path, before obstacles were added.

Integration with UE4

RIP UE4 Python

Here's where the fun begins...

UE4 uses C++ and a native visual scripting languages called Blueprint, which is essentially high-level CPP that compiles way faster and is easier to use, while not necessarily being better-optimized than writing native C++. It also autocompletes correctly, unlike anything I've ever used in Visual Studio.

However, UE4 (unlike Unity, the main competitor) is source-available, which has allowed for tons of great plugins. In particular, I like this plugin allowing you to use Python in UE4 (https://github.com/dontnod/ue4-python), which is especially helpful for using scikit, numpy, and matplotlib. I've used it in the past to output hundreds of columns of data per-frame of a user study simulation for later analysis.

There are some low-level issues that make it a bit hard to use:

The precompiled version of the plugin, for some bizarre reason, does not list Anaconda as a Python directory. You can't fix this without recompiling the plugin from scratch in C++, which can cause plenty of other problems in a UE4 project and can be incompatible for no apparent reason (I've used this plugin for 3 years and still haven't gotten to the bottom of it).
The version of the plugin that has embedded Python uses the typical portable version of Python which really only works correctly with easy_install. You can install pip, but I always run into compiling errors of some type (usually something with cython). With easy_install, you can install popular libraries like scikit and matplotlib, but more specific libraries needed for the above MPC implementation, e.g. OSQP and scikit-image, cannot be installed this way.

I tried to find any way of getting this plugin to work with the MPC implementation above... but unfortunately, nothing worked. In the end, I resorted to an old trick...

Cross-Process Communication

A relatively unknown trick in the game development community (really, most people outside of networking) is that you can communicate between pretty much any processes on the same machine, regardless of program language or API, by connecting them by UDP. Most people think UDP (as well as TCP) is for exclusively inter-PC communication, but it's great for processes as well, and it has basically no latency when the processes are on the same machine.

My proudest use of the UDP trick is in this AR project here (https://www.nickvr.me/ARmulticamlaparo), in which I got 7 independent camera processes, a VR tracking process, a game engine, and a separate AR device to all communicate correctly with little latency

This is my favorite reference for simple UDP communication in Python: https://wiki.python.org/moin/UdpCommunication

I used the same process to get UE4 to communicate with Python. Since I couldn't get the plugin above to work, I ran the MPC controller on a separate Python application that was constantly awaiting input (in the form of specific strings sent through UDP), and I used a UDP plugin (https://github.com/getnamo/udp-ue4) for UE4 to communicate with that Python thread.

I needed both a UE4->Python connection and the reverse, so I opened 2 UDP ports.

The UE4->Python connection is used to tell the MPC controller where the obstacles are, where the vehicle is, the actual timestep (as determined by rendering, which is not necessarily consistent), etc. It could also be used to say which map to use, in the future. Ultimately, I ended up just using the MPC simulator itself to dictate where the UE4 car should go, since there were some synchronization issues and coordinate system bugs (explained later), but this was a sound strategy of doing this communication. Since UE4 has a powerful vehicle physics engine, I want to explore this later, but for this mini-project, I did not have time to move past the bicycle model.

The Python->UE4 connection tells UE4 the result of the MPC controller's recommended motion parameters. It also listens for the result of the car actually moving.

Coordinates

An important issue with any spatial cross-communication is calibrating the coordinate systems, and this MPC implementation did not do a good job of explaining what defines the origin or direction of the map.

Here are my findings:

-The map is flipped. I'm not sure why and it's never mentioned anywhere in the documentation. To the right, you can see the demo, and below that is the actual image file that the program uses.

-The above issue made it very hard to figure out where the origin is. The code says that they defined the image origin as (-1, -2), which is very mysterious. Through a lot of testing, I found that the coordinates are defines as in the third image on the right. I have no idea why they chose that origin.... maybe a bad dart toss.

-The car starts at the pink dot in the third picture (obvious from the video)

-They define a set of waypoints, which are the green cones in the image below. To make the reference path in the center of the track, they use some kind of spline interpolant. I'm not sure why they didn't just put the waypoints in the center of the track....

-The obstacles are the black dots in the video and the red dots in the below picture (they appear slightly smaller in UE4 because I'm not sure the scale is accurately calibrated for these).

-The entire track is about 35.3m x 35.3m in size. This is determined by a weird calculation they have where they define some ratio of size:pixels. This is a bad idea from a graphics perspective because a higher-res track will be misinterpreted as much bigger if the same ratio is used. The image they use for the map is 500px x 500px, and the ratio is 0.005.

-They have another test map that is very noisy and used real data (4th image on the right). I tried it on their simulator and it looks ok, but a bit too simple for what I want. I made a video for it below. They have some code that tries to remove holes in the data, so this seems to be how they get the trajectory so smooth.

-The yaw direction is flipped between coordinate systems (multiply by -1)

-UE4 uses cm while this MPC uses meters.

-We can take advantage of game engine features to simplify keeping the car in the local transform space of that weird origin (the scale is important as well) by simply making the car a child of the origin, as well as all of the obstacles (see bottom image).

realdata.flv

Inputs

A major limitation of this MPC implementation, which is also mentioned in their README, is that this bicycle model is so simple that it doesn't take any input or outputs besides the 2D position and the yaw/bearing of the vehicle. This is all that is used to decide what the robot does and whether or not it succeeded.

However, for vehicles, we usually want a better model for physical forces. Land vehicles need friction and other surface interaction forces, and air vehicles worry much more about drag and wind. UE4 has a powerful vehicle physics library (e.g. below, it has a great free vehicle template with realistic wheel physics), but I could not take advantage of it. It actually should not be too hard to model since UE4 is essentially already doing it (making it easy for the higher-level programmer to simply provide throttle and steering as seen below), but I did not have time :(

Thus, our framewise motion inputs are still simply the next (x,y) location and next yaw (phi).

Results

I made sure the basic model worked in UE4 (basically replicating their exact demo from the above video of the track), then I wanted to stress test it with a couple of modifications to the track. I'll describe these and show demos below.

I also played around the vehicle size parameters, but they weren't very interesting (as you would expect, the vehicle will not move through a path it cannot fit through.... the simulation just crashes). The only fun I had with vehicle parameters were acceleration/velocity. Setting these ridiculously high completely breaks the simulation, as you might imagine (see right).

breaksaccel.flv

Basic Map

This is the basic demo map they provided. I copied the visuals here for reference

The first video shows the basic map with the same obstacles as above. As you can see, it works fine, with pretty much the same trajectory as before.

The second video shows the same map with many more obstacles, but the car still navigates around successfully (a bit less smoothly towards the end with the clump of obstacles).

basicmap.flv

map1.flv

Open Map

I cut large holes into the above map to see if it would confuse or change the path of the vehicle. Since the waypoints that are provides were not ordered for the most part when provided to the system, the direction itself should not have been hardcoded. Actually, the waypoints initially provided are only used for precomputation; the system actually generates as many waypoints are there are grey dots in the original video after the initialization.

It didn't seem to affect anything. The found path is probably the most optimal path through all of the waypoints.

empty.flv

Noisy Map

For this test, I wanted to see the denoising they're doing in action. Recall that this is how they correct the real data example map above. It seems that the noise is successfully passed in this map, since they are only a few pixels wide, but as we see in the next tests, bigger pieces of noise confused the car significantly.

noise.flv

Blot Map

In these tests, I added big blots of noise around the map. This generally wasn't a problem, and in some cases, the pointy parts of the noise are not enough to be considered obstacles, but the more serious concern is that adding some noise near the top of the map (top relative to the demo video, not these flipped maps) seemed to completely break the trajectory of the vehicle, causing it to dive into a wall. Oddly, it did this even when there were no blobs in that corner it's diving into.

This error is not very promising for robust systems...

blobs.flv

blobs2.flv

blobs3.flv

blobs4.flv

Colors & Shapes Maps

In these tests, I wanted to see if color mattered and if hollow shapes would get registered correctly.

Shapes that are too narrow don't get registered correctly.... Notice the change when that black blot that is sticking out is drawn more thick. At some point, the model decides it can't go through any more.

Hollow shapes seem to get registered correctly (see the spike walls) although the bug causing it to dive into the wall seemed to happen if the top left corner is too dense with obstacles.

It seems that color does matter. If obstacles are not labeled with a color that is almost black, they don't seem to get registered correctly (see the 4th test when that red blot is changed to black).

colors.flv

colors2.flv

colors3.flv

colors4.flv

Challenges & Failures

I ended up not being able to get this working in 3D space like the inspiration paper, mostly because of the way that all of the available Github code seemed to implement the navigation with 2D maps. I tried a very simple test where the Y coordinate was also used to change Z (so the map would also describe basically a tunnel that the vehicle goes through.... but only in 1 axis (so they're not really tubes.... it's more like an ant farm). It didn't really work and the visualization was very jumpy. This implementation in 3D would require a 2D map for every single cross section of the space, which is tedious and inefficient. You would need something like a heightfield to accurately and efficiently represent such a space, voxelizing it much like they turn these 2D maps into grids (see my mini-project 1: sites.google.com/robotsinvr).
- This is not really that important to me since my robot dog is constrained to the ground.

We see in the blot example videos multiple pathfinding failures in strange cases. I would need to narrow this down to use this for anything robust. I think part of it is the extremely low resolution of the picture (500p). This could be causing some weird invisible grid piece causes by a single noise pixel or something like that.
The map needs to be more or less black or white to work. This would generally not be much of an issue, since navmesh-based navigation works similarly--a surface either has a collision or doesn't. However, if you wanted a robot to be able to destroy or go through something for some reason, I could be a limitation.
I tried getting dynamic obstacles working, but it didn't pan out. I could not successfully remove obstacles, even though the code seems right to me and it doesn't complain. I think there's an extra step needed to update the computed grid. For the same reason, dynamic obstacles did not quite work either, but once I figure out how to update the constraints, having the link to UE4 makes it much easier to have all kinds of dynamic obstacles compared to Python-only.
The waypoint system is annoying and tedious. In simulation.py, they define these waypoints manually, but really, this should be done with a navmesh or something like that instead of hardcoding and using magic numbers. I'll forgive them because it was a class project and I'll probably end up doing the same thing :)

Conclusion

In conclusion, I wasn't really able to replicate an exact figure from the 2016 paper, but I was able to try out the basic method in a large variety of different environments to see where the publicly-available implementation (that actually works correctly on something besides Linux) fails. I also described my method of taking advantage of Python features in processes that are not designed to use Python, such as game engines, so hopefully that can be helpful to someone.

It turns out there's a lot of work to do if I want to use this for my haptic tethering project. However, after this, I'm not entirely sure that MPC is the first step to the haptics problem--it doesn't seem to be a good way of modelling the dynamic behavior of the human or leash, because it seems that MPC requires that the model is well-defined, which is probably impossible for a human. Additionally, it seems more useful for trajectory and spatial control than more abstract/physically-based control. MPC is useful for later parts of the process when I get to more robust navigation.

The Github repo is here (https://github.com/nrewkowski/MPCVehicle), but you obviously need UE4 to run it correctly or see the blueprint code (it's not as interesting as it sounds; I can screenshot it if you have trouble getting to it. It basically just sets up a UDP listener and parses messages from MPC).

The Python part is in the subfolder called Multi-Purpose-MPC-master (it a modified version of the repo mentioned before: https://github.com/matssteinweg/Multi-Purpose-MPC/). If I were better at Github, I probably could have made it a legit branch...

Thanks for reading!

Page updated

Google Sites

Report abuse