Close this search box.

Planning High Quality Motion Paths for Robots, Workshop, Spring 2013

Software Workshop (sadna), Spring 2013 (0368-3500-43)








Instructor: Dan Halperin

Teaching Assistants: Oren Salzman, Kiril Solovey
Wed 14-16, Schreiber 08

Code and related files / links: 

  • CGAL installation guidelines [link]
  • Workshop code framework ALPHA (simple GUI, no server) [code] [documentation]
  • Programming assignment – CGAL bootcamp [code]

General information: Motion-planning algorithms compute collision-free motion paths for objects that move among obstacles.  They arise in robotics, graphical animation, computer-aided surgery, navigation systems, computational biology and computer games, among other domains. In this workshop we will explore and apply algorithms for planning high-quality motion paths. Specifically, the students will utilize a framework, called Motion Planning via Manifold Samples (MMS), where exact geometric tools are combined with a sampling-based approach, to implement a new planner. The workshop will end in a competition between the different teams’ planners.

Link to Sadna 2009

Link to Sadna 2010

Link to Sadna 2011

Link to Sadna 2012


In the beginning of the workshop, we will present you with relevant background material in a series of lectures:

Course Requirements

  • Programming abilities: The students are expected to program in C++. Familiarity with the language is preferred but not required.
  • Teamwork: The project is intended to involve a significant amount of teamwork. The recommended team size is three.
  • Course staff: Except for doing the project for you, we are here to help you with any question. Please do not hesitate to contact the course’s team.

Project examples from the 2009 workshop

1. Non holonomic motion planner

2. Motion planning for a Lego-NXT robot

Tentative Schedule (stay tuned!)

The project will proceed according to the following milestones:

20.3 Forming teams – You will submit (by email) the group members, a group name and a general description (100-200 words) of the project’s main idea.

17.4 Planned project presentation – You will have a 15 minutes slot to present the project to us in class, followed by a discussion.

24.4 Submission of final plan. Submit a detailed final project plan (by email), based on the initial plan, but this time we will expect more detailed solutions:
(i) Algorithmic details.
(ii) Milestones towards the final goal.
(iii) Tools that are going to help you program / run the project (libraries, programming languages, etc.).
(iv) Open questions, conflicts and so on.29.5 Project “Proof of Concept” By this time, you will be required to show (presentation in the lab) that the basic technical infrastructure of the project works (e.g. tools or programming libraries that need to be installed, etc.). We will give you a small programming task that you will need to accomplish by this time. By this date you will need to decide (and commit to this decision) what prototype you will be presented at the next milestone.

