OpenRAVE Documentation

Source code for openravepy.examples.constraintplanning

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# Copyright (C) 2009-2012 Rosen Diankov (rosen.diankov@gmail.com)
# 
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#     http://www.apache.org/licenses/LICENSE-2.0
# 
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
"""Shows how to use simple gradient-based jacobians to constrain the motion of the robot while planning.

.. examplepre-block:: constraintplanning

Description
-----------

A good introduction to these methods can be found in [1]_.

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: 

.. code-block:: cpp

  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);

.. examplepost-block:: constraintplanning

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

"""
from __future__ import with_statement # for python 2.5
__author__ = 'Rosen Diankov'

import time
import openravepy
if not __openravepy_build_doc__:
    from openravepy import *
    from numpy import *

[docs]class ConstraintPlanning: def __init__(self,robot,randomize=False,dests=None,switchpatterns=None,plannername=None): self.envreal = robot.GetEnv() self.robot = robot self.manip = self.robot.GetActiveManipulator() self.ikmodel = databases.inversekinematics.InverseKinematicsModel(robot=robot,iktype=IkParameterization.Type.Transform6D) if not self.ikmodel.load(): self.ikmodel.autogenerate() self.gmodel = databases.grasping.GraspingModel(robot=self.robot,target=self.envreal.GetKinBody('mug1')) if not self.gmodel.load(): self.gmodel.autogenerate() self.basemanip = interfaces.BaseManipulation(self.robot,plannername=plannername) self.taskmanip = interfaces.TaskManipulation(self.robot,graspername=self.gmodel.grasper.plannername,plannername=plannername)
[docs] def graspAndMove(self,showgoalcup=True): target = self.gmodel.target print 'grasping %s'%target.GetName() # only use one grasp since preshape can change validgrasps,validindices = self.gmodel.computeValidGrasps(returnnum=10) validgrasp=validgrasps[random.randint(len(validgrasps))] with self.robot: self.gmodel.setPreshape(validgrasp) jointvalues = self.robot.GetDOFValues() self.robot.GetController().SetDesired(jointvalues) self.robot.WaitForController(0) matrices = [self.gmodel.getGlobalGraspTransform(validgrasp,collisionfree=True)] self.basemanip.MoveToHandPosition(matrices=matrices,maxiter=1000,maxtries=1,seedik=10) self.robot.WaitForController(0) self.taskmanip.CloseFingers() self.robot.WaitForController(0) self.robot.Grab(target) showtarget = None if showgoalcup: # create a dummy cup to show destinations with self.envreal: showtarget = RaveCreateKinBody(self.envreal,'') showtarget.Clone(target,0) self.envreal.Add(showtarget,True) showtarget.Enable(False) for geom in showtarget.GetLinks()[0].GetGeometries(): geom.SetTransparency(0.7) try: print 'moving mug without global XY rotation' while True: # find the z rotation axis of the cup's frame localrotaxis = dot(linalg.inv(target.GetTransform()[0:3,0:3]),[0,0,1]) xyzconstraints = random.permutation(3)[0:2] constraintfreedoms = ones(6) # rotation xyz, translation xyz constraintfreedoms[3+xyzconstraints] = 0 index = argmax(abs(localrotaxis)) constraintfreedoms[index] = 0 localrotaxis = zeros(3) localrotaxis[index] = 1 print localrotaxis print 'planning with freedoms: %s, local rot axis: %s '%(constraintfreedoms,localrotaxis) constrainterrorthresh = 0.005 for iter in range(3): with self.robot: vcur = self.robot.GetDOFValues() Tee = self.manip.GetTransform() while True: Ttarget = target.GetTransform() Tlocaltarget = matrixFromAxisAngle(localrotaxis*2*(random.rand()-0.5)) Tlocaltarget[0:3,3] = 0.5*(random.rand(3)-0.5)*(1.0-array(constraintfreedoms[3:])) Tnewtarget = dot(Ttarget,Tlocaltarget) T = dot(Tnewtarget, dot(linalg.inv(target.GetTransform()), Tee)) if self.manip.FindIKSolution(T,IkFilterOptions.CheckEnvCollisions) is not None: break if showtarget is not None: showtarget.SetTransform(Tnewtarget) self.envreal.UpdatePublishedBodies() Tplane = array(Ttarget) Tplane[0:3,0:2] = Tplane[0:3,xyzconstraints] Tplane[0:3,2] = cross(Tplane[0:3,0],Tplane[0:3,1]) hplane = self.envreal.drawplane(transform=Tplane,extents=[1.0,1.0],texture=reshape([1,1,0.5,0.5],(1,1,4))) try: constrainttaskmatrix=dot(linalg.inv(Tee),target.GetTransform()) constraintmatrix = linalg.inv(target.GetTransform()) self.basemanip.MoveToHandPosition(matrices=[T],maxiter=3000,maxtries=1,seedik=40,constraintfreedoms=constraintfreedoms,constraintmatrix=constraintmatrix, constrainttaskmatrix=constrainttaskmatrix,constrainterrorthresh=constrainterrorthresh,steplength=0.002) except planning_error,e: print e self.robot.WaitForController(0) finally: if showtarget is not None: self.envreal.Remove(showtarget)
[docs]def main(env,options): "Main example code." env.Load(options.scene) robot = env.GetRobots()[0] env.UpdatePublishedBodies() time.sleep(0.1) # give time for environment to update self = ConstraintPlanning(robot) self.graspAndMove()
from optparse import OptionParser from openravepy.misc import OpenRAVEGlobalArguments @openravepy.with_destroy
[docs]def run(args=None): """Command-line execution of the example. :param args: arguments for script to parse, if not specified will use sys.argv """ parser = OptionParser(description='RRT motion planning with constraints on the robot end effector.') OpenRAVEGlobalArguments.addOptions(parser) parser.add_option('--scene', action="store",type='string',dest='scene',default='data/lab1.env.xml', help='Scene file to load (default=%default)') (options, leftargs) = parser.parse_args(args=args) OpenRAVEGlobalArguments.parseAndCreateThreadedUser(options,main,defaultviewer=True)
if __name__ == "__main__": run()

Questions/Feedback

Having problems with OpenRAVE?