initial commit

.gitignore

@@ -0,0 +1 @@
+build*

CHANGELOG

@@ -0,0 +1,8 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for GPMP2
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.1.0 (2016-06-16)
+------------------
+* Initial release
+* Contributors: Jing Dong, Mustafa Mukadam

CMakeLists.txt

@@ -0,0 +1,65 @@
+cmake_minimum_required(VERSION 2.6)
+enable_testing()
+project(gpmp2 CXX C)
+
+# Mac ONLY. Define Relative Path on Mac OS
+if(NOT DEFINED CMAKE_MACOSX_RPATH)
+  set(CMAKE_MACOSX_RPATH 0)
+endif()
+
+# version indicator
+set(GPMP2_VERSION_MAJOR 0)
+set(GPMP2_VERSION_MINOR 1)
+set(GPMP2_VERSION_PATCH 0)
+set(GPMP2_VERSION_STRING "${GPMP2_VERSION_MAJOR}.${GPMP2_VERSION_MINOR}.${GPMP2_VERSION_PATCH}")
+
+
+# option: whether turn on Matlab toolbox
+option(GPMP2_BUILD_MATLAB_TOOLBOX "whether build matlab toolbox" OFF)
+
+
+# Find GTSAM components
+find_package(GTSAM REQUIRED) # Uses installed package
+include_directories(${GTSAM_INCLUDE_DIR})
+set(GTSAM_LIBRARIES gtsam)   # TODO: automatic search libs
+
+find_package(GTSAMCMakeTools)
+include(GtsamMakeConfigFile)
+include(GtsamBuildTypes)
+include(GtsamTesting)
+
+
+# for unittest scripts
+set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${GTSAM_DIR}/../GTSAMCMakeTools")
+
+# Boost - same requirement as gtsam
+find_package(Boost 1.50 REQUIRED)
+find_package(Boost COMPONENTS filesystem REQUIRED)
+find_package(Boost COMPONENTS system REQUIRED)
+find_package(Boost COMPONENTS thread REQUIRED)
+find_package(Boost COMPONENTS serialization REQUIRED)
+
+include_directories(${Boost_INCLUDE_DIR})
+
+
+# include current source folder, at the very beginning
+include_directories(BEFORE ${CMAKE_CURRENT_SOURCE_DIR})
+
+# Process source subdirs
+add_subdirectory(gpmp2)
+
+
+# Wrapping to MATLAB
+if(GPMP2_BUILD_MATLAB_TOOLBOX)
+  # wrap
+  include(GtsamMatlabWrap)
+  wrap_and_install_library(gpmp2.h ${PROJECT_NAME} "${CMAKE_CURRENT_SOURCE_DIR}" "")
+
+  # install matlab functions and scripts
+  add_subdirectory(matlab)
+endif()
+
+# Install config and export files
+GtsamMakeConfigFile(gpmp2)
+export(TARGETS ${gpmp2_EXPORTED_TARGETS} FILE gpmp2-exports.cmake)
+

LICENSE

@@ -0,0 +1,13 @@
+Copyright (c) 2016, Georgia Tech Research Corporation
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
+
+3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

README.md

