Fawkes API
Fawkes Development Version
|
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