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.envreal = robot.GetEnv()
41 
42  ## robot to be used
43  self.robot = robot
44 
45  ## target to be used
46  self.target = target
47 
48  with self.envreal:
49  gmodel = databases.grasping.GraspingModel(robot=self.robot,target=self.target)
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.gmodel = 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.robot
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.envreal
75  robot = self.robot
76  gmodel = self.gmodel
77  dests = None
78 
79  with env:
80  ## taskmanipulation problem/module
81  self.taskmanip = interfaces.TaskManipulation(self.robot,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.taskmanip.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.trajdata = 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.waitrobot(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.target.GetName()
124  with self.envreal:
125  self.robot.ReleaseAllGrabbed()
126  success = self.graspObject()
127  print 'success: ',success
128  return self.trajdata
129  except planning_error, e:
130  print 'failed to grasp object %s'%self.target.GetName()
131  print e
graspplanning.GraspPlanner.gmodel
gmodel
grasping model for given robot and target
Definition: graspplanning.py:57
graspplanning.GraspPlanner.__init__
def __init__(self, robot, target)
Constructor.
Definition: graspplanning.py:38
graspplanning.GraspPlanner.envreal
envreal
environment to be used
Definition: graspplanning.py:40
graspplanning.GraspPlanner.waitrobot
def waitrobot(self, robot=None)
Wait for robot to complete action.
Definition: graspplanning.py:62
graspplanning.GraspPlanner
Class to plan a grasp for a given robot and target.
Definition: graspplanning.py:32
graspplanning.GraspPlanner.target
target
target to be used
Definition: graspplanning.py:46
graspplanning.GraspPlanner.taskmanip
taskmanip
taskmanipulation problem/module
Definition: graspplanning.py:81
graspplanning.GraspPlanner.graspObject
def graspObject(self)
Grasps an object.
Definition: graspplanning.py:73
graspplanning.GraspPlanner.robot
robot
robot to be used
Definition: graspplanning.py:43
graspplanning.GraspPlanner.trajdata
trajdata
stored trajectory for planned path
Definition: graspplanning.py:94