Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
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 graspObject
Grasps an object.
def waitrobot
Wait for robot to complete action.
robot
robot to be used
gmodel
grasping model for given robot and target
Class to plan a grasp for a given robot and target.
def __init__
Constructor.
envreal
environment to be used
trajdata
stored trajectory for planned path