24.7 Prototype and Integration (league interface freeze) At this point in time, we wish to see (presentation in the lab) your initial development, in order to make sure you are working in the right direction. You will show us a basic prototype of the final project (as you committed to). The prototype is expected to be a relatively small part of the final project (it is not expected to be fully functional). However, it should give a very good feeling of where you’re heading. In addition, by this stage we will check that your code can integrate with ours for the league to run smoothly.
28.8 Submission. Submitting a fully functional project, including documented code, a detailed user guide and a developer guide .
(i) Developer guide – will include details about the algorithm, the implementation and the code design, to allow a new developer to approach your code.
(ii) User guide – should outline the purpose of your project and its usage in a clear manner.8.9 League + Presentation Participation required.
Presentation – 15 minutes EXACTLY for each group + 5 minutes time for questions (we will be strict on time). You should give a concise description of the the algorithm and the implementation.  You will need to discuss the advantages and disadvantages of your implementation.

    • Oren Salzman, Michael Hemmer, Barak Raveh, and Dan Halperin.
      Motion Planning via Manifold Samples
      In Proceedings of the 19th Annual European Symposium on Algorithms (ESA), pages 493-505, 2011 [link];  arXiv, 2011 [pdf]
    • Oren Salzman, Michael Hemmer, and Dan Halperin.
      On the Power of Manifold Samples in Exploring Configuration Spaces and the Dimensionality of Narrow Passages
      arXiv, 2012 [pdf]
    • Barak Raveh, Angela Enosh, and Dan Halperin
      A Little More, a Lot Better: Improving Path Quality by a Path Merging Algorithm (H-Graphs)
      IEEE Transactions on Robotics, 27(2): 365-371, 2011 [link] ; arXiv [link]

      Relevant Papers

    Additional Reading Material

    • Chapter 7: Sampling-Based Algorithms in Principles of Robot Motion: Theory, Algorithms, and Implementations
      H. Choset, K.M. Lynch, S. Hutchinson, G. Kantor, W. Burgard, L.E. Kavraki, and S. Thrun, The MIT Press, 2005.
    • Chapter 5: Sampling-Based Motion Planning in Planning Algorithms,
      S.M. LaValle, Cambridge University Press, 2006
      Web version
    • Motion Planning via Manifold Samples

      Robot Section

      This year projects will be based on Pololu 3pi robots, groups will be given up to 5 robots in 1-2 robots increments.

      in order to control the robots all you have to do is use a serial communication protocol through the usb dongle we will provide. for the first step you can use the Arduino serial monitor.

      • Arduino program download:
        • note that the arduino software comes with the drivers to operate the modem, but if you are using windows 8 or a unix/mac system you might need to download FTDI drivers on your own –
      • Once you have the software ready (just unzipping it) select the proper COM port under tools and than start the “Serial Monitor” this little terminal allows you to transmit chars to your robots for controls.
        • make sure to change the serial communication speed to 57600.
      • for starting phase you only get 4 char commands, more than that and robot will not behave as expected the list of commands is:
      1. r – turn right
      2. l – turn left.
      3. f<#> f plus number 1 to nine makes it move a # patches.
      4. b<#> same but reverse (towards the blue lights)
      5. a <char> <char> <char> – sets speed to left right and delay. allows for specific control.
        • meaning you can have the robot do 4 consecutive right turns by sending “rrrr” or going forward and backwards by sending “f8b3” but a command of “rrllf9” will not carry the forward part.
        • the a for arch allows you to control each motor individually (left, right). the last char is for duration of movement. you can read exact numbers at:   the delay is in miliseconds – and in that time the robot will not perform other actions.

      an example of how to communicate with the robot using python with the pyserial module:
      import serial
      ser = serial.Serial(‘COM9’,57600)


      ser.write(“a00z”) #this zero is not a zero speed but the ASCII zero meaning 48, z is 122.




      ser.write(100) # these 4 commands will also initiate a proper sequence.

at the first stage each group will get one robot fully equipped an XBEE modem with a USB and these commands settings. later during the workshop you will get further commands to allow for grouping the robots or receiving other information from them.

  • those who want to program the robots themselves for personal fittings are welcomed to do so – the robots can be programed using C/C++ Tom and I will be able to help – programming guides:

Good luck.

Lab guide: Ziv Ramati – [email protected]

3pi control

Added here is a python library to control your 3pi robots using serial wireless dongle (zigbee/bluetooth/UHF).

Unlike the first stage this gives you direct control over the robot allowing movement by selecting a speed for each wheel – now the timing factor is on you! make sure your robot is burned with the code for this kind of movement (displaying “Sadna  2.1” or greater on startup).

the speed can be any number x as long as -256<x<256 (notice the no ‘=’). 255 is 1m/s in either direction so calculate your timing.

example of use:

import robo3pi as s
import time
s.init(‘com11’) # this is the com the xbee dongle identified itself on my computer.
s.move(‘1’, 255, -117)
s.addToGroup(‘1’, ‘G’)
s.addToGroup(‘2’, ‘G’)
s.stop(‘G’) # ok so robot #2 never started moving but this will control all the group.


readme file is attached to the installation pack and has a list of commands.

  • Oren Salzman, Michael Hemmer, Barak Raveh, and Dan Halperin.
    Motion Planning via Manifold Samples
    In Proceedings of the 19th Annual European Symposium on Algorithms (ESA), pages 493-505, 2011 [link];  arXiv, 2011 [pdf]
  • Oren Salzman, Michael Hemmer, and Dan Halperin.
    On the Power of Manifold Samples in Exploring Configuration Spaces and the Dimensionality of Narrow Passages
    arXiv, 2012 [pdf]
  • Barak Raveh, Angela Enosh, and Dan Halperin
    A Little More, a Lot Better: Improving Path Quality by a Path Merging Algorithm (H-Graphs)
    IEEE Transactions on Robotics, 27(2): 365-371, 2011 [link] ; arXiv [link]

