def __init__(self, env):
self.env = env
self.robot = self.env.GetRobot('pr2')
self.target = self.env.ReadKinBodyXMLFile('data/mug2.kinbody.xml')
self.env.AddKinBody(self.target)
self.target.SetTransform(
array(
mat([[1, 0, 0, .7], [0, 1, 0, 0], [0, 0, 1, .673],
[0, 0, 0, 1]])))
v = self.robot.GetActiveDOFValues()
v[35] = 3.14 / 2 # l shoulder pan
v[56] = -3.14 / 2 # r shoulder pan
v[14] = 0 # torso
v[47] = .54 # l gripper
self.robot.SetActiveDOFValues(v)
self.gmodel = grasping.GraspingModel(robot=self.robot,
target=self.target)
if not self.gmodel.load():
print 'go to http://people.csail.mit.edu/liuhuan/pr2/openrave/.openrave/ to get the database for openrave r1741!'
self.irmodel = inversereachability.InverseReachabilityModel(
robot=self.robot)
if not self.irmodel.load():
print 'go to http://people.csail.mit.edu/liuhuan/pr2/openrave/.openrave/ to get the database for openrave r1741!'
def __init__(self, robot):
self.robot = robot
self.env = self.robot.GetEnv()
self.robot.SetActiveManipulator('leftarm_torso')
self.manip = self.robot.GetActiveManipulator()
# initialize robot pose
v = self.robot.GetActiveDOFValues()
v[self.robot.GetJoint('l_shoulder_pan_joint').GetDOFIndex()] = 3.14 / 2
v[self.robot.GetJoint(
'r_shoulder_pan_joint').GetDOFIndex()] = -3.14 / 2
v[self.robot.GetJoint('l_gripper_l_finger_joint').GetDOFIndex()] = .54
self.robot.SetActiveDOFValues(v)
# load inverserechability database
self.irmodel = inversereachability.InverseReachabilityModel(
robot=self.robot)
starttime = time.time()
print 'loading irmodel'
if not self.irmodel.load():
print 'do you want to generate irmodel for your robot? it might take several hours'
print 'or you can go to http://people.csail.mit.edu/liuhuan/pr2/openrave/openrave_database/ to get the database for PR2'
input = raw_input('[Y/n]')
if input == 'y' or input == 'Y' or input == '\n' or input == '':
self.irmodel.autogenerate()
self.irmodel.load()
else:
self.env.Destroy()
RaveDestroy()
sys.exit()
print 'time to load inverse-reachability model: %fs' % (time.time() -
starttime)
# make sure the robot and manipulator match the database
assert self.irmodel.robot == self.robot and self.irmodel.manip == self.robot.GetActiveManipulator(
)
def __init__(self, env):
self.env = env
self.robot = self.env.GetRobot('pr2')
self.manip = self.robot.GetActiveManipulator()
# initialize robot pose
v = self.robot.GetActiveDOFValues()
v[35] = 3.14 / 2 # l shoulder pan
v[56] = -3.14 / 2 # r shoulder pan
"""note here the torso height must be set to 0, because the database was generated for torso height=0"""
v[14] = 0 # torso
v[47] = .54 # l gripper
self.robot.SetActiveDOFValues(v)
# load inverserechability database
self.irmodel = inversereachability.InverseReachabilityModel(
robot=self.robot)
starttime = time.time()
print 'loading irmodel'
if not self.irmodel.load():
print 'do you want to generate irmodel for your robot? it might take several hours'
print 'or you can go to http://people.csail.mit.edu/liuhuan/pr2/openrave/.openrave/ to get the database for PR2'
input = raw_input('[Y/n]')
if input == 'y' or input == 'Y' or input == '\n' or input == '':
class IrmodelOption:
def __init__(self,
robot,
heightthresh=.05,
quatthresh=.15,
Nminimum=10,
id=None,
jointvalues=None):
self.robot = robot
self.heightthresh = heightthresh
self.quatthresh = quatthresh
self.Nminimum = Nminimum
self.id = id
self.jointvalues = jointvalues
option = IrmodelOption(self.robot)
self.irmodel.autogenerate(option)
self.irmodel.load()
else:
self.env.Destroy()
RaveDestroy()
sys.exit()
print 'time to load inverse-reachability model: %fs' % (time.time() -
starttime)
# make sure the robot and manipulator match the database
assert self.irmodel.robot == self.robot and self.irmodel.manip == self.robot.GetActiveManipulator(
)
def __init__(self,robot,target=None,irmodels=None,irgmodels=None,maxvelmult=None):
self.robot = robot
self.env = robot.GetEnv()
self.irmodels=irmodels
self.maxvelmult=maxvelmult
self.irgmodels = []
if irgmodels is None:
if target is not None:
gmodel = grasping.GraspingModel(robot=robot,target=target,maxvelmult=maxvelmult)
if not gmodel.load():
gmodel.autogenerate()
irmodel = inversereachability.InverseReachabilityModel(robot=robot)
if not irmodel.load():
irmodel.autogenerate()
self.irgmodels = [[irmodel,gmodel]]
else:
# check for consistency
for irmodel,gmodel in irgmodels:
assert irmodel.robot == self.robot and irmodel.robot == gmodel.robot and irmodel.manip == gmodel.manip
self.irgmodels = irgmodels
robot.SetDOFValues([-0.7, -0.7], [19, 20])
manip = robot.SetActiveManipulator('leftArm')
env.AddKinBody(target)
sys.stdin.readline()
gmodel = grasping.GraspingModel(robot=robot, target=target)
print "grasping model filename:"
print gmodel.getfilename()
if not gmodel.load():
print 'go to http://people.csail.mit.edu/liuhuan/pr2/openrave/.openrave/ to get the database for openrave r1741!'
print "getting inverse reachability model"
irmodel = inversereachability.InverseReachabilityModel(robot=robot)
print "inverse reachability model filename:"
print irmodel.getfilename()
if not irmodel.load():
print 'go to http://people.csail.mit.edu/liuhuan/pr2/openrave/.openrave/ to get the database for openrave r1741!'
validgrasps, validindices = gmodel.computeValidGrasps(checkik=False,
backupdist=0.01)
print 'validgrasps', len(validgrasps)
print 'validindices', validindices
def graspiter():
for grasp, graspindex in izip(validgrasps, validindices):
if not gmodels[0].load():
print "generating grasping set for robot_0 and the target"
gmodels[0].autogenerate()
else:
print "grasping set already exists for robot_0"
gmodels.append(openravepy.databases.grasping.GraspingModel(robots[1], mybox))
if not gmodels[1].load():
print "generating grasping set for robot_1 and the target"
gmodels[1].autogenerate()
else:
print "grasping set already exists for robot_1"
print "getting inverse reachability model for robot_0"
irmodels = []
irmodels.append(inversereachability.InverseReachabilityModel(robot=robots[0]))
print "inverse reachability model filename:"
print irmodels[0].getfilename()
print "loading inverse reachability model for robot_0. Please wait..."
if not irmodels[0].load():
print 'could not load inverse reachability model for robot_0'
else:
print 'inverse reachability model for robot_0 is loaded.'
irmodels.append(inversereachability.InverseReachabilityModel(robot=robots[1]))
print "inverse reachability model filename:"
print irmodels[1].getfilename()
print "loading inverse reachability model for robot_1. Please wait..."