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 import openravepy
24 from openravepy import *
25 from numpy import *
26 
27 ## Class to plan a grasp for a given robot and target.
28 #
29 # This class loads a pregenerated grasping database and can use
30 # those grasps to find a valid grasp for the given target, and
31 # calculate a collision-free path for the arm to move to a grasping
32 # position.
34 
35  ## Constructor.
36  #
37  # @param robot the robot to be used for planning.
38  # @param target the target KinBody.
39  def __init__(self,robot,target):
40  ## environment to be used
41  self.envreal = robot.GetEnv()
42 
43  ## robot to be used
44  self.robot = robot
45 
46  ## target to be used
47  self.target = target
48 
49  with self.envreal:
50  gmodel = databases.grasping.GraspingModel(robot=self.robot,target=self.target)
51  print 'check if model can be loaded'
52  if not gmodel.load():
53  print 'need to autogenerate model'
54  gmodel.autogenerate()
55  print gmodel;
56 
57  ## grasping model for given robot and target
58  self.gmodel = gmodel
59 
60  ## Wait for robot to complete action.
61  # @param robot The robot to be checked.
62  # @return void
63  def waitrobot(self,robot=None):
64  if robot is None:
65  robot = self.robot
66  while not robot.GetController().IsDone():
67  time.sleep(0.01)
68 
69  ## Grasps an object.
70  # This version returns the first valid grasp found. Should be tweaked in later
71  # versions, as the first valid grasp might be at the bottom of the target
72  # instead of the middle, which would be preferred.
73  # @return graspindex if successfull, -1 if failed to find valid grasp
74  def graspObject(self):
75  env = self.envreal
76  robot = self.robot
77  gmodel = self.gmodel
78  dests = None
79 
80  with env:
81  ## taskmanipulation problem/module
82  self.taskmanip = interfaces.TaskManipulation(self.robot,graspername=gmodel.grasper.plannername)
83 
84  approachoffset = 0.0
85  istartgrasp = 0
86  target = gmodel.target
87 
88  while istartgrasp < len(gmodel.grasps):
89  goals,graspindex,searchtime,trajdata = self.taskmanip.GraspPlanning(graspindices=gmodel.graspindices,grasps=gmodel.grasps[istartgrasp:],
90  target=target,approachoffset=approachoffset,destposes=dests,
91  seedgrasps = 3,seeddests=8,seedik=1,maxiter=1000,
92  randomgrasps=True,randomdests=True,outputtraj=True,execute=False)
93  istartgrasp = graspindex+1
94  ## stored trajectory for planned path
95  self.trajdata = trajdata
96  Tglobalgrasp = gmodel.getGlobalGraspTransform(gmodel.grasps[graspindex],collisionfree=True)
97 
98  print 'grasp %d initial planning time: %f'%(graspindex,searchtime)
99  print 'goals:'
100  print goals
101  print 'trajdata'
102  print trajdata
103  self.waitrobot(robot)
104 
105  with env:
106  robot.ReleaseAllGrabbed()
107 
108  return graspindex # return successful grasp index
109 
110  # exhausted all grasps
111  return -1
112 
113 ## Run graspplanning.
114 # @param envId unique id of the OpenRAVE Environment
115 # @param robotName unique name of the OpenRAVE Robot
116 # @param targetName unique name of the target (an OpenRAVE KinBody)
117 # @return planned grasping trajectory as a string
118 def runGrasp(envId, robotName, targetName):
119  env = RaveGetEnvironment(envId)
120  robot = env.GetRobot(robotName)
121  target = env.GetKinBody(targetName)
122 
123  self = GraspPlanner(robot, target)
124  try:
125  print 'grasping object %s'%self.target.GetName()
126  with self.envreal:
127  self.robot.ReleaseAllGrabbed()
128  success = self.graspObject()
129  print 'success: ',success
130  return self.trajdata
131  except planning_error, e:
132  print 'failed to grasp object %s'%self.target.GetName()
133  print e
taskmanip
taskmanipulation problem/module
target
target to be used
def __init__(self, robot, target)
Constructor.
robot
robot to be used
gmodel
grasping model for given robot and target
Class to plan a grasp for a given robot and target.
envreal
environment to be used
def waitrobot(self, robot=None)
Wait for robot to complete action.
trajdata
stored trajectory for planned path
def graspObject(self)
Grasps an object.