@@ -0,0 +1,78 @@
+GPMP2
+===================================================
+This library is an implementation of GPMP2 (Gaussian Process Motion Planner 2) algorithm described in Motion Planning
+as Probabilistic Inference using Gaussian Processes and Factor Graphs (RSS 2016). The core library is developed in C++
+language, and an optional Matlab toolbox is provided. Examples are provided in Matlab scripts.
+An OpenRAVE plugin, [orgpmp2](https://github.com/gtrll/gpmp2_orplugin), is also avaible with examples.
+
+GPMP2 is being developed by [Jing Dong](mailto:[email protected]) and 
+[Mustafa Mukadam](mailto:[email protected]) as part of their work at Georgia Tech Robot Learning Lab. 
+
+Prerequisites
+------
+
+- CMake >= 2.6 (Ubuntu: `sudo apt-get install cmake`), compilation configuration tool.
+- [Boost](http://www.boost.org/) >= 1.50 (Ubuntu: `sudo apt-get install libboost-all-dev`), portable C++ source libraries.
+- [GTSAM](https://bitbucket.org/gtborg/gtsam) >= 4.0 alpha, a C++ library that implement smoothing and mapping (SAM) framework in robotics and vision.
+Here we use factor graph implementations and inference/optimization tools provided by GTSAM.
+
+Compilation & Installation
+------
+
+In the library folder excute:
+
+```
+$ mkdir build
+$ cd build
+$ cmake ..
+$ make check  # optonal, run unit tests
+$ make install
+```
+
+Matlab Toolbox
+-----
+
+An optional Matlab toolbox is provided to use our library in Matlab. To enable Matlab toolbox during compilation:
+
+```
+$ cmake -DGPMP2_BUILD_MATLAB_TOOLBOX:OPTION=ON -DGTSAM_TOOLBOX_INSTALL_PATH:PATH=/path/install/toolbox ..
+$ make install
+```
+
+After you install the Matlab toolbox, don't forget to add `/path/install/toolbox` to your Matlab path.
+
+Tested Compatibility
+-----
+
+The gpmp2 library is designed to be cross-platform, however it's only tested on Ubuntu Linux for now.
+
+- Compilers: GCC 4.8, 4.9, 5.3
+- Boost version: 1.50 - 1.60
+
+
+Questions & Bug reporting
+-----
+
+Please use Github issue tracker to report bugs. For other questions please contact [Jing Dong](mailto:[email protected])
+or [Mustafa Mukadam](mailto:[email protected]) .
+
+
+Citing
+-----
+
+If you use GPMP2 in an academic context, please cite following publications:
+
+```
+@inproceedings{Dong-RSS-16,
+   Author = "Jing Dong and Mustafa Mukadam and Frank Dellaert and Byron Boots",
+   booktitle = {Proceedings of Robotics: Science and Systems (RSS-2016)},
+   Title = "Motion Planning as Probabilistic Inference using Gaussian Processes and Factor Graphs",
+   year = {2016}
+}
+```
+
+
+License
+-----
+
+GPMP2 is released under the BSD license, reproduced in the file LICENSE in this directory.

doc/CplusplusDevelopment.md

@@ -0,0 +1,100 @@
+C++ development guide
+=========================================
+Although all examples are given in Matlab scripts, all the core APIs are developed in C++, 
+and it's easy to develop your C++ examples or use GPMP2 in your C++ projects. 
+Here we briefly explain how to use GPMP2 in a C++ project.
+
+Link to your own projects
+-----
+
+We provide easy linking to external *CMake* projects. Add the following lines to your CMakeLists.txt
+
+```cmake
+find_package(gpmp2 REQUIRED)
+include_directories(${gpmp2_INCLUDE_DIR})
+```
+
+and link your targets to gpmp2
+
+```cmake
+target_link_libraries(${YOUR_BUILD_TARGET} gpmp2)
+```
+
+It's also OK to directly add header files and shared library path to your compiler's flags, if you are not using CMake.
+
+C++ source code structure
+-----
+
+All the C++ source code is located in [gpmp2](../gpmp2/) folder. The source files are divided in several sub folders, 
+based on their functionalities.
+
+- **gp** contains all Gaussian Process related classes and functions that include GP prior factors, and GP interpolation.
+- **kinematics** contains all robot model related classes and functions that include forward kinematics models, and sphere-based robot physical models.
+- **obstacle** contains all obstacle related classes and functions that include signed distance fields, obstacle cost/likelihood functions, and obstacle avoidance factors.
+- **planner** contains all integrated GPMP2 motion planners that include batch planners and iSAM2 replanners.
+- **utils** contains some utility functions.
+
+You can explore the header files in sub folders to further learn the interfaces.
+
+Build your own robot arm
+-----
+
+If you want to build your own robot arm model in C++, there are two steps needed: 
+first is to build an *abstract* robot arm, which contains only forward kinematics information, 
+second is to build a *physical* robot arm, which contains forward kinematics information plus physical body information.
+
+To construct an abstract robot arm, call [Arm](../gpmp2/kinematics/Arm.h) class constructor:
+
+```cpp
+class Arm : public ForwardKinematics<gtsam::Vector, gtsam::Vector> {
+  ...
+  /// Contructor take in number of joints for the arm, its DH parameters
+  /// the base pose (default zero pose), and theta bias (default zero)
+  Arm(size_t dof, const gtsam::Vector& a, const gtsam::Vector& alpha, const gtsam::Vector& d,
+      const gtsam::Pose3& base_pose = gtsam::Pose3(gtsam::Rot3(), gtsam::Point3(0,0,0)),
+      boost::optional<const gtsam::Vector&> theta_bias = boost::none);
+  ...
+```
+
+The constructor takes degree of freedom, DH parameters (a, \alpha, d, and optional \theta bias), 
+and optional arm base pose (default zero), builds an abstract arm object. 
+
+In GPMP2, the physical robot information is stored by a series of spheres, in [BodySphereVector](../gpmp2/kinematics/RobotModel.h) class. 
+
+```cpp
+/// vector of body sphere, typedef here to wrap in matlab
+typedef std::vector<BodySphere> BodySphereVector;
+```
+
+Each sphere in [BodySphere](../gpmp2/kinematics/RobotModel.h) needs a radius, attached 
+link index (0 - nr_link-1), and relative position to the link.
+
+```cpp
+/// body sphere class, each one indicate a body sphere
+struct BodySphere {
+  size_t link_id;       // attched link id, 0 - nr_link-1
+  double radius;        // sphere radius
+  gtsam::Point3 center;        // sphere center position to the link base
+  // constructor
+  BodySphere(size_t id, double r, const gtsam::Point3& c) : link_id(id), radius(r), center(c) {}
+  ...
+```
+Once you initialize the sphere information in [BodySphereVector](../gpmp2/kinematics/RobotModel.h),
+you can construct an [ArmModel](../gpmp2/kinematics/ArmModel.h) class, which is a templated version of 
+[RobotModel](../gpmp2/kinematics/RobotModel.h), with the forward kinematics [Arm](../gpmp2/kinematics/Arm.h)
+and physical model information [BodySphereVector](../gpmp2/kinematics/RobotModel.h).
+
+Matlab toolbox wrapping
+-----
+
+All the code APIs should be first developed in C++, and then wrapped in Matlab.
+All the C++ classes and functions need to be wrapped in Matlab and are declared in [gpmp2.h](../gpmp2.h) header,
+and GPMP2 uses GTSAM matlab wrapper to automatically generate .mex library and .m files for all C++ interfaces wrapped.
+
+If you would like to wrap your own C++ interfaces (classes and functions) in Matlab,
+just put the declarations in [gpmp2.h](../gpmp2.h) header file, and to import the Matlab toolbox
+add the following line at the beginning of Matlab script before using it.
+
+```matlab
+import gpmp2.*
+```

doc/ExampleMatlab2D.md

@@ -0,0 +1,123 @@
+Example: A simple 2D Matlab example 
+===================================================
+We start with a very simple Matlab example, with a 3-link planer arm avoiding obstacles. 
+Make sure you have properly installed gpmp2 with Matlab toolbox, and added the toolbox path to Matlab paths.
+The full example is available at [matlab/Arm3PlannerExample.m](../matlab/Arm3PlannerExample.m).
+
+Dataset
+-----
+The generate2Ddataset utility function will generate a dataset object that includes the ground truth 2D occupancy grid, 
+cell size of the occupancy grid, and the occupancy grid origin (in gtsam.Point2 object) in world coordinate frame.
+
+```matlab
+%% small dataset
+dataset = generate2Ddataset('TwoObstaclesDataset');
+```
+
+Signed Distance Field
+-----
+After generating datasets, the signed distance field can be calculated from the ground truth occupancy grid.
+
+In 2D cases, the signed distance field is just a matrix. Matrix column index represents X direction, 
+and row index represents Y direction.
+The origin is the position of cell (1,1) in the matrix.
+A [PlanarSDF](../gpmp2/obstacle/PlanarSDF.h) object which stores the signed distance field is initialized as follows:
+
+```matlab
+% signed distance field
+field = signedDistanceField2D(dataset.map, cell_size);
+sdf = PlanarSDF(origin_point2, cell_size, field);
+```
+
+Robot Arm
+-----
+A simple 3-link arm in an ArmModel object can be generated from Matlab generateArm utility function for convenience:
+
+```matlab
+% arm model
+arm = generateArm('SimpleThreeLinksArm');
+```
+
+[ArmModel](../gpmp2/kinematics/ArmModel.h) class consists of a forward kinematics model (DH parameter arm implemented by [Arm](../gpmp2/kinematics/Arm.h) class) 
+and a physical arm represented by a series of spheres (implemented by [BodySphereVector](../gpmp2/kinematics/RobotModel.h) class). 
+Details can be found in [generateArm.m](../matlab/+gpmp2/generateArm.m).
+
+We can now visualize the dataset setting and start/goal configurations in Matlab
+
+|**Figure 1:** WAM arm dataset visualized in Matlab, with start (blue) and end (red) configuration.|
+|:-----------|
+|![Matlab scene](pics/3link_setting.png)|
+
+Initialize the Trajectory
+-----
+
+To start the iterative non-linear least square optimization, we need to give an initial trajectory.
+Here we initialize the trajectory as straight line in configuration space. The initial trajectory should be stored in a gtsam.Values object. 
+GPMP2 provides a utility function [initArmTrajStraightLine](../gpmp2/planner/TrajUtils.h) to initialize straight line given start/end configuration, 
+and total number of states/waypoints for optimization.
+
+```matlab
+%% init values
+init_values = initArmTrajStraightLine(start_conf, end_conf, total_time_step);
+```
+
+Parameters
+-----
+All the trajectory optimization related parameters are stored in [TrajOptimizerSetting](../gpmp2/planner/TrajOptimizerSetting.h) class. 
+Please check [parameters](Parameters.md) page for details on tuning/setting these parameters. 
+
+```matlab
+%% optimization settings
+opt_setting = TrajOptimizerSetting(3);          % initialization with DOF
+opt_setting.set_total_step(total_time_step);    % total number of states/waypoints for optimization
+opt_setting.set_total_time(total_time_sec);     % total runtime (second) of the trajectory
+opt_setting.set_epsilon(epsilon_dist);          % \epsilon for hingeloss function, see the paper
+opt_setting.set_cost_sigma(cost_sigma);         % \sigma_obs for obstacle cost, see the paper
+opt_setting.set_obs_check_inter(check_inter);   % number of waypoints are checked by GP interpolation, 0 means GP interpolation is not used
+opt_setting.set_conf_prior_model(pose_fix_sigma); % start/end pose prior sigma, leave as default (0.0001) for most cases
+opt_setting.set_vel_prior_model(vel_fix_sigma); % start/end velocity prior sigma, leave as default (0.0001) for most cases
+opt_setting.set_Qc_model(Qc);         % GP hyperparameter
+opt_setting.setGaussNewton();         % optimization method
+```
+
+Optimization
+-----
+Factor graph generation and optimization are performed in [BatchTrajOptimize2DArm](../gpmp2/planner/BatchTrajOptimizer.h) function. The BatchTrajOptimize2DArm needs input of arm model, signed distance field, start/end configuration+velocity, 
+initial trajectory values, and parameters.
+
+```matlab
+% optimize!
+result = BatchTrajOptimize2DArm(arm, sdf, start_conf, start_vel, end_conf, end_vel, init_values, opt_setting);
+```
+
+Densify the Trajectory by GP Interpolation (Optional)
+-----
+If you need dense trajectory for robot execution, you can densify the trajectory by GP Interpolation. 
+GPMP2 provides a utility function [interpolateArmTraj](../gpmp2/planner/TrajUtils.h) to densify the trajectory using GP Interpolation and by providing optimized values, GP hyperparameter, time interval between states, and number of points you would like to interpolate between each two states (0 means not interpolation).
+
+```matlab
+plot_values = interpolateArmTraj(result, Qc_model, delta_t, plot_inter-1);
+```
+
+Display
+-----
+Once optimized trajectory values are available, we can query the configuration and plot the arm. 
+The configuration vector can be queried from a gtsam.Values object by ```plot_values.atVector(symbol('x', i));```,
+where ```i``` is the index of the state you query (starting from 0 and ending at ```opt_setting.total_step```). 
+Then [plotPlanarArm](../matlab/+gpmp2/plotPlanarArm.m) is used to plot the arm
+
+```matlab
+% plot arm
+conf = plot_values.atVector(symbol('x', i));
+plotPlanarArm(arm.fk_model(), conf, 'b', 2);
+```
+
+Here's the output results of non-densified and densified trajectory
+
+|**Figure 2:** Non-densified trajectory for 2D 3-link arm example.|
+|:-----------|
+|![2D 3-link arm non-densified](pics/2d_3link_arm_nondense.gif)|
+
+|**Figure 3:** Densified trajectory for 2D 3-link arm example.|
+|:-----------|
+|![2D 3-link arm densified](pics/2d_3link_arm_dense.gif)|