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