Fawkes API  Fawkes Development Version
graspplanning.py
1 
2 ###########################################################################
3 # graspplanning.py - Graspplanning script
4 #
5 # Created: Thu Oct 13 12:50:34 2011
6 # Copyright 2011 Bahram Maleki-Fard, AllemaniACs RoboCup Team
7 #
8 ############################################################################
9 
10 # This program is free software; you can redistribute it and/or modify
11 # it under the terms of the GNU General Public License as published by
12 # the Free Software Foundation; either version 2 of the License, or
13 # (at your option) any later version.
14 #
15 # This program is distributed in the hope that it will be useful,
16 # but WITHOUT ANY WARRANTY; without even the implied warranty of
17 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 # GNU Library General Public License for more details.
19 #
20 # Read the full text in the LICENSE.GPL file in the doc directory.
21 
22 import time
23 from openravepy import *
24 from numpy import *
25 
26 ## Class to plan a grasp for a given robot and target.
27 #
28 # This class loads a pregenerated grasping database and can use
29 # those grasps to find a valid grasp for the given target, and
30 # calculate a collision-free path for the arm to move to a grasping
31 # position.
32 class GraspPlanner(object):
33 
34  ## Constructor.
35  #
36  # @param robot the robot to be used for planning.
37  # @param target the target KinBody.
38  def __init__(self,robot,target):
39  ## environment to be used
40  self.envrealenvreal = robot.GetEnv()
41 
42  ## robot to be used
43  self.robotrobot = robot
44 
45  ## target to be used
46  self.targettarget = target
47 
48  with self.envrealenvreal:
49  gmodel = databases.grasping.GraspingModel(robot=self.robotrobot,target=self.targettarget)
50  print 'check if model can be loaded'
51  if not gmodel.load():
52  print 'need to autogenerate model'
53  gmodel.autogenerate()
54  print gmodel;
55 
56  ## grasping model for given robot and target
57  self.gmodelgmodel = gmodel
58 
59  ## Wait for robot to complete action.
60  # @param robot The robot to be checked.
61  # @return void
62  def waitrobot(self,robot=None):
63  if robot is None:
64  robot = self.robotrobot
65  while not robot.GetController().IsDone():
66  time.sleep(0.01)
67 
68  ## Grasps an object.
69  # This version returns the first valid grasp found. Should be tweaked in later
70  # versions, as the first valid grasp might be at the bottom of the target
71  # instead of the middle, which would be preferred.
72  # @return graspindex if successfull, -1 if failed to find valid grasp
73  def graspObject(self):
74  env = self.envrealenvreal
75  robot = self.robotrobot
76  gmodel = self.gmodelgmodel
77  dests = None
78 
79  with env:
80  ## taskmanipulation problem/module
81  self.taskmaniptaskmanip = interfaces.TaskManipulation(self.robotrobot,graspername=gmodel.grasper.plannername)
82 
83  approachoffset = 0.0
84  istartgrasp = 0
85  target = gmodel.target
86 
87  while istartgrasp < len(gmodel.grasps):
88  goals,graspindex,searchtime,trajdata = self.taskmaniptaskmanip.GraspPlanning(graspindices=gmodel.graspindices,grasps=gmodel.grasps[istartgrasp:],
89  target=target,approachoffset=approachoffset,destposes=dests,
90  seedgrasps = 3,seeddests=8,seedik=1,maxiter=1000,
91  randomgrasps=True,randomdests=True,outputtraj=True,execute=False)
92  # istartgrasp = graspindex+1
93  ## stored trajectory for planned path
94  self.trajdatatrajdata = trajdata
95 
96  print 'grasp %d initial planning time: %f'%(graspindex,searchtime)
97  print 'goals:'
98  print goals
99  print 'trajdata'
100  print trajdata
101  self.waitrobotwaitrobot(robot)
102 
103  with env:
104  robot.ReleaseAllGrabbed()
105 
106  return graspindex # return successful grasp index
107 
108  # exhausted all grasps
109  return -1
110 
111 ## Run graspplanning.
112 # @param envId unique id of the OpenRAVE Environment
113 # @param robotName unique name of the OpenRAVE Robot
114 # @param targetName unique name of the target (an OpenRAVE KinBody)
115 # @return planned grasping trajectory as a string
116 def runGrasp(envId, robotName, targetName):
117  env = RaveGetEnvironment(envId)
118  robot = env.GetRobot(robotName)
119  target = env.GetKinBody(targetName)
120 
121  self = GraspPlanner(robot, target)
122  try:
123  print 'grasping object %s'%self.targettarget.GetName()
124  with self.envrealenvreal:
125  self.robotrobot.ReleaseAllGrabbed()
126  success = self.graspObjectgraspObject()
127  print 'success: ',success
128  return self.trajdatatrajdata
129  except planning_error, e:
130  print 'failed to grasp object %s'%self.targettarget.GetName()
131  print e
Class to plan a grasp for a given robot and target.
def __init__(self, robot, target)
Constructor.
trajdata
stored trajectory for planned path
target
target to be used
def graspObject(self)
Grasps an object.
taskmanip
taskmanipulation problem/module
envreal
environment to be used
def waitrobot(self, robot=None)
Wait for robot to complete action.
gmodel
grasping model for given robot and target