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!"