def handle_error(self, error):
assert isinstance(error, ExecutingException)
robot = error.robot
obj = error.object_to_grasp
print "Handling an error"
if "collision" in error.problem:
print "Got a collision error, finding occlusions"
if not doJointInterpretation:
body_filter = lambda b: b.GetName().startswith("random") or\
b.GetName().startswith('object')
else:
futureObjects = set(self.executor.objSequenceInPlan) - self.handled_objs
body_filter = lambda b: (b.GetName() not in futureObjects) \
and (b.GetName() not in self.executor.unMovableObjects)
(pose,
sol, torso_angle,
collision_list) = reachability.get_occluding_objects_names(self.executor.getGoodBodies(),
robot,
obj,
body_filter,
occluding_objects_grasping_samples,
just_one_attempt=True,
return_pose=True)
if len(collision_list) == 0:
raise ExecutingException("No way I can grasp that object!", error.line_number)
#updating the executor cache
self.executor.grasping_locations_cache[obj.GetName()] = pose, sol, torso_angle
first_collisions = collision_list.pop()
object_to_grasp_name = error.object_to_grasp.GetName()
obst_list = "\n".join("(Obstructs %s %s %s)" %("gp_"+ object_to_grasp_name,
obstr, object_to_grasp_name) for obstr in first_collisions)
error.pddl_error_info = "LineNumber: %d\n%s" % (error.line_number, obst_list)
return
elif "heavy" in error.problem:
print "They tray is too heavy!"
msg = "(Heavy %s)" % error.object_to_grasp.GetName()
error.pddl_error_info = "LineNumber: %d\n%s" % (error.line_number,
msg )
return
elif "Incompatible" in error.problem:
print "Objects are incompatible!"
#msg = "(Bigger %s %s)" % (error.object_to_grasp.GetName(),
# error.stacktop.GetName())
msg = "(not (smaller %s %s))" % (error.object_to_grasp.GetName(),
error.stacktop.GetName())
error.pddl_error_info = "LineNumber: %d\n%s" % (error.line_number,
msg )
elif 'ROS pickup' in error.problem:
print 'ROS pickup failed!'
elif 'ROS putdown' in error.problem:
print 'ROS putdown failed!'
else:
print "Don't know how to handle this problem!"
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 handle_error(self, error):
assert isinstance(error, ExecutingException)
robot = error.robot
obj = error.object_to_grasp
print "Handling an error"
if "collision" in error.problem:
print "Got a collision error, finding occlusions"
(pose,
sol, torso_angle,
collision_list) = reachability.get_occluding_objects_names(robot,
obj,
lambda b:b.GetName().startswith("random"),
occluding_objects_grasping_samples,
just_one_attempt=True,
return_pose=True)
if len(collision_list) == 0:
raise ExecutingException("No way I can grasp that object!", error.line_number)
#updating the executor cache
self.executor.grasping_locations_cache[obj.GetName()] = pose, sol, torso_angle
first_collisions = collision_list.pop()
object_to_grasp_name = error.object_to_grasp.GetName()
obst_list = "\n".join("(Obstructs %s %s %s)" %("gp_"+ object_to_grasp_name,
obstr, object_to_grasp_name) for obstr in first_collisions)
error.pddl_error_info = "LineNumber: %d\n%s" % (error.line_number, obst_list)
return
elif "heavy" in error.problem:
print "They tray is too heavy!"
msg = "(Heavy %s)" % error.object_to_grasp.GetName()
error.pddl_error_info = "LineNumber: %d\n%s" % (error.line_number,
msg )
return
elif "Incompatible" in error.problem:
print "Objects are incompatible!"
#msg = "(Bigger %s %s)" % (error.object_to_grasp.GetName(),
# error.stacktop.GetName())
msg = "(not (smaller %s %s))" % (error.object_to_grasp.GetName(),
error.stacktop.GetName())
error.pddl_error_info = "LineNumber: %d\n%s" % (error.line_number,
msg )
else:
print "Don't know how to handle this problem!"
def handle_error(self, error):
assert isinstance(error, ExecutingException)
robot = error.robot
obj = error.object_to_grasp
print "Handling an error"
if "collision" in error.problem:
print "Got a collision error, finding occlusions"
if not doJointInterpretation:
body_filter = lambda b: b.GetName().startswith("random") or\
b.GetName().startswith('object')
else:
futureObjects = set(
self.executor.objSequenceInPlan) - self.handled_objs
body_filter = lambda b: (b.GetName() not in futureObjects) \
and (b.GetName() not in self.executor.unMovableObjects)
(pose, sol, torso_angle,
collision_list) = reachability.get_occluding_objects_names(
self.executor.getGoodBodies(),
robot,
obj,
body_filter,
occluding_objects_grasping_samples,
just_one_attempt=True,
return_pose=True)
if len(collision_list) == 0:
raise ExecutingException("No way I can grasp that object!",
error.line_number)
#updating the executor cache
self.executor.grasping_locations_cache[
obj.GetName()] = pose, sol, torso_angle
first_collisions = collision_list.pop()
object_to_grasp_name = error.object_to_grasp.GetName()
obst_list = "\n".join(
"(Obstructs %s %s %s)" %
("gp_" + object_to_grasp_name, obstr, object_to_grasp_name)
for obstr in first_collisions)
error.pddl_error_info = "LineNumber: %d\n%s" % (error.line_number,
obst_list)
return
elif "heavy" in error.problem:
print "They tray is too heavy!"
msg = "(Heavy %s)" % error.object_to_grasp.GetName()
error.pddl_error_info = "LineNumber: %d\n%s" % (error.line_number,
msg)
return
elif "Incompatible" in error.problem:
print "Objects are incompatible!"
#msg = "(Bigger %s %s)" % (error.object_to_grasp.GetName(),
# error.stacktop.GetName())
msg = "(not (smaller %s %s))" % (error.object_to_grasp.GetName(),
error.stacktop.GetName())
error.pddl_error_info = "LineNumber: %d\n%s" % (error.line_number,
msg)
elif 'ROS pickup' in error.problem:
print 'ROS pickup failed!'
elif 'ROS putdown' in error.problem:
print 'ROS putdown failed!'
else:
print "Don't know how to handle this problem!"