Additional Reading Material

  • Chapter 7: Sampling-Based Algorithms in Principles of Robot Motion: Theory, Algorithms, and Implementations
    H. Choset, K.M. Lynch, S. Hutchinson, G. Kantor, W. Burgard, L.E. Kavraki, and S. Thrun, The MIT Press, 2005.
  • Chapter 5: Sampling-Based Motion Planning in Planning Algorithms,
    S.M. LaValle, Cambridge University Press, 2006
    Web version
  • Motion Planning via Manifold Samples

Installing CGAL and related programs on Windows operating system

This page explains how to install CGAL 4.9 with Boost 1.59.0 and Qt5 5.7.1 on Windows 8 using Visual Studio 12 (2013) generating 64 bit binaries. The same procedure should apply to earlier versions of Windows.

Note that during the entire setup you need Internet connection!

Note that the installation requires significant disk space. Make sure to free enough disk space before the installation.

Instructions on adding Environment Variables in Windows are at the end.


IMPORTANT: make sure that you install everything in 64-bit.

  1. Visual Studio 12 Professional (recommended) or Express.
    Professional – Students can get a free version here
  2. Boost 1.59.0: here
  3. CMake 3.7.1
  4. CGAL 4.9.0
  5. Qt5 5.7.1
In parentheses are the paths on my computer.
1) Visual C++ 2013:
  •  If you use the offline version, choose to install Visual C++.
  •  Accept the license terms.
  •  You may select a custom installation (instead of full) and select only Visual C++ (unselect other features).
  •  You don’t need to install “SQL Server” & “Silverlight Runtime” although you may.
  •  There is a chance that it is recommended to reboot now.
2) Boost

Versions earlier than 1.55 used to include a binary installer. Such an installer is no longer available. However, the procedure is fairly simple. There are several ways to build boost; the following is referred to as “build from source”.

  • Open a command-line terminal, such as ‘cmd’.
  • Make the compiler known by running the following batch file:
c:\Program Files (x86)\Microsoft Visual Studio 12.0\VC\bin\vcvars32.bat
  • Download the boost archive, unpack it to a target directory of your choice (e.g., C:\boost\boost_1_55_0), and change directory to target directory.
  • Run:
  • Then, build the static, shared, single and multi threaded boost libraries:
.\b2 link=static,shared threading=single,multi variant=debug,release
  • Make sure the subdirectory stage\lib\ has been populated with the compiled libraries (both .lib and .dll files).
3) CMake
  •  -Agree to the license.
  •  Check “Add CMake to the system PATH for all users”.
  • Check “create desktop icon”.
  •  Next, Next, Next.
  •  Finish
4) QT
  •  Agree to the license.
  •  Next, Next, Next, Install.
  •  Add QTDIR variable with the value C:\Qt\5.7.1 to the environment variables (if it’s not already there).
  •  Add <QT>\bin to the system PATH. (C:\Qt\5.7.1\bin)
4.1) QT Visual Studio 2013 addin:
  •  install the addin (Is it still necessary in VC 12?)
  •  CGAL installation will need to connect to the internet for GMP and MPFR.
  •  Be amazed by the splash screen.
  •  Agree to the license.
  •  Just choose the default: with GMP and MPFR, and with examples and demos.
  •  64-bit (for me).
  •  In the “Setting Environment Variables” screen, choose all users and make sure that CGAL_DIR is checked.
  •  Install.
  •  Add <CGAL_DIR >\auxiliary\gmp\lib to the system PATH. (C:\Program Files\CGAL-4.9\auxiliary\gmp\lib)
