def step(self, oracle): oracle.set_robot_config(self.pap.approach_config) yield oracle.set_robot_config(self.pap.grasp_config) open_gripper(oracle) release(oracle, self.object_name) yield oracle.set_robot_config(self.pap.approach_config) yield
def collision_ignorant_grasp(oracle, geom_hash, grasp_tform, approach_vector, manip_name=None): # TODO - try to make a valid grasp if possible with oracle.state_saver(): if manip_name is not None: oracle.robot.SetActiveManipulator(manip_name) oracle.set_active_state(active_obstacles=False, active_objects=set()) open_gripper(oracle) oracle.robot.SetDOFValues(oracle.default_left_arm_config, oracle.robot.GetActiveManipulator().GetArmIndices()) gripper_config, gripper_traj = oracle.task_manip.CloseFingers(offset=None, movingdir=None, execute=False, outputtraj=False, outputfinal=True, coarsestep=None, translationstepmult=None, finestep=None, outputtrajobj=True) return Grasp(grasp_tform, gripper_config, TrajTrajectory(CSpace(oracle.robot, indices=oracle.robot.GetActiveManipulator().GetGripperIndices()), gripper_traj), approach_vector)
def collision_ignorant_grasp(oracle, geom_hash, grasp_tform, approach_vector, manip_name=None): # TODO - try to make a valid grasp if possible with oracle.state_saver(): if manip_name is not None: oracle.robot.SetActiveManipulator(manip_name) oracle.set_active_state(active_obstacles=False, active_objects=set()) open_gripper(oracle) oracle.robot.SetDOFValues(oracle.default_left_arm_config, oracle.robot.GetActiveManipulator().GetArmIndices()) gripper_config, gripper_traj = oracle.task_manip.CloseFingers(offset=None, movingdir=None, execute=False, outputtraj=False, outputfinal=True, coarsestep=None, translationstepmult=None, finestep=None, outputtrajobj=True) return Grasp(grasp_tform, gripper_config, TrajTrajectory(CSpace(oracle.robot, indices=oracle.robot.GetActiveManipulator().GetGripperIndices()), gripper_traj), approach_vector) return None
def collision_free_grasp(oracle, geom_hash, grasp_tform, approach_vector, manip_name=None): body_name = oracle.get_body_name(geom_hash) with oracle.state_saver(): if manip_name is not None: oracle.robot.SetActiveManipulator(manip_name) oracle.set_active_state(active_obstacles=False, active_objects=set([body_name])) open_gripper(oracle) oracle.robot.SetDOFValues(oracle.default_left_arm_config, oracle.robot.GetActiveManipulator().GetArmIndices()) set_trans(oracle.bodies[body_name], object_trans_from_manip_trans(get_manip_trans(oracle), grasp_tform)) if not robot_collision(oracle, body_name): gripper_config, gripper_traj = oracle.task_manip.CloseFingers(offset=None, movingdir=None, execute=False, outputtraj=False, outputfinal=True, coarsestep=None, translationstepmult=None, finestep=None, outputtrajobj=True) return Grasp(grasp_tform, gripper_config, TrajTrajectory(CSpace(oracle.robot, indices=oracle.robot.GetActiveManipulator().GetGripperIndices()), gripper_traj), approach_vector) return None
def step(self, oracle): oracle.set_robot_config(self.pap.approach_config) yield if STEP_VECTOR: oracle.set_robot_config(self.pap.vector_config) yield oracle.set_robot_config(self.pap.grasp_config) open_gripper(oracle) release(oracle, self.object_name) yield if STEP_VECTOR: oracle.set_robot_config(self.pap.vector_config) yield oracle.set_robot_config(self.pap.approach_config) yield
def plan_base_traj(oracle, start_config, end_config, holding=None, obstacle_poses={}): with oracle.state_saver(): oracle.set_all_object_poses(obstacle_poses) oracle.set_robot_config(end_config) base_trans = get_trans(oracle.robot) oracle.set_robot_config(start_config) open_gripper(oracle) if holding is not None: oracle.set_pose(holding.object_name, object_pose_from_robot_config(oracle, start_config, holding.grasp)) grab(oracle, holding.object_name) traj = cspace_traj(oracle, CSpace.robot_base(oracle.robot), base_values_from_trans(base_trans)) if holding is not None: release(oracle, holding.object_name) return traj
def execute(): oracle.unlock() oracle.problem.set_viewer(oracle.env) oracle.clear_regions() oracle.draw_goals() set_state(plan.start, oracle) open_gripper(oracle) # TODO - do I always want this? update_viewer() if display: raw_input('Hit enter to finish' if len(plan.operators) == 0 else 'Hit enter to step') else: sleep(1) # NOTE - Gives time for the viewer to update if save is not None: save_image(oracle.env, IMAGE_FORMAT%(save, 0, 0)) # TODO - sometimes captures images before the viewer is ready for i, action in enumerate(plan.operators): if isinstance(action, StepExecutable): for j, _ in enumerate(action.step(oracle)): update_viewer() if display: raw_input('Hit enter to finish' if i == len(plan.operators) - 1 else 'Hit enter to step') else: sleep(1) # Gives time for the viewer to update if save is not None: save_image(oracle.env, IMAGE_FORMAT%(save, i+1, j)) # TODO - sometimes captures images before the viewer is ready