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 = ['table']
self.objSequenceInPlan = []
self.handled_objs = set()
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 __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 placeontray(self, unused1, obj_name, unused2, tray_name, unused4):
"""Put an item on the tray.
"""
tray = self.env.GetKinBody(tray_name)
if tray is None:
raise ValueError("Object %s does not exist" % tray_name)
obj = self.env.GetKinBody(obj_name)
if obj is None:
raise ValueError("Object %s does not exist" % obj_name)
if (not len(self.tray_stack) == 0 and
not tray_world.can_stack(self.tray_stack[-1], obj)):
e = ExecutingException("Incompatible objects")
e.robot = self.robot
e.object_to_grasp = obj
e.stacktop = self.tray_stack[-1]
raise e
T = tray_world.tray_putdown_pose(tray, self.tray_stack)
try:
(pose,
sol,
torso_angle) = generate_reaching_poses.get_collision_free_ik_pose(
self.robot,
T,
)
except generate_reaching_poses.GraspingPoseError:
raise ExecutingException("Putting down on tray has problems!")
self.pause("Going to the tray")
self.robot.SetTransform(pose)
self.pause("Arm/Torso in position")
self.robot.SetDOFValues([torso_angle],
[self.robot.GetJointIndex('torso_lift_joint')])
self.robot.SetDOFValues(sol,
self.robot.GetActiveManipulator().GetArmIndices())
print "Releasing object"
self.robot.Release(obj)
self.tray_stack.append(obj)
#putting the object straight
rot_angle = (np.pi / 2., 0., 0) #got this from the model
rot_mat = openravepy.rotationMatrixFromAxisAngle(rot_angle)
T[:3, :3] = rot_mat
T[2,3] -= 0.05
obj.SetTransform(T)
try:
del self.grasping_locations_cache[obj_name]
except KeyError:
print "funny, object ", obj_name, " was not in cache.. something wrong here!"
print "cache is: ", self.grasping_locations_cache
raw_input("Press return to continue")
print "Back to rest"
utils.pr2_tuck_arm(self.robot)
print "The tray now has: ", self.tray_stack
def generate_all_obstructions(env = None):
"""Loads an environment and generates, for each object, the list of obstructions
to reach it (if they exist).
"""
import settings
if env is None:
env = openravepy.Environment()
env.Load('boxes.dae')
elif type(env) is str:
filename = env
env = openravepy.Environment()
env.Load(filename)
#env.SetViewer('qtcoin')
robot=env.GetRobots()[0]
utils.pr2_tuck_arm(robot)
manip = robot.SetActiveManipulator('rightarm')
objects = [b
for b in env.GetBodies()
if b.GetName().startswith("random_")]
obstructions_text = []
position_index = 0
for obj in objects:
#trying to grasp
print "Testing object ", obj
try:
get_collision_free_grasping_pose(
robot,
obj,
max_trials=settings.collision_free_grasping_samples
)
print "Object ", obj, "is graspable"
except GraspingPoseError:
print "Object ", obj, "is NOT graspable, getting occlusions"
collision_list = reachability.get_occluding_objects_names(robot,
obj,
lambda b:b.GetName().startswith("random"),
settings.occluding_objects_grasping_samples,
just_one_attempt=False)
for coll in collision_list:
for obstr in coll:
s = "(Obstructs p%d %s %s)" %(position_index,
obstr, obj.GetName())
obstructions_text.append(s)
position_index += 1
print "\n\n\n"
print "\n".join(obstructions_text)
def __init__(self, robot, viewer = False):
self.robot = robot
self.env = robot.GetEnv ()
self.grasping_locations_cache = {}
self.viewMode = viewer
self.tray_stack = []
#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 putdowntray(self, unused1, tray_name, tray_loc):
"""Move the robot to the tray goal location and releases the tray
"""
tray = self.env.GetKinBody(tray_name)
if tray is None:
raise ValueError("Object %s does not exist" % tray_name)
if tray_loc == "trayloc2":
T = tray_world.tray_destination
else:
T = tray_world.tray_initial_loc
self.pause("Moving to target position")
tray_world.move_robot_base_infront_tray(self.robot, T)
print "Releasing the tray"
self.robot.Release(tray)
for obj in self.tray_stack:
self.robot.Release(obj)
self.tray_stack = []
utils.pr2_tuck_arm(self.robot)
def putdowntray(self, unused1, tray_name, tray_loc):
"""Move the robot to the tray goal location and releases the tray
"""
tray = self.env.GetKinBody(tray_name)
if tray is None:
raise ValueError("Object %s does not exist" % tray_name)
if tray_loc == "trayloc2":
T = tray_world.tray_destination
else:
T = tray_world.tray_initial_loc
self.pause("Moving to target position")
tray_world.move_robot_base_infront_tray(self.robot, T)
print "Releasing the tray"
self.robot.Release(tray)
for obj in self.tray_stack:
self.robot.Release(obj)
self.tray_stack = []
utils.pr2_tuck_arm(self.robot)
def grasp(self, obj_name, _unused1, _unused2):
print "Grasping object ", obj_name
obj = self.env.GetKinBody(obj_name)
if obj is None:
raise ValueError("Object %s does not exist" % obj_name)
cached_value = self.grasping_locations_cache.get(obj_name, None)
if cached_value is None:
print "Object %s is not cached, looking for a value" % obj_name
try:
pose, sol, torso_angle = generate_reaching_poses.get_collision_free_grasping_pose(
self.robot,
obj,
max_trials=collision_free_grasping_samples
)
self.grasping_locations_cache[obj_name] = pose, sol, torso_angle
except generate_reaching_poses.GraspingPoseError:
e = ExecutingException("Object in collision")
e.robot = self.robot
e.object_to_grasp = obj
raise e
else:
print "Object %s already cached" % obj_name
pose, sol, torso_angle = cached_value
self.pause("Moving to location")
self.robot.SetTransform(pose)
self.pause("Moving arm")
self.robot.SetDOFValues([torso_angle],
[self.robot.GetJointIndex('torso_lift_joint')])
self.robot.SetDOFValues(sol,
self.robot.GetActiveManipulator().GetArmIndices())
self.pause("Grasping object")
self.robot.Grab(obj)
utils.pr2_tuck_arm(self.robot)
def placeontray(self, unused1, obj_name, unused2, tray_name, unused4):
"""Put an item on the tray.
"""
tray = self.env.GetKinBody(tray_name)
if tray is None:
raise ValueError("Object %s does not exist" % tray_name)
obj = self.env.GetKinBody(obj_name)
if obj is None:
raise ValueError("Object %s does not exist" % obj_name)
if (not len(self.tray_stack) == 0
and not tray_world.can_stack(self.tray_stack[-1], obj)):
e = ExecutingException("Incompatible objects")
e.robot = self.robot
e.object_to_grasp = obj
e.stacktop = self.tray_stack[-1]
raise e
T = tray_world.tray_putdown_pose(tray, self.tray_stack)
try:
(pose, sol,
torso_angle) = generate_reaching_poses.get_collision_free_ik_pose(
self.getGoodBodies(),
self.env,
obj,
self.robot,
T,
)
except generate_reaching_poses.GraspingPoseError:
raise ExecutingException("Putting down on tray has problems!")
self.pause("Going to the tray")
self.robot.SetTransform(pose)
self.pause("Arm/Torso in position")
self.robot.SetDOFValues([torso_angle],
[self.robot.GetJointIndex('torso_lift_joint')])
self.robot.SetDOFValues(
sol,
self.robot.GetActiveManipulator().GetArmIndices())
print "Releasing object"
self.robot.Release(obj)
self.tray_stack.append(obj)
#putting the object straight
rot_angle = (np.pi / 2., 0., 0) #got this from the model
rot_mat = openravepy.rotationMatrixFromAxisAngle(rot_angle)
T[:3, :3] = rot_mat
T[2, 3] -= 0.05
obj.SetTransform(T)
try:
del self.grasping_locations_cache[obj_name]
except KeyError:
print "funny, object ", obj_name, " was not in cache.. something wrong here!"
print "cache is: ", self.grasping_locations_cache
raw_input("Press return to continue")
print "Back to rest"
utils.pr2_tuck_arm(self.robot)
print "The tray now has: ", self.tray_stack