Now you need to compile CGAL
  •  Open CMake – cmake-gui (on the Desktop) – if you are using win7 make sure you open the program in administrator mode – right click on the icon and click on “run as administrator”.
  •  For both “Where is the source code” and “Where to build the binaries” specify the CGAL Installation folder (C:\Program Files\CGAL-4.9)
  •  Click Configure.
  •  Choose “Visual Studio 12 2013” and click “Finish”
  •  Click Generate
  •  A solution named CGAL was created in the directory.
  •  Compile ALL_BUILD project both in Debug and Release (ignore the compilation error in CGAL_imageIO).
  •  All CGAL libraries should be under the lib directory.
6) Sanity check (optional)
  •  Open CMake (cmake-gui, can be found on the desktop) – (for win7 users, use “Run as admin”)
  •  Choose “Where is the source code:” to be the Triangulation_2 demo directory under the CGAL installation. Namely, <CGAL>/demo/Triangulation_2 (C:\Program Files\CGAL-4.9/demo/Triangulation_2).
  •  Choose “Where to build the binaries:” to the same directory.
  •  Click Configure
  •  Click Generate
  •  Go to the directory (C:\Program Files\CGAL-4.9) and open the solution and compile. Run the Delaunay_triangulation project for check (in debug and release)
7) Customizing env – If you are not using CMake to create new VS projects
 Note: the following operations should be repeated for the Debug and Release modes
  •  Right-click on the selected project and select “Properties”.
   Go to C/C++ -> General . Add the following to “Additional Include Directories” (include)
   * include: <Boost> (C:\Program Files\boost\boost_1_59)
   * include: <CGAL>\include (C:\Program Files\CGAL-4.9\include)
   * include: <CGAL>\auxiliary\gmp\include (C:\Program Files\CGAL-4.9\auxiliary\gmp\include)
   * include: <QT>\include\Qt (C:\Qt\5.7.1\include\Qt)
   * include: <QT>\include\QtCore (C:\Qt\5.7.1\include\QtCore)
   * include: <QT>\include\QtGui (C:\Qt\5.7.1\include\QtGui)
  Go to Linker -> General. Add the following to “Additional Dependencies” (include)
   * library: <CGAL>\lib (C:\Program Files\CGAL-4.9\lib)
   * library: <QT>\lib (C:\Qt\5.7.1\lib)
   * library: <Boost>\lib (C:\Program Files\boost\boost_1_59\lib)
   * library: <CGAL>\auxiliary\gmp\lib (C:\Program Files\CGAL-4.9\auxiliary\gmp\lib)
For a specific project using CGAL you need to ignore the auto-link of gmp and mpfr. The names below are for Debug.
 – Linker -> Input
   * Add libgmp-10.lib and libmpfr-4.lib to “Additional Dependencies”
   * Add gmp-vc100-mt-gd.lib and mpfr-vc100-mt-gd.lib to “Ignore Specific Library” (gmp-vc100-mt.lib and mpfr-vc100-mt.lib in release mode).
For a specific project using QT:
 – Linker -> Input
   * Add qtmaind.lib, QtGuid4.lib, and QtCored4.lib to “Additional Dependencies” (qtmain.lib;QtCore4.lib;QtGui4.lib in release).
Important: In case that the compilation succeeds but the linker is unable to find cgal-related dlls (“the program can’t start because cgal-vc100-mt-gd-4.2.dl is missing”)
find these files in the CGAL directory and copy them either to the system32 folder of windows, or the folder of the visual studio project.
Try to compile this programs:
Hello CGAL:

Setting up PATH variable or other Environment variables on windows systems

    1. From the desktop, right-click My Computer and click properties.
    2. (on Vista/Win7/Win8 click Advanced system settings on the left side)
    3. In the System Properties window, click on the Advanced tab.
    4. In the Advanced section, click the Environment Variables button.
    5. Finally, in the Environment Variables window, highlight the path variable in the Systems Variable section and click edit. Add or modify the path lines with the paths you wish the computer to access. Each different directory is separated with a semicolon as shown below.C:\Program Files;C:\Winnt;C:\Winnt\System32

LegoNXT Robot – ABS Motion Planning Project

 This work is part of a High quality Motion Planning Workshop (spring-2009).

Project Goal:

Generate a path from source to target locations while avoiding obstacles, optimized in some aspects, for our constructed robot to follow.

