def __init__(self, robot, viewer = False):
self.robot = robot
self.env = robot.GetEnv ()
self.grasping_locations_cache = {}
self.viewMode = viewer
self.tray_stack = []
self.unMovableObjects = {self.env.GetKinBody('table')}
self.objSequenceInPlan = []
self.handled_objs = set()
self.object_mover = ObjectMover(self.env, use_ros, self.unMovableObjects)
self.obstruction_digraph = digraph()
v = self.robot.GetActiveDOFValues()
v[self.robot.GetJoint('torso_lift_joint').GetDOFIndex()]=1.0
self.robot.SetDOFValues(v)
if use_ros:
self.pr2robot = PR2Robot(self.env)
self.pr2robot.robot = self.robot
self.arm_mover = PR2MoveArm()
#loading the IK models
utils.pr2_tuck_arm(robot)
robot.SetActiveManipulator('leftarm')
ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(
robot,iktype=openravepy.IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
robot.SetActiveManipulator('rightarm')
ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(
robot,iktype=openravepy.IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
class Executor(object):
def __init__(self, robot, viewer = False):
self.robot = robot
self.env = robot.GetEnv ()
self.grasping_locations_cache = {}
self.viewMode = viewer
self.tray_stack = []
self.unMovableObjects = {self.env.GetKinBody('table')}
self.objSequenceInPlan = []
self.handled_objs = set()
self.object_mover = ObjectMover(self.env, use_ros, self.unMovableObjects)
self.obstruction_digraph = digraph()
v = self.robot.GetActiveDOFValues()
v[self.robot.GetJoint('torso_lift_joint').GetDOFIndex()]=1.0
self.robot.SetDOFValues(v)
if use_ros:
self.pr2robot = PR2Robot(self.env)
self.pr2robot.robot = self.robot
self.arm_mover = PR2MoveArm()
#loading the IK models
utils.pr2_tuck_arm(robot)
robot.SetActiveManipulator('leftarm')
ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(
robot,iktype=openravepy.IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
robot.SetActiveManipulator('rightarm')
ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(
robot,iktype=openravepy.IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
def getGoodBodies(self):
if not doJointInterpretation:
body_filter = lambda b: b.GetName().startswith("random") or\
b.GetName().startswith('object')
else:
futureObjects = set(self.objSequenceInPlan) - self.handled_objs
body_filter = lambda b: (b.GetName() not in futureObjects) \
and (b.GetName() not in self.unMovableObjects)
return filter(body_filter, self.env.GetBodies())
def get_bad_bodies(self, obj_to_grasp):
obj_name = obj_to_grasp.GetName()
if doJointInterpretation:
obstructions = accessibility(self.obstruction_digraph).get(obj_name, None)
future_objects = []
if doJointInterpretation and obstructions is not None:
future_objects = obstructions
future_objects.remove(obj_name)
# print self.obstruction_digraph
# print obj_name, future_objects
# raw_input("!!")
bad_body_filter = lambda b: (b.GetName() in future_objects) \
or (b.GetName() in self.unMovableObjects)
return set(filter(bad_body_filter, self.env.GetBodies()))
def setObjSequenceInPlan(self, objList):
self.objSequenceInPlan = objList
print
print "Will try to pick objs in the order " + repr(objList)
def clear_gp_cache(self):
self.grasping_locations_cache = {}
self.objSequenceInPlan = []
self.object_mover.clear_cache()
def pause(self, msg = None):
if self.viewMode:
if msg is None:
raw_input("Press return to continue")
else:
print msg
#time.sleep(0.5)
raw_input(msg + "... [press return]")
def moveto(self, _unused1, pose):
if type(pose) is str:
print "Pose ", pose, " ignored"
return
else:
print "Moving to pose "
self.robot.SetTransform(pose)
def movetowithinr1(self, _unused1, unused2):
print "Ignore me!"
def movetowithinr2(self, _unused1, unused2):
print "ignore me !"
# def movetoacrossrooms(self, _unused1, unused2, unused):
# print "ignore me !"
def movetoacrossrooms(self, _unused1, unused2):
print "ignore me !"
def grasp(self, obj_name, _unused1, _unused2):
if use_ros:
self.pr2robot.update_rave()
obj = self.env.GetKinBody(obj_name)
try:
self.object_mover.pickup(obj)
except ObjectMoveError, error:
e = ExecutingException("Object in collision")
e.robot = self.robot
e.object_to_grasp = obj
e.collision_list = error.collision_list
raise e