def load_grasp_database(robot, filename):
grasps = []
for grasp_tform, gripper_config, (trajectory, indices), approach_vector in read_pickle(filename):
grasps.append(Grasp(np.array(grasp_tform), np.array(gripper_config),
TrajTrajectory(CSpace(robot, indices=np.array(indices)), new_traj().deserialize(trajectory)), approach_vector))
if DEBUG: print 'Loaded', filename
return grasps
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_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
