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 getGraspables(self,manip=None,loadik=True):
print('searching for graspable objects (robot=%s)...'%(self.robot.GetRobotStructureHash()))
graspables = []
with self.env:
manips = self.robot.GetManipulators() if manip is None else [manip]
for m in manips:
self.robot.SetActiveManipulator(m)
for target in self.env.GetBodies():
if not target.IsRobot():
gmodel = grasping.GraspingModel(robot=self.robot,target=target,maxvelmult=self.maxvelmult)
if gmodel.load():
print('%s is graspable by %s'%(target.GetName(),m.GetName()))
ikmodel = inversekinematics.InverseKinematicsModel(robot=self.robot,iktype=IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
graspables.append(gmodel)
return graspables
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
def __init__(self, env):
self.env = env
self.env.SetDebugLevel(DebugLevel.Verbose)
self.collision_checker = self.env.CreateCollisionChecker('pqp')
self.collision_checker.SetCollisionOptions(CollisionOptions.Distance
| CollisionOptions.Contacts)
self.env.SetCollisionChecker(self.collision_checker)
self.robot = self.env.GetRobot('pr2')
self.ikmodel = inversekinematics.InverseKinematicsModel(
self.robot, iktype=IkParameterization.Type.Transform6D)
if not self.ikmodel.load():
self.ikmodel.autogenerate()
self.basemanip = BaseManipulation(self.robot)
self.taskmanip = None
self.updir = array((0, 0, 1))
#self.target = self.env.ReadKinBodyXMLFile('models/wg_database/18718.kinbody.xml')
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]])))
self.target.SetTransform(
array(
mat([[1, 0, 0, .7], [0, 1, 0, 0], [0, 0, 1, .674],
[0, 0, 0, 1]])))
# init robot pose
self.leftarm = self.robot.GetManipulator('leftarm')
self.rightarm = self.robot.GetManipulator('rightarm')
v = self.robot.GetActiveDOFValues()
v[35] = 3.14 / 2 # l shoulder pan
v[56] = -3.14 / 2 # r shoulder pan
v[14] = .31 # 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():
self.gmodel.autogenerate(options=self.MyGrasperOptions())
self.taskmanip = TaskManipulation(
self.robot, graspername=self.gmodel.grasper.plannername)
self.display_handles = []
O_T_Target = array([[1., 0., 0., 1.], [0., 1., 0., 0.],
[0., 0., 1., 1.], [0., 0., 0., 1.]])
target.SetTransform(O_T_Target)
target_mass = .5
setObjectTransparency(target)
env.SetViewer('qtcoin')
time.sleep(.2)
# init robot
robot = env.ReadRobotXMLFile('robots/pr2-beta-static.robot.xml')
env.AddRobot(robot)
# init robot pose: l_shoulder_pan, r_shoulder_pan, torso, l_gripper
robot.SetJointValues([pi / 2, -pi / 2, 0.31, 0.548], [35, 56, 14, 47])
# init gmodel
gmodel = grasping.GraspingModel(robot, target)
if not gmodel.load(): gmodel.autogenerate(options=MyGrasperOptions())
center_of_gravity = target.ComputeAABB().pos()
handle = env.drawarrow(p1=center_of_gravity,
p2=center_of_gravity + .01 * array([0, 0, -1]),
linewidth=.001,
color=[1, 0, 0])
# handle=drawTransform(env,robot.GetLink('l_gripper_l_finger_tip_link').GetTransform())
# pause()
# handle=drawTransform(env,robot.GetLink('l_gripper_r_finger_tip_link').GetTransform())
# pause()
# handle = None
#{'grasptrans_nocol': [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11],
#'igrasptrans': [21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32],
#'igrasproll': [33], 'igraspdir': [12, 13, 14], 'igraspstandoff': [15],