The project included:

  • Designing and constructing a robot based on Lego (TM) parts with the MindStorm controller.
  • Designing an algorithm that calculates an efficient path according to parameters entered by the user.
  • Creating A second algorithm that converts the output of the first algorithm to a set of instructions for our robot to follow.

Project Milestones

  • Construction – we have decided to construct our robot as a Car-like robot. I.e. Forward motion generated in the front and steering generated in the back. This decision stemmed from our desire to make the project more interesting, compared to common robot construction models. Retrospectively this was proven to be harder than we thought, due to unexplained misbehavior by the robot from time to time (inaccuracies while following instructions).
  • Optimization aspects were selected:
    • Clearance – maximum distance to obstacles
    • Distance – minimal path distance
    • Turns – preference for turns that the robot performs well
  • Implementation & Design
    • Development/Final Environment: Our development environment consisted of “Google Sketchup” for designing the problem to be solved, “OOPSMP” libraries for existing motion planning algorithms and framework, “Microsoft Visual Studio” for coding, and “Bram NXT” library for controlling the robot.
    • Motion planning: Our approach was to use existing algorithms for motion planning, and hybridize their solution paths to a single path optimized for the aspects we selected, while prioritizing these aspects using weights given by the user.
    • Translation to robot control: We decided to address the issue of control by regarding the solution path as a path to follow, rather than a description of the robot’s exact state at a given time (which is the broadly accepted convention).


Video of our robot in action

Non holonomic motion planner project

Authors : Bosmat Eldar, Itamar Berger, Gal Zohar
This project is part of the high-quality motion paths for robots workshop, Spring 2009

Project Goal

Generating high-quality paths for non-holonomic robots.

In 2D worlds, finding a path from point A to point B is an interesting problem which can easily be solved. However, when there are limitations on the robot’s movement, the problem may become difficult. In this project we’ve tried to deal with the limitations of cars’ movement. Cars are non-holonomic, so actions such as parking or making a sharp turn are not always trivial. A typical GPS doesn’t take these limitations into account.

In order to make high quality paths, we’ve defined several criteria for path quality: length, smoothness, clearance and minimizing reverse and steering amount.

The resulting project

We’ve examined the solutions currently suggested for this problem over the net, and a particular article appeared to be a good start for our basic demands –  “Randomized Motion Planning for Car-like Robots with C-PRM” by Guang Song and Nancy M. Amato from Texas A&M University. [PDF]

The C-PRM algorithm provides us the ability to control the preferred paths procedure. We used its basic structure to achieve our quality criteria.

The results were encouraging, however, we thought the Hybrydization method we’d learned in the workshop might improve the path quality. Merging the methods wasn’t trivial. The combination of the paths required some creativity because the C-PRM algorithm wasn’t meant to support it.
This optimization improved the results in every quality measure, but, as expected,
solving the problem multiple times reqiured that much more time.

We noticed that custom optimizations can assist in specific situations,
so we’ve added some more optimizations for situations such as parking and narrow passage.

Finally , the resulting project is a fully GUI standalone application which contains a
“Scene Creator” – a painter program for creating 2d worlds ( and saving as XML files)
made from polygons, and the main program – an impressive non-holonomic motion planner.

Visual results

green: start point
red: end point


The project was written in C++ with support of the following software packages:

  • OOPSMP – we used this package as a basic framework for our project.
    This package contains some useful tools such as Graph structures,
    Search Algorithms, State Sampler ,XML reader, basic GUI etc.
  • PQP – Collision Detector , assisted in finding collision and distance measurements between polygons.
  • Triangulate – used for triangulate the polygons for the collision detector.
  • QT – used for 2D GUI implementation.
  • OpenGL – used to draw the problem.

We’ve implemented and modified the C-PRM algorithm.

More Details regarding implantation can be found in our Developer Guide Manual.


We’ve learned a lot in this workshop. We’ve been introduced to the interesting field of MotionPlanning –
A broad field with much room for innovation.
In this project we were required to deal with a large software package and a lot of self learning.

We were given a great deal of freedom and support throughout the project.
We believe our final results are interesting and perhaps even innovative in the field of car motion planning.


Press here to watch the movie

Yair Oz - Webcreator


Skip to content