Merge pull request #49 from kalyanvasudev/cleanup

gpmp2 python bindings and examples

Author: mhmukadam

CHANGELOG

@@ -2,6 +2,22 @@
 Changelog for GPMP2
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.3.0 (2021-08-13)
+------------------
+* Python wrapper with utils and examples
+* Self collision factor
+* Bugfix for theta_bias in Arm.h
+* Contributors: Kalyan Vasudev Alwala, Mustafa Mukadam
+
+0.2.1 (2021-07-21)
+------------------
+* Joint limit factors with examples
+* Optimizer no increase feature
+* More robot models (two arm, w/ mobile base, w/ linear actuator)
+* Workspace i.e. end effector position, orientation, and full pose constraint
+* Updated documentation and some bugfixes
+* Contributors: Jing Dong, Mustafa Mukadam
+
 0.2.0 (2017-06-16)
 ------------------
 * Update installation documentation

CMakeLists.txt

@@ -1,6 +1,8 @@
-cmake_minimum_required(VERSION 2.6)
+cmake_minimum_required(VERSION 3.0)
 enable_testing()
 project(gpmp2 CXX C)
+set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w")
 
 # Mac ONLY. Define Relative Path on Mac OS
 if(NOT DEFINED CMAKE_MACOSX_RPATH)
@@ -9,14 +11,15 @@ endif()
 
 # version indicator
 set(GPMP2_VERSION_MAJOR 0)
-set(GPMP2_VERSION_MINOR 2)
+set(GPMP2_VERSION_MINOR 3)
 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_STATIC_LIBRARY "whether build static library" OFF)
 option(GPMP2_BUILD_MATLAB_TOOLBOX "whether build matlab toolbox, need shared lib" OFF)
+option(GPMP2_BUILD_PYTHON_TOOLBOX "whether build python toolbox, need shared lib" OFF)
 
 if(GPMP2_BUILD_STATIC_LIBRARY AND GPMP2_BUILD_MATLAB_TOOLBOX)
   message(FATAL_ERROR "matlab toolbox needs shared lib")
@@ -71,6 +74,22 @@ if(GPMP2_BUILD_MATLAB_TOOLBOX)
 endif()
 
 
+# Wrapping to Python
+if(GPMP2_BUILD_PYTHON_TOOLBOX)
+  include_directories(${GTSAM_DIR}/cython)
+  include_directories(/usr/local/cython)
+  include(GtsamCythonWrap)
+  include_directories(${GTSAM_EIGENCY_INSTALL_PATH})
+
+  wrap_and_install_library_cython("gpmp2.h"
+                                  "from gtsam.gtsam cimport *" # extra import of gtsam/gtsam.pxd Cython header
+                                  "${CMAKE_INSTALL_PREFIX}/cython" # install path
+                                  gpmp2  # library to link with
+                                  "gtsam"  # dependencies which need to be built before wrapping
+                                  )
+  add_definitions(-DBOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES -DBOOST_OPTIONAL_CONFIG_ALLOW_BINDING_TO_RVALUES)
+endif()
+
 # Install config and export files
 GtsamMakeConfigFile(gpmp2)
 export(TARGETS ${GPMP2_EXPORTED_TARGETS} FILE gpmp2-exports.cmake)

CONTRIBUTING.md

@@ -18,8 +18,9 @@ TODO List
 ------
 More or less in order of priority
 
+- Update to latest GTSAM version
+- Support Python 3.x
 - Load environments from urdf/xml style formats to construct signed distance fields
 - Load robot urdf/xml files and construct forward kinematics for them automatically
-- Basic python wrapper for front end scripting that can replace the matlab wrapper
 - Simple visualizer for environment, robot, trajectory, etc on the python wrapper side
 - Alternate obstacle cost functions like, barrier function

README.md

