From OpenRAVE

by Rosen Diankov

Running the Example

python:

 openrave.py --example constraintplanning

program options/description:

 openrave.py --example constraintplanning --help

Description

Example shows how to use simple gradient-based jacobians to constrain the motion of the robot while planning. A good introduction to these methods can be found in:

• M. Stilman. Task constrained motion planning in robot joint space. In: Proceedings of the IEEE International Conference on Intelligent Robots and Systems (IROS), 2007.

A GripperJacobianConstrains class is defined in the rmanipulation plugin. It holds a RetractionConstraint function that takes in a robot configuration, and constrains the manipulator to lie in a certain manifold specified by a target frame and the degrees of freedom to constraint (translation and rotation about axes). If the projection succeeded, it returns true along with the new configuration. Such functions can be set to any planner at any time by filling the PlannerBase::PlannerParameters::_constraintfn field. In the example above, the constraint function is set inside basemanipulation.h in the following way:

PlannerBase::PlannerParametersPtr params(new PlannerBase::PlannerParameters());
 
// ...
// other params initialization like distance metrics (_distmetricfn)
// ...
 
// constrained params initialization
Transform tConstraintTargetWorldFrame; // target frame in world coordinates
RobotBase::ManipulatorPtr manip = robot->GetActiveManipulator(); // manipulator
boost::array<double,6> vconstraintfreedoms = {{1,1,0,0,0,0}}; // rotx, roty, rotz, transx, transy, transz
double constrainterrorthresh = 0.02; // threshold
// create the class
boost::shared_ptr<CM::GripperJacobianConstrains<double> > pconstraints(new CM::GripperJacobianConstrains<double>(manip,tConstraintTargetWorldFrame,vconstraintfreedoms,constrainterrorthresh));
 
// set the distance metric used from the one already defined in params
pconstraints->_distmetricfn = params->_distmetricfn;
 
// set the constraint function
params->_constraintfn = boost::bind(&CM::GripperJacobianConstrains<double>::RetractionConstraint,pconstraints,_1,_2,_3);