Fawkes API  Fawkes Development Version
graspplanning.py
00001 
00002 ###########################################################################
00003 #  graspplanning.py - Graspplanning script
00004 #
00005 #  Created: Thu Oct 13 12:50:34 2011
00006 #  Copyright  2011  Bahram Maleki-Fard, AllemaniACs RoboCup Team
00007 #
00008 ############################################################################
00009 
00010 #  This program is free software; you can redistribute it and/or modify
00011 #  it under the terms of the GNU General Public License as published by
00012 #  the Free Software Foundation; either version 2 of the License, or
00013 #  (at your option) any later version.
00014 #
00015 #  This program is distributed in the hope that it will be useful,
00016 #  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017 #  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018 #  GNU Library General Public License for more details.
00019 #
00020 #  Read the full text in the LICENSE.GPL file in the doc directory.
00021 
00022 import time
00023 import openravepy
00024 from openravepy import *
00025 from numpy import *
00026 
00027 ## Class to plan a grasp for a given robot and target.
00028 #
00029 # This class loads a pregenerated grasping database and can use
00030 # those grasps to find a valid grasp for the given target, and
00031 # calculate a collision-free path for the arm to move to a grasping
00032 # position.
00033 class GraspPlanner:
00034 
00035     ## Constructor.
00036     #
00037     # @param robot the robot to be used for planning.
00038     # @param target the target KinBody.
00039     def __init__(self,robot,target):
00040         ## environment to be used
00041         self.envreal = robot.GetEnv()
00042 
00043         ## robot to be used
00044         self.robot = robot
00045 
00046         ## target to be used
00047         self.target = target
00048 
00049         with self.envreal:
00050             gmodel = databases.grasping.GraspingModel(robot=self.robot,target=self.target)
00051             print 'check if model can be loaded'
00052             if not gmodel.load():
00053                 print 'need to autogenerate model'
00054                 gmodel.autogenerate()
00055             print gmodel;
00056 
00057         ## grasping model for given robot and target
00058         self.gmodel = gmodel
00059 
00060     ## Wait for robot to complete action.
00061     # @param robot The robot to be checked.
00062     # @return void
00063     def waitrobot(self,robot=None):
00064         if robot is None:
00065             robot = self.robot
00066         while not robot.GetController().IsDone():
00067             time.sleep(0.01)
00068 
00069     ## Grasps an object.
00070     # This version returns the first valid grasp found. Should be tweaked in later
00071     # versions, as the first valid grasp might be at the bottom of the target
00072     # instead of the middle, which would be preferred.
00073     # @return graspindex if successfull, -1 if failed to find valid grasp
00074     def graspObject(self):
00075         env = self.envreal
00076         robot = self.robot
00077         gmodel = self.gmodel
00078         dests = None
00079 
00080         with env:
00081             ## taskmanipulation problem/module
00082             self.taskmanip = interfaces.TaskManipulation(self.robot,graspername=gmodel.grasper.plannername)
00083 
00084         approachoffset = 0.0
00085         istartgrasp = 0
00086         target = gmodel.target
00087 
00088         while istartgrasp < len(gmodel.grasps):
00089             goals,graspindex,searchtime,trajdata = self.taskmanip.GraspPlanning(graspindices=gmodel.graspindices,grasps=gmodel.grasps[istartgrasp:],
00090                                                                                 target=target,approachoffset=approachoffset,destposes=dests,
00091                                                                                 seedgrasps = 3,seeddests=8,seedik=1,maxiter=1000,
00092                                                                                 randomgrasps=True,randomdests=True,outputtraj=True,execute=False)
00093             istartgrasp = graspindex+1
00094             ## stored trajectory for planned path
00095             self.trajdata = trajdata
00096             Tglobalgrasp = gmodel.getGlobalGraspTransform(gmodel.grasps[graspindex],collisionfree=True)
00097 
00098             print 'grasp %d initial planning time: %f'%(graspindex,searchtime)
00099             print 'goals:'
00100             print goals
00101             print 'trajdata'
00102             print trajdata
00103             self.waitrobot(robot)
00104 
00105             with env:
00106                 robot.ReleaseAllGrabbed()
00107 
00108             return graspindex # return successful grasp index
00109 
00110         # exhausted all grasps
00111         return -1
00112 
00113 ## Run graspplanning.
00114 # @param envId unique id of the OpenRAVE Environment
00115 # @param robotName unique name of the OpenRAVE Robot
00116 # @param targetName unique name of the target (an OpenRAVE KinBody)
00117 # @return planned grasping trajectory as a string
00118 def runGrasp(envId, robotName, targetName):
00119     env = RaveGetEnvironment(envId)
00120     robot = env.GetRobot(robotName)
00121     target = env.GetKinBody(targetName)
00122 
00123     self = GraspPlanner(robot, target)
00124     try:
00125         print 'grasping object %s'%self.target.GetName()
00126         with self.envreal:
00127             self.robot.ReleaseAllGrabbed()
00128         success = self.graspObject()
00129         print 'success: ',success
00130         return self.trajdata
00131     except planning_error, e:
00132         print 'failed to grasp object %s'%self.target.GetName()
00133         print e