Motion planning

This page contains the code for the paper "Motion planning algorithms
for the Dubins Travelling Salesperson Problem"


A randomised algorithm that finds quickly feasible solutions to large instances of the Dubins Travelling Salesperson problem.


Languages: C++ and Matlab
Necessary infrastructure: • a C++ compiler that supports the C++11 dialect
• the library ‘random’ from the boost libraries
• the Matrix Template Library (MTL)
• Matlab
Preferred infrastructure: the necessary infrastructure + Xcode
Files: download


  1. Download the
    • .zip file from the link, above,
    • the library ‘random’ from boost, and
    • the MTL.

    [Both ‘random’ and the MTL are header-only libraries, so you only need to save them somewhere; there is no installation. Some comments about these two dependencies can be found below.]

  2.  Compilation:
    • If you have Xcode: In the .zip file there is an Xcode project that can be readily compiled, once you provide the path where boost and MTL reside (the path information goes in the field ‘User header search path’ in the Build Settings of the Xcode project). Of course, the #include directives in the source code should be in accordance with the header search paths.
    • If you do not have Xcode: All the header and implementation files are in the same folder. You can compile from the command line.
  3. Once you have an executable, run it and it will create a .txt file in the same directory. The filename will look like <date>__size_.txt
  4. Run plotCppOuput.m (Matlab script provided), after making sure that, somewhere in the first lines of the script, the load command loads the file from the previous step. That is, make sure that the line “load(‘date_time_size_number.txt‘)” contains the correct filename.
  5. The results should look like the figure below.


    Admissible DTSP tours


One of the algorithms we introduce in our paper (the 2-Opt 2-step LAA), generates the tour in the lower right corner. The other algorithm introduced in the paper, the 2-step LAA, generates even shorter tours in general, however it is much more computationally and memory intensive and will need algorithmic and implementation improvements to handle an instance of the size shown in the figure, within reasonable time.

Some comments

  • Reproducibility is a sine qua non of honest research and this is why the code, above, has been made available (under the MIT licence).
  • The code lacks in user-friendliness and, due to many practical constraints at the time it was written, it has not been designed with software-engineering best practices in mind. Rather, it is just some code that does what it is supposed to do. Many easy improvements can be made that would lead to a download-and-double-click kind of end product, however I am not working on the code anymore. I am not even working in the area of motion planning anymore. If, by sheer coincidence, some person out there cares enough to polish the code or even transcribe it to python and put it in a module, wrapped in a bow tie, that would be great.
  • At the end of the day, once you overcome the initial psychological barrier raised by obscure code written by someone else, all you have to do is compile the C++ files, run the executable to generate a .txt file, and run the provided Matlab script on the .txt file.
  • Another thing to keep in mind is that some of the implemented algorithms have inherently complex components. It is not the coding style that obfuscates matters; an understanding of the problem and the solutions published in the literature is necessary to follow the logic.
  • The dependence on the MTL library is just silly. When I was developing the code I had to experiment with prototypes and I needed a high-level concise syntax for operations with matrices. However, once the dust settles, all you need is two-dimensional vectors and 2×2 matrices, and simple multiplications between them and with scalars. It is straightforward to write (preferably inline) functions that do such things and eliminate the need for linking with MTL. If you code in C++, the way to do it should be obvious. If not, it will be mysterious. That said, if you bother to get rid of the dependency on the MTL, then the STL suffices for the code to run. Also, the MTL code will most likely emit warnings about conversions that lose precision. View them as extra incentive to get rid of the dependency.
  • The dependence on the ‘random’ library from boost is not silly, but is not necessary either. The mersenne twister (according to some people, the ultimate honour in maths is to have your name written all in lowercase — think of abelian groups) is now part of the STL.
  • I would be happy to provide short clarifications and answer questions that are answerable by email, however I do not provide support for the code provided here. I do not work in this area any more and I am being paid to do completely different things at this point in my life.
  • If all you are looking for is a way to compute the length of Dubins and relaxed Dubins paths, and plot them, you will find such functions in the .m file (i.e., the Matlab script). Most likely, I have improved versions of these functions, in case you find them useful, but you encounter problems. Before you despair or decide that the code is wrong, please make sure you understand the subtleties and challenges of the problem. Even the basic problem of finding a Dubins path between two configurations has a discontinuous value function (I am referring to the value function of the optimal-control formulation of the problem) and, therefore, small changes in the data of the problem (positions or orientations) can lead to significant changes in the results (completely different paths).


An interesting little project—if you are, say, a master’s student working on algorithms for the DTSP—would be to build on the 2-Opt 2-step LAA to create an algorithm that runs on much larger instances of the DTSP. The 2-Opt 2-step LAA is almost embarrassingly parallelisable. That is, you can have different threads running the 2-Opt 2-step LAA to find open paths through (mutually exclusive) subsets of points, if you partition the original set of points into clusters. Then, the open paths can be combined into a single tour through all the points. One question to be addressed is the construction of the clusters; they could be constructed using a k-NN type of algorithm, where the “distance” between the points is the length of relaxed Dubins paths. This is, of course, a philosophy not a rigorous definition of an algorithm, but the details are not hard to fill in.

This entry was posted in . Bookmark the permalink.

Leave a Reply

Fill in your details below or click an icon to log in: Logo

You are commenting using your account. Log Out /  Change )

Google+ photo

You are commenting using your Google+ account. Log Out /  Change )

Twitter picture

You are commenting using your Twitter account. Log Out /  Change )

Facebook photo

You are commenting using your Facebook account. Log Out /  Change )

Connecting to %s