@@ -1,50 +1,86 @@
 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](http://www.cc.gatech.edu/~bboots3/files/GPMP2.pdf) (RSS 2016). The core library is developed in C++ language, and an optional Matlab toolbox is provided. Examples are provided in Matlab scripts. A ROS interface is also available within [PIPER](https://github.com/gtrll/piper). GPMP2 was started at Georgia Tech Robot Learning Lab, see [THANKS](THANKS.md) for contributors.
+
+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](http://www.cc.gatech.edu/~bboots3/files/GPMP2.pdf) (RSS 2016). The core library is developed in C++ language with an optional Python 2.7 toolbox. GPMP2 was started at the Georgia Tech Robot Learning Lab, see [THANKS](THANKS.md) for contributors.
 
 
 Prerequisites
 ------
 
-- CMake >= 2.6 (Ubuntu: `sudo apt-get install cmake`), compilation configuration tool.
+- CMake >= 3.0 (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://github.com/borglab/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 execute:
-
-```
-$ mkdir build
-$ cd build
-$ cmake ..
-$ make check  # optional, run unit tests
-$ make install
-```
-
-Matlab Toolbox
------
+- [Anaconda2](https://docs.anaconda.com/anaconda/install/linux/), virtual environment needed if installing python toolbox.
+- [GTSAM](https://github.com/borglab/gtsam/tree/wrap-export) == `wrap_export`, a C++ library that implements smoothing and mapping (SAM) framework in robotics and vision. Here we use the factor graph implementations and inference/optimization tools provided by GTSAM.
 
-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. It has been tested on Ubuntu Linux and Windows for now.
+Installation (C++ only)
+------
 
-- Ubuntu: GCC 4.8 - 4.9, 5.3 - 5.4
-- Windows: Visual C++ 2015 (Matlab toolbox not tested)
-- Boost: 1.50 - 1.61
+- Install GTSAM.
+  ```bash
+  git clone https://github.com/borglab/gtsam.git
+  cd gtsam
+  git checkout wrap-export
+  mkdir build && cd build
+  cmake ..
+  make check  # optional, run unit tests
+  sudo make install
+  ```
+- Setup paths.
+  ```bash
+  echo 'export LD_LIBRARY_PATH=/usr/local/lib:${LD_LIBRARY_PATH}' >> ~/.bashrc
+  echo 'export LD_LIBRARY_PATH=/usr/local/share:${LD_LIBRARY_PATH}' >> ~/.bashrc
+  source ~/.bashrc
+  ```
+- Install gpmp2.
+  ```bash
+  git clone https://github.com/gtrll/gpmp2.git
+  cd gpmp2 && mkdir build && cd build
+  cmake ..
+  make check  # optional, run unit tests
+  sudo make install
+  ```
+
+
+Installation (C++ with Python toolbox)
+------
+- Setup virtual environment.
+  ```bash
+  conda create -n gpmp2 pip python=2.7
+  conda activate gpmp2
+  pip install cython numpy scipy matplotlib
+  conda deactivate
+  ```
+- Install GTSAM.
+  ```bash
+  conda activate gpmp2
+  git clone https://github.com/borglab/gtsam.git
+  cd gtsam
+  git checkout wrap-export
+  mkdir build && cd build
+  cmake -DGTSAM_INSTALL_CYTHON_TOOLBOX:=ON ..
+  make check  # optional, run unit tests
+  sudo make install
+  conda deactivate
+  ```
+- Setup paths.
+  ```bash
+  echo 'export LD_LIBRARY_PATH=/usr/local/lib:${LD_LIBRARY_PATH}' >> ~/.bashrc
+  echo 'export LD_LIBRARY_PATH=/usr/local/share:${LD_LIBRARY_PATH}' >> ~/.bashrc
+  echo 'export PYTHONPATH=/usr/local/cython:${PYTHONPATH}' >> ~/.bashrc
+  source ~/.bashrc
+  ```
+- Install gpmp2.
+  ```bash
+  conda activate gpmp2
+  git clone https://github.com/gtrll/gpmp2.git
+  cd gpmp2 && mkdir build && cd build
+  cmake -DGPMP2_BUILD_PYTHON_TOOLBOX:=ON ..
+  make check  # optional, run unit tests
+  sudo make install
+  cd ../gpmp2_python && pip install -e .
+  conda deactivate
+  ```
 
 
 Citing

THANKS.md

@@ -3,3 +3,4 @@ GPMP2 was made possible by the efforts of the following people:
 - [Jing Dong](https://github.com/dongjing3309)
 - [Mustafa Mukadam](https://github.com/mhmukadam)
 - [Xinyan Yan](https://github.com/XinyanGT)
+- [Kalyan Vasudev Alvala](https://github.com/kalyanvasudev)

gpmp2.h

@@ -1,19 +1,35 @@
 // gpmp2 matlab wrapper declarations
 
+/* *   Forward declarations and class definitions for Cython:
+ *     - Need to specify the base class (both this forward class and base 
+          class are declared in an external cython header)
+ *       This is so Cython can generate proper inheritance.
+ *       Example when wrapping a gtsam-based project:
+ *          // forward declarations
+ *          virtual class gtsam::NonlinearFactor
+ *          virtual class gtsam::NoiseModelFactor : gtsam::NonlinearFactor
+ *          // class definition
+ *          #include <MyFactor.h>
+ *          virtual class MyFactor : gtsam::NoiseModelFactor {...};
+ *    - *DO NOT* re-define overriden function already declared in the external (forward-declared) base class
+ *        - This will cause an ambiguity problem in Cython pxd header file*/
+
 // gtsam deceleration
 class gtsam::Vector6;
 class gtsam::Vector3;
 class gtsam::Point3;
 class gtsam::Pose3;
 class gtsam::Point2;
 class gtsam::Pose2;
+//class gtsam::Vector;
 
 class gtsam::GaussianFactorGraph;
 class gtsam::Values;
 virtual class gtsam::noiseModel::Base;
 virtual class gtsam::NonlinearFactor;
 virtual class gtsam::NonlinearFactorGraph;
-virtual class gtsam::NoiseModelFactor;
+virtual class gtsam::NoiseModelFactor : gtsam::NonlinearFactor;
+//virtual class gtsam::NoiseModelFactor;
 
 namespace gpmp2 {
 
@@ -40,11 +56,21 @@ class Pose2Vector {
 ////////////////////////////////////////////////////////////////////////////////
 
 // prior factor
-#include <gpmp2/gp/GaussianProcessPriorLinear.h>
 
+//template<class VALUE1, class VALUE2, class VALUE3, class VALUE4>
+//template<Vector, Vector, Vector, Vector>
+//virtual class gtsam::NoiseModelFactor4: gtsam::NoiseModelFactor;
+
+#include <gpmp2/gp/GaussianProcessPriorLinear.h>
+//template<gtsam::Vector, gtsam::Vector, gtsam::Vector, gtsam::Vector>
 virtual class GaussianProcessPriorLinear : gtsam::NoiseModelFactor {
   GaussianProcessPriorLinear(size_t key1, size_t key2, size_t key3, size_t key4,
       double delta, const gtsam::noiseModel::Base* Qc_model);
+  //Vector evaluateError(Vector pose) const;
+  Vector evaluateError(const Vector& pose1, const Vector& vel1, 
+                    const Vector& pose2, const Vector& vel2);
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 #include <gpmp2/gp/GaussianProcessPriorPose2.h>
@@ -469,6 +495,7 @@ virtual class ObstaclePlanarSDFFactorArm : gtsam::NoiseModelFactor {
       size_t posekey, const gpmp2::ArmModel& arm,
       const gpmp2::PlanarSDF& sdf, double cost_sigma, double epsilon);
   Vector evaluateError(Vector pose) const;
+  void serialize() const;
 };
 
 
@@ -657,6 +684,13 @@ virtual class ObstacleSDFFactorGPPose2MobileVetLin2Arms : gtsam::NoiseModelFacto
       double delta_t, double tau);
 };
 
+// self collision Arm
+#include <gpmp2/obstacle/SelfCollisionArm.h>
+virtual class SelfCollisionArm : gtsam::NoiseModelFactor {
+  SelfCollisionArm(size_t poseKey, const gpmp2::ArmModel& arm, Matrix data);
+  Vector evaluateError(Vector pose) const;
+};
+
 ////////////////////////////////////////////////////////////////////////////////
 // planner
 ////////////////////////////////////////////////////////////////////////////////

gpmp2/gp/GaussianProcessPriorLinear.h

@@ -62,21 +62,21 @@ class GaussianProcessPriorLinear: public gtsam::NoiseModelFactor4<
       boost::optional<gtsam::Matrix&> H3 = boost::none,
       boost::optional<gtsam::Matrix&> H4 = boost::none) const {
 
-    using namespace gtsam;
+    //using namespace gtsam;
 
     // state vector
-    Vector x1 = (Vector(2*dof_) << pose1, vel1).finished();
-    Vector x2 = (Vector(2*dof_) << pose2, vel2).finished();
+    gtsam::Vector x1 = (gtsam::Vector(2*dof_) << pose1, vel1).finished();
+    gtsam::Vector x2 = (gtsam::Vector(2*dof_) << pose2, vel2).finished();
 
     // Jacobians
-    if (H1) *H1 = (Matrix(2*dof_, dof_) << Matrix::Identity(dof_, dof_),
-        Matrix::Zero(dof_, dof_)).finished();
-    if (H2) *H2 = (Matrix(2*dof_, dof_) << delta_t_ * Matrix::Identity(dof_, dof_),
-        Matrix::Identity(dof_, dof_)).finished();
-    if (H3) *H3 = (Matrix(2*dof_, dof_) << -1.0 * Matrix::Identity(dof_, dof_),
-        Matrix::Zero(dof_, dof_)).finished();
-    if (H4) *H4 = (Matrix(2*dof_, dof_) << Matrix::Zero(dof_, dof_),
-        -1.0 * Matrix::Identity(dof_, dof_)).finished();
+    if (H1) *H1 = (gtsam::Matrix(2*dof_, dof_) << gtsam::Matrix::Identity(dof_, dof_),
+        gtsam::Matrix::Zero(dof_, dof_)).finished();
+    if (H2) *H2 = (gtsam::Matrix(2*dof_, dof_) << delta_t_ * gtsam::Matrix::Identity(dof_, dof_),
+        gtsam::Matrix::Identity(dof_, dof_)).finished();
+    if (H3) *H3 = (gtsam::Matrix(2*dof_, dof_) << -1.0 * gtsam::Matrix::Identity(dof_, dof_),
+        gtsam::Matrix::Zero(dof_, dof_)).finished();
+    if (H4) *H4 = (gtsam::Matrix(2*dof_, dof_) << gtsam::Matrix::Zero(dof_, dof_),
+        -1.0 * gtsam::Matrix::Identity(dof_, dof_)).finished();
 
     // transition matrix & error
     return calcPhi(dof_, delta_t_) * x1 - x2;

gpmp2/kinematics/Arm.cpp

@@ -14,14 +14,9 @@ namespace gpmp2 {
 
 /* ************************************************************************** */
 Arm::Arm(size_t dof, const Vector& a, const Vector& alpha, const Vector& d,
-    const gtsam::Pose3& base_pose, boost::optional<const Vector&> theta_bias) :
-    Base(dof, dof), a_(a), alpha_(alpha), d_(d), base_pose_(base_pose) {
-
-  // theta bias
-  if (theta_bias)
-    theta_bias_ = *theta_bias;
-  else
-    theta_bias_ = Vector::Zero(dof);
+    const Pose3& base_pose, const Vector& theta_bias) :
+    Base(dof, dof), a_(a), alpha_(alpha), d_(d), base_pose_(base_pose),
+    theta_bias_(theta_bias) {
 
   // DH transformation for each link, without theta matrix
   // Spong06book, page. 69, eq. (3.10)

gpmp2/kinematics/Arm.h

@@ -44,9 +44,16 @@ class GPMP2_EXPORT 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) :
+    Arm(dof, a, alpha, d, gtsam::Pose3(gtsam::Rot3(), gtsam::Point3(0,0,0)), gtsam::Vector::Zero(dof)) {}
+
+  Arm(size_t dof, const gtsam::Vector& a, const gtsam::Vector& alpha, const gtsam::Vector& d,
+      const gtsam::Pose3& base_pose) :
+    Arm(dof, a, alpha, d, base_pose, gtsam::Vector::Zero(dof)) {}
+
   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);
+      const gtsam::Pose3& base_pose, const gtsam::Vector& theta_bias);
+
 
   /// Default destructor
   virtual ~Arm() {}