doc updates, fix this_side_up and unit test it

Author: joschu

cmake/add_python_unit_tests.cmake

@@ -4,6 +4,7 @@ set(python_test_scripts
   arm_to_cart_target.py
   fullbody_plan.py
   position_base.py
+  this_side_up.py
 )
 
 foreach(pyfile ${python_test_scripts})

doc/Makefile

@@ -49,6 +49,9 @@ all: sphinx doxygen
 doxygen:
 	doxygen Doxyfile
 
+shell:
+	sh generate_shell_output.sh
+
 upload:
 	rsync -azvu --delete --progress  ./ [email protected]:/var/www/trajopt/doc/
 

doc/cmd_output/ctest.txt

@@ -0,0 +1,23 @@
+Test project /home/joschu/build/trajopt
+    Start 1: arm_to_joint_target.py
+1/9 Test #1: arm_to_joint_target.py .............   Passed    1.53 sec
+    Start 2: arm_to_cart_target.py
+2/9 Test #2: arm_to_cart_target.py ..............   Passed    1.78 sec
+    Start 3: fullbody_plan.py
+3/9 Test #3: fullbody_plan.py ...................   Passed    2.63 sec
+    Start 4: position_base.py
+4/9 Test #4: position_base.py ...................   Passed    1.84 sec
+    Start 5: arm_to_cart_target_position_only
+5/9 Test #5: arm_to_cart_target_position_only ...   Passed    1.77 sec
+    Start 6: sco-unit
+6/9 Test #6: sco-unit ...........................   Passed    0.08 sec
+    Start 7: collision-checker-unit
+7/9 Test #7: collision-checker-unit .............   Passed    0.09 sec
+    Start 8: planning-unit
+8/9 Test #8: planning-unit ......................   Passed    1.05 sec
+    Start 9: cast-cost-unit
+9/9 Test #9: cast-cost-unit .....................   Passed    0.09 sec
+
+100% tests passed, 0 tests failed out of 9
+
+Total Test time (real) =  10.90 sec

doc/generate_shell_output.sh

@@ -0,0 +1,6 @@
+#!/bin/sh
+
+curdir=`pwd`
+cd ~/build/trajopt
+ctest > $curdir/cmd_output/ctest.txt
+

doc/install.rst

@@ -9,29 +9,31 @@ This code has been tested on Linux and OSX.
 Dependencies
 ------------
 
-- OpenRAVE
+- OpenRAVE (>= 0.8 required, >= 0.9 highly recommended)
 - OpenSceneGraph
 - CMake
 - boost
 - Eigen
-- Gurobi [#gurobi]_
+- Gurobi [#gurobi]_ (recommended)
+
+.. [#gurobi] On Linux, Trajopt now works with the free solver BPMPD, which is included with the source distribution (as a static library). However, Gurobi is better-tested with Trajopt and usually performs better.
 
-.. [#gurobi] Good news! Trajopt now works with the excellent free solver BPMPD. It's not very well-tested at the moment, but you can use it now by checking out the devel branch and setting TRAJOPT_CONVEX_SOLVER=BPMPD.
 
 Instructions
 -------------
 
-- install OpenRAVE 0.8 or above
-
-.. note:: For best results, install a new version of OpenRAVE (version 0.9).  Due to a recent bugfix, optimization over affine DOFs won't work with 0.8. You can install from source or use the `openrave testing <https://launchpad.net/~openrave/+archive/testing>`_ PPA.
+- install OpenRAVE. To obtain OpenRAVE 0.9, you can install from source or use the `openrave testing <https://launchpad.net/~openrave/+archive/testing>`_ PPA.
 
 
 - install OpenSceneGraph, CMake, boost, and Eigen using your package manager. In Ubuntu, that's::
 
-    sudo apt-get install libopenscene-graph cmake libboost-all-dev libeigen3-dev
+    sudo apt-get install libopenscenegraph-dev cmake libboost-all-dev libeigen3-dev python-numpy 
+
+- Gurobi (optional):
+
+  - download and install Gurobi from `<http://www.gurobi.com>`_. You'll need to make an account and obtain a license. It's free for academic use, and non-academic users can get a free trial.
+  - set the environment variable GUROBI_HOME to point to the folder of your Gurobi directory that contains the folders :file:`include` and :file:`lib`. On my development machines, these are :file:`/home/username/Src/gurobi502/linux64` and :file:`/Library/gurobi501/mac64`.
 
-- download and install Gurobi from `<http://www.gurobi.com>`_. You'll need to make an account and obtain a license. It's free for academic use, and non-academic users can get a free trial.
-- set the environment variable GUROBI_HOME to point to the folder of your Gurobi directory that contains the folders :file:`include` and :file:`lib`. On my development machines, these are :file:`/home/username/Src/gurobi502/linux64` and :file:`/Library/gurobi501/mac64`.
 - Clone trajopt from `github <https://github.com/joschu/trajopt>`_
 - Follow the usual CMake procedure::
 
@@ -55,26 +57,10 @@ You can check if the build worked by typing
 
   ctest
   
-in the build directory. The output should look something like this::
-
-  Running tests...
-  Test project /Users/joschu/build/trajopt-release
-      Start 1: arm_to_cart_target.py
-  1/6 Test #1: arm_to_cart_target.py ............   Passed    2.30 sec
-      Start 2: arm_to_joint_target.py
-  2/6 Test #2: arm_to_joint_target.py ...........   Passed    1.93 sec
-      Start 3: sco-unit
-  3/6 Test #3: sco-unit .........................   Passed    0.04 sec
-      Start 4: collision-checker-unit
-  4/6 Test #4: collision-checker-unit ...........   Passed    0.05 sec
-      Start 5: planning-unit
-  5/6 Test #5: planning-unit ....................   Passed    1.43 sec
-      Start 6: cast-cost-unit
-  6/6 Test #6: cast-cost-unit ...................   Passed    0.05 sec
-
-  100% tests passed, 0 tests failed out of 7
-
-  Total Test time (real) =   8.09 sec
+in the build directory. The output should look something like this:
+
+.. include:: cmd_output/ctest.txt
+   :literal:
 
 If one of the unit tests fails, you can get more diagnostic information by running with ``ctest -V``, or running the test scripts individually. The python executables are in ``SOURCE_DIR/python_examples`` and the compiled c++ executables are in ``BUILD_DIR/bin``. 
 
@@ -94,4 +80,4 @@ Common installation problems
 
 * *All the python tests fail* with an ``ImportError``, because ``trajoptpy`` is not found or ``ctrajoptpy`` is not found. That means your ``PYTHONPATH`` is not set correctly. It should have both the trajopt source directory ``/path/to/trajopt`` and the ``lib`` subdirectory of the build directory, ``/path/to/build_trajopt/lib``.
 
-* *Almost all the tests fail, where OpenRAVE symbols aren't found*. Set ``LD_LIBRARY_PATH=/usr/local/lib`` or whereever libopenrave.so is. (Note: if you know how to fix this problem through RPATH settings or linker flags, please enlighten me.)
+* *Almost all the tests fail, where OpenRAVE symbols aren't found*. Set ``LD_LIBRARY_PATH=/usr/local/lib`` or whereever libopenrave.so is. (Note: if you know how to fix this problem through RPATH settings or linker flags, please enlighten me.)

doc/misc.rst

@@ -11,36 +11,38 @@ Downloading test data
 
 First navigate to the ``bigdata`` directory, and then run the script ``download.py``.
 
+
+.. _collcosts:
+
 Collision costs
 ------------------
 
 There are two types of collision penalty provided: a discrete-time penalty and a continuous-time penalty. The discrete-time penalty is based on the signed distance between the robot's links and the obstacles (including robot links). The continuous-time penalty considers the volume swept-out by the robot's links as it moves from one timestep to the next. See the paper for more details. While the discrete-time penalty will often jump through a thin obstacle, the continuous-time penalty will properly penalize such events. The only downside of the continuous-time penalty is that it currently doesn't check self-collisions. If you want to avoid self-collisions, you can add a discrete-time and continuous-time penalty.
 
-From python, both penalties have the same API. The optional ``first_step`` and ``last_step`` arguments are the first and last timestep (inclusive) for the costs.
+In JSON, you can switch between the types of cost with the ``continuous`` parameter, which is ``True`` by default.
 
 Discrete-time collision:
 
 .. code-block:: python
 
         {
             "type" : "collision",
-            "params" : {"coeffs" : [10],"dist_pen" : [0.025], "first_step":0, "last_step":19}
+            "params" : {"coeffs" : [20],"dist_pen" : [0.025], "first_step":0, "last_step":19, "continuous":False}
         }
 
-TODO: update this
 Continuous-time collision:
 
 .. code-block:: python
 
         {
-            "type" : "continuous_collision",
-            "params" : {"coeffs" : [10],"dist_pen" : [0.025], "first_step":0, "last_step":19}
+            "type" : "collision",
+            "params" : {"coeffs" : [20],"dist_pen" : [0.025], "first_step":0, "last_step":19, "continuous":True}
         }
         
 Finding out about available costs and constraints
 ----------------------------------------------------------------
 
 Some costs and constraints have been implemented but are not currently documented here.
-One way to find out about which ones have been implemented and what parameters are available is to look at the Doxygen documentation for the subclasses of `CostInfo <../../dox_build/structtrajopt_1_1_cost_info.html>`_ and `CntInfo <../../dox_build/structtrajopt_1_1_cnt_info.html>`_ because each item under "costs" or "constraints" in the JSON document is first converted to one of these structs when the JSON file is read.
+One way to find out about which ones have been implemented and what parameters are available is to look at the Doxygen documentation for the subclasses of `TermInfo <../../dox_build/structtrajopt_1_1_term_info.html>`_ because each item under "costs" or "constraints" in the JSON document is first converted to one of these structs when the JSON file is read.
 
 

doc/open_index.sh

@@ -0,0 +1,9 @@
+#!/bin/sh
+
+index=sphinx_build/html/index.html
+if [ `uname` = Linux ] 
+then
+    google-chrome $index
+else 
+    open -a Google\ Chrome  $index
+fi

doc/tutorial.rst

@@ -21,16 +21,17 @@ The optimization will pause at every iteration and plot the current trajectory a
 
 Every problem description contains four sections: basic_info, costs, constraints, and init_info (i.e., initialization info).
 
-Here, there are two cost components: joint-space velocity, and the collision penalty. There is one constraint: the final joint state. 
+Here, there are two cost components: joint-space velocity, and the collision penalty. There is one constraint: the final joint state.
 
 .. note:: Why do we use a collision cost, rather than a constraint? In the sequential convex optimization procedure, constraints get converted into costs--specifically, :math:`\ell_1` penalties (see the paper). So the difference between a constraint and an :math:`\ell_1` cost is that for a constraint, the optimizer checks to see if it is satisfied (to some tolerance), and if not, it jacks up the penalty coefficient. The thing about collisions is that it's often necessary to violate the safety margin, e.g. when you need to contact an object. So you're best off handling the logic yourself of adjusting penalties and safety margins, based on your specific problem.
 
+See :ref:`collcosts` for more info on the collision penalty.
+
 
 Move arm to pose target
 -------------------------
 Full source code:  :file:`python_examples/arm_to_cart_target.py`
 
-
 Next, let's solve the same problem, but instead of specifying the target joint position, we'll specify the target pose.
 
 Now we use a pose constraint instead of a joint constraint:
@@ -48,6 +49,7 @@ Initialize using collision-free IK:
 ...
 
 .. literalinclude:: ../python_examples/arm_to_cart_target.py
+  :language: python
   :start-after: BEGIN init
   :end-before: END init
 
@@ -62,6 +64,49 @@ Whether you use a straight-line initialization, stationary initialization, or so
 You can see this strategy in ``benchmark.py``, which uses four waypoints. Using this strategy, the algorithm solves 100% of 204 planning problems in the three provided scenes (tabletop, bookshelves, and kitchen_counter).
 
 
+All in all, there are several ways to initialize:
+
+- **Stationary**: Initialize with the trajectory that stands still for ``n_steps`` timesteps.
+
+  .. code-block:: python
+
+    "init_info" : {
+        "type" : "stationary"
+    }
+
+- **With a given trajectory**: 
+
+  .. code-block:: python
+
+    "init_info" : {
+        "type" : "given_traj"
+        "data" : [[ 0.   ,  0.   ,  0.   ],
+                   [ 0.111,  0.222,  0.333],
+                   [ 0.222,  0.444,  0.667],
+                   [ 0.333,  0.667,  1.   ],
+                   [ 0.444,  0.889,  1.333],
+                   [ 0.556,  1.111,  1.667],
+                   [ 0.667,  1.333,  2.   ],
+                   [ 0.778,  1.556,  2.333],
+                   [ 0.889,  1.778,  2.667],
+                   [ 1.   ,  2.   ,  3.   ]]
+    }
+    
+  ``data`` must be a 2d array with ``n_steps`` rows, and the first row must be the robot's current DOF values
+    
+- **With a straight line in configuration space**.
+
+  .. code-block:: python
+
+    "init_info" : {
+        "type" : "given_traj"
+        "endpoint" : [ 1.   ,  2.   ,  3.   ]
+    }
+    
+  The robot's DOF values are used for the start point of the line.
+    
+    
+    
 .. _rollingyourown:
 
 Rolling your own costs and constraints in python
@@ -72,7 +117,8 @@ Full source code:  :file:`python_examples/this_side_up.py`
 
 This next script shows how you can easily implement your own costs and constraints. The constraint we consider here says that a certain vector defined in the robot gripper frame must point up (in the world coordinates). For example, one might imagine that the robot is holding a glass of water, and we don't want it to spill.
 
-The basic setup of the problem is similar to the previous examples.
+The basic setup of the problem is similar to the previous examples. Before getting to the python constraint functions, we'll add some constraints to the problem in the json file, including a cartesian velocity constraint.
+
 We set a pose constraint at the end of the trajectory. This time, we ignore the rotation (by setting :code:`rot_coefs = [0,0,0]`).
 
 .. literalinclude:: ../python_examples/this_side_up.py

notes/trajopt-todo.txt

@@ -14,17 +14,15 @@ RobotAndDOF is a mess now
 
 filtermask arguments in collision_terms
 
+fixed length steps: need a custom constraint + projection
+
 Eventually
 ===========
 
-do something about METERS
 
-fix up bpmd interface
 
 boost python arguments
 
-voxel grid collision checking. or at least preprocess voxel grid data.
-
 TrajArray/DblMatrix/MatrixXd mean the same thing
 
 PECost is PE^2

python_examples/arm_to_cart_target.py

@@ -42,7 +42,6 @@
   {
     "type" : "joint_vel", # joint-space velocity cost
     "params": {"coeffs" : [1]} # a list of length one is automatically expanded to a list of length n_dofs
-    # Also valid: "coeffs" : [7,6,5,4,3,2,1]
   },
   {
     "type" : "collision",
@@ -61,8 +60,7 @@
     "params" : {"xyz" : xyz_target, 
                 "wxyz" : quat_target, 
                 "link": "r_gripper_tool_frame",
-                # "timestep" : 9
-                # omitted because timestep = n_steps-1 is default
+                "timestep" : 9
                 }
 
   }

python_examples/arm_to_joint_target.py

@@ -32,7 +32,7 @@
   {
     "type" : "joint_vel", # joint-space velocity cost
     "params": {"coeffs" : [1]} # a list of length one is automatically expanded to a list of length n_dofs
-    # Also valid: "coeffs" : [7,6,5,4,3,2,1]
+    # also valid: [1.9, 2, 3, 4, 5, 5, 4, 3, 2, 1]
   },
   {
     "type" : "collision",

python_examples/this_side_up.py

@@ -67,7 +67,7 @@
     "type" : "cart_vel",
     "name" : "cart_vel",
     "params" : {
-        "distance_limit" : .05,
+        "max_displacement" : .05,
         "first_step" : 0,
         "last_step" : n_steps-1, #inclusive
         "link" : "r_gripper_tool_frame"
@@ -117,4 +117,4 @@ def dfdx(x):
 # END add_constraints
 
 result = trajoptpy.OptimizeProblem(prob) # do optimization
-print result
+assert [viol <= 1e-4 for (_name,viol) in result.GetConstraints()]

src/trajopt/problem_description.cpp

@@ -420,7 +420,7 @@ void CartVelCntInfo::fromJson(const Value& v) {
     PRINT_AND_THROW( boost::format("invalid link name: %s")%linkstr);
   }
 
-  const char* all_fields[] = {"first_step", "last_step", "max_displacement"};
+  const char* all_fields[] = {"first_step", "last_step", "max_displacement","link"};
   ensure_only_members(params, all_fields, sizeof(all_fields)/sizeof(char*));