initial commit

.gitignore

@@ -0,0 +1,3 @@
+*pyc
+*dat
+*~

CHANGELOG

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

CMakeLists.txt

@@ -0,0 +1,6 @@
+# To use this file, create CMakeLists:
+#   - rosbuild.cmake : the CMakeLists used for ROS rosbuild builds
+
+cmake_minimum_required(VERSION 2.8.3)
+project(orgpmp2)
+include(${PROJECT_SOURCE_DIR}/catkin.cmake)

Makefile

@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk

README.md

@@ -0,0 +1,71 @@
+orgpmp2
+===================================================
+orgpmp2 is a OpenRAVE plugin for GPMP2 (Gaussian Process Motion Planner 2) algorithm described in Motion Planning as Probabilistic Inference using Gaussian Processes and Factor Graphs (RSS 2016). Examples provided for WAM arm and PR2 use python scripts.
+
+orgpmp2 is being developed by [Mustafa Mukadam](mailto:mmukadam3@gatech.edu) and [Jing Dong](mailto:thu.dongjing@gmail.com) as part of their work at Georgia Tech Robot Learning Lab and is a modified version of [CHOMP openrave plugin](https://github.com/personalrobotics/or_cdchomp).
+
+Compilation & Installation
+------
+
+- Install [ROS](http://wiki.ros.org/indigo/Installation/Ubuntu). We have tested ROS indigo.
+- Install [OpenRAVE](http://openrave.org/). If you have trouble compiling the original version, please check out and install [our fork](https://github.com/gtrll/openrave) which has fixed few minor bugs.
+- Install [GPMP2](https://github.com/gtrll/gpmp2) core C++ library.
+- Install a few addtional dependencies
+
+    ```bash
+    sudo apt-get install gfortran libgsl0-dev python-enum
+    ```
+
+- Initialize a catkin workspace (if you use a existing catkin workspace this step is not needed)
+
+    ```bash
+    mkdir -p ~/gpmp2_catkin_ws/src
+    cd gpmp2_catkin_ws/src
+    catkin_init_workspace
+    ```
+
+- Add orgpmp2, [PrPy](https://github.com/personalrobotics/prpy) (python library for OpenRAVE),  [openrave_catkin](https://github.com/personalrobotics/openrave_catkin) (utility package for OpenRAVE) to ```gpmp2_catkin_ws/src```
+
+    ```bash
+    git clone https://github.com/gtrll/orgpmp2.git
+    git clone https://github.com/personalrobotics/openrave_catkin.git
+    git clone https://github.com/personalrobotics/prpy.git
+    ```
+
+- Compile the catkin workspace
+
+    ```bash
+    catkin_make -DCMAKE_BUILD_TYPE=Release
+    ```
+
+- Before running the examples, the last step is setup the environment variables
+
+    ```bash
+    source ~/gpmp2_catkin_ws/devel/setup.bash
+    ```
+
+Questions & Bug reporting
+-----
+
+Please use Github issue tracker to report bugs. For other questions please contact [Mustafa Mukadam](mailto:mmukadam3@gatech.edu) or [Jing Dong](mailto:thu.dongjing@gmail.com).
+
+
+Citing
+-----
+
+If you use orgpmp2 or 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
+-----
+
+orgpmp2 is released under the GPL license, reproduced in the file license-gpl.txt in this directory.
+

catkin.cmake

@@ -0,0 +1,44 @@
+cmake_minimum_required(VERSION 2.8.3)
+
+# catkin
+find_package(catkin REQUIRED COMPONENTS openrave_catkin)
+catkin_package()
+catkin_python_setup()
+
+# C++11 flags for GTSAM 4.0
+set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
+
+# find OpenRAVE
+find_package(OpenRAVE REQUIRED)
+include_directories(${catkin_INCLUDE_DIRS})
+
+# find GTSAM
+find_package(GTSAM REQUIRED)
+include_directories(${GTSAM_INCLUDE_DIR})
+set(GTSAM_LIBRARIES gtsam)
+
+# find GPMP2
+find_package(gpmp2 REQUIRED)
+include_directories(${gpmp2_INCLUDE_DIR})
+set(gpmp2_LIBRARIES gpmp2)
+
+openrave_plugin("${PROJECT_NAME}_plugin"
+    src/orgpmp2.cpp
+    src/orgpmp2_mod.cpp
+    src/orgpmp2_kdata.cpp
+    src/orcwrap.cpp
+    src/utils/grid.c
+    src/utils/grid_flood.c
+    src/utils/kin.c
+    src/utils/mat.c
+    src/utils/util_shparse.c
+)
+target_link_libraries("${PROJECT_NAME}_plugin"
+    blas
+    lapacke
+    lapack
+    gsl
+    ${GTSAM_LIBRARIES}
+    ${gpmp2_LIBRARIES}
+    ${catkin_LIBRARIES}
+)

doc/Parameters.md

@@ -0,0 +1,32 @@
+Parameters
+=========================================
+Here we describe parameters associated with orgpmp2 and explain how to set them.
+
+
+Signed Distance Field Parameters
+-----
+- **centroid**: This is the centroid of the 3D SDF cube. Set this as the base of the robot or arm being planned for.
+- **extents**: This is the extent of the 3D SDF cube from the centroid in +/- x, y, and z directions. 
+- **res**: This is the resolution parameter and defines the cell size of the SDF cube. For environments with small obstacles pick a finer resolution.
+
+Note that SDF computation takes longer with smaller cell size and larger extents. All dimensions are in meters.
+
+
+GPMP2 parameters
+-----
+More information on parameters associated with the GPMP2 algorithm can be found in [GPMP2's doc](https://github.com/gtrll/gpmp2/doc/Parameters.md).
+
+- **robot**: openrave robot object.
+- **end_conf**: goal in configuration space.
+- **base_pose**: pose for base of the robot or arm.
+- **dh_a**, **dh_alpha**, **dh_d**, **dh_theta**: DH parameters of the robot.
+- **robot_idx** and **link_idx**: when planning for only part of the robot, for example, PR2's arm, spheres need to be matched correctly to the GPMP2 robot model. A mapping between the true index of the link on the robot (**robot_idx**) and its index in the GPMP2 model (**link_idx**) has to be provided.  
+- **starttraj** (optional): initialization of the trajectory. Straight line used if not passed.
+- **total_step**: total states to be optimized on the trajectory.
+- **obs_check_inter**: number of states between any two optimized states where obstacle cost is evaluated.
+- **output_inter**: number of states between any two optimized states for the output trajectory.
+- **total_time**: Total runtime of the trajectory in seconds.
+- **fix_pose_sigma** and **fix_vel_sigma**: pose/velocity prior model covariance.
+- **cost_sigma**: \sigma_obs for obstacle cost that controls the balance between 'smoothness' of the trajectory and 'obstacle avoidance'. Smaller \sigma_obs refers to less smooth trajectory, with lower probability of colliding with obstacles and vice versa.
+- **hinge_loss_eps**: \epsilon for hinge loss function, the 'safety distance' from obstacles.
+- **Qc**: GP hyperparameter.

doc/Usage.md

@@ -0,0 +1,74 @@
+Usage
+=============
+Before using the python script make sure all the dependencies and the plugin are installed correctly. The example python scripts are located in the [examples](../examples) folder and the robot and environment files are located in the [data](../examples/data) folder. WAM is setup for the lab environment and PR2 works with several environments. For convenience, settings based on environments for PR2 are accessed through [problemsets.py](../examples/data/problemsets.py).
+
+
+Initialize module
+-----
+Once openrave is initialized with appropriate environment and robot xml files set the active DOF of the robot and their **start_conf**. Then create an object of orgpmp2
+
+```python
+m_gpmp2 = RaveCreateModule(e,'orgpmp2')
+orgpmp2.orgpmp2.bind(m_gpmp2)
+```
+
+
+Signed Distance Field
+-----
+Signed distance field is calculated by the ```computedistancefield``` function. First disable the part of the robot being planned for and then compute the SDF. For example, [WAM](../examples/gpmp2_wam.py) is disabled completely but [PR2 example](../examples/gpmp2_pr2.py) plans for it's right arm so only that is disabled.
+
+```python
+# Compute distance field for the env
+# first disable robot
+r.Enable(False)
+# get a distance field for env
+m_gpmp2.computedistancefield(cache_filename='sdf_env_lab.dat',
+  centroid=numpy.array([0,0,0]),extents=numpy.array([1.2,1.2,1.2]),res=0.02)
+# enable robot
+r.Enable(True)
+```
+See [Parameters](Parameters.md) for their description and how to set them.
+
+
+Initializing trajectory
+-----
+**starttraj** is used to pass an initialization of the trajectory if available. If nothing is passed a straight line initialization is used by default. The trajectory can be initialized as follows
+
+```python
+st = RaveCreateTrajectory(e,'')
+st.Init(r.GetActiveConfigurationSpecification())
+st.Insert(0,init_p) #position
+st.Insert(total_step+1,init_v) # velocity
+```
+See initializing with trajectory [example](../examples/gpmp2_wam_withtraj.py) on WAM for more details.
+
+
+Running GPMP2
+-----
+After setting up the environment and calculating the SDF the GPMP2 algorithm can be run
+
+```python
+# Run gpmp2
+try:
+  t = m_gpmp2.rungpmp2(...parameters go here...)
+except RuntimeError as ex:
+  print ex
+  t = None
+```
+See [Parameters](Parameters.md) for their description and how to set them.
+
+
+Displaying optimized trajectory
+-----
+Once a solution is found it can be displayed as follows
+
+```python
+# Run optimized trajectory
+try:
+  while t is not None:
+    raw_input('Press [Enter] to run the trajectory ...\n')
+    with e:
+      r.GetController().SetPath(t)
+except KeyboardInterrupt:
+  print
+```

doc/index.md

@@ -0,0 +1,7 @@
+Documentation
+=============
+
+This is documentation for orgpmp2 that is a OpenRAVE plugin for the [GPMP2](https://github.com/gtrll/gpmp2) algorithm. This plugin comes with a python wrapper and its usage is described here using examples on WAM arm and PR2 robot. For a detailed description about the algorithm see [GPMP2 docs](https://github.com/gtrll/gpmp2/doc/index.md).
+
+- [Usage](Usage.md)
+- [Parameters](Parameters.md)

examples/data/envs/bookshelves.env.xml

@@ -0,0 +1,56 @@
+<Environment>
+	<KinBody name="bookshelf_bottom">
+		<Body type="static">
+			<Geom type="box">
+				<extents>0.3 0.55 0.01</extents>
+				<translation>0.95 0 0.20</translation>
+				<quat>1 0 0 0</quat>
+			</Geom>
+		</Body>
+	</KinBody>
+	<KinBody name="bookshelf_shelf1">
+		<Body type="static">
+			<Geom type="box">
+				<extents>0.3 0.55 0.01</extents>
+				<translation>0.95 0 0.60</translation>
+				<quat>1 0 0 0</quat>
+			</Geom>
+		</Body>
+	</KinBody>
+	<KinBody name="bookshelf_shelf2">
+		<Body type="static">
+			<Geom type="box">
+				<extents>0.3 0.55 0.01</extents>
+				<translation>0.95 0 1.00</translation>
+				<quat>1 0 0 0</quat>
+			</Geom>
+		</Body>
+	</KinBody>
+	<KinBody name="bookshelf_top">
+		<Body type="static">
+			<Geom type="box">
+				<extents>0.3 0.55 0.01</extents>
+				<translation>0.95 0 1.40</translation>
+				<quat>1 0 0 0</quat>
+			</Geom>
+		</Body>
+	</KinBody>
+	<KinBody name="bookshelf_left_wall">
+		<Body type="static">
+			<Geom type="box">
+				<extents>0.3 0.01 0.7</extents>
+				<translation>0.95 0.55 0.7</translation>
+				<quat>1 0 0 0</quat>
+			</Geom>
+		</Body>
+	</KinBody>
+	<KinBody name="bookshelf_right_wall">
+		<Body type="static">
+			<Geom type="box">
+				<extents>0.3 0.01 0.7</extents>
+				<translation>0.95 -0.55 0.7</translation>
+				<quat>1 0 0 0</quat>
+			</Geom>
+		</Body>
+	</KinBody>
+</Environment>

examples/data/envs/cabinet.xml

@@ -0,0 +1,9 @@
+<KinBody name="cabinet">
+  <Body type="static" name="cabinet">
+    <Geom type="trimesh">
+      <Render>stl/cabinet.stl</Render>
+      <Data>stl/cabinet.stl</Data>
+      <diffuseColor>0.4 0.4 0.2</diffuseColor>
+    </Geom>
+  </Body>
+</KinBody>