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
22import time
23from openravepy import *
24from 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.
32class 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
116def 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
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