def is_possible_fr_trans(oracle, base_trans, manip_trans):
load_ir_database(oracle)
return point_in_aabb(
oracle.fr_aabb,
base_values_from_trans(
np.dot(np.linalg.inv(base_trans),
manip_base_values(oracle.robot, manip_trans))))
def create_custom_ir(robot, manip_base_iterator, n=IR_DATABASE_SAMPLES):
if DEBUG: print 'Creating inverse reachability database'
ir_database = []
for manip_trans, base_trans in take(manip_base_iterator, n):
manip_trans2D = manip_base_values(robot, manip_trans)
base_trans2D = trans2D_from_trans(base_trans)
ir_database.append(
base_values_from_trans(np.linalg.solve(manip_trans2D,
base_trans2D))) # M * T = B
return ir_database
def load_ir_database(oracle):
if not hasattr(oracle, 'ir_database'):
oracle.ir_database = get_custom_ir(
oracle.robot) # TODO - allow multiple IR databases
oracle.ir_aabb = aabb_from_points(np.array(oracle.ir_database).T)
fr_database = [
base_values_from_trans(forward_transform(ir_trans))
for ir_trans in oracle.ir_database
]
oracle.fr_aabb = aabb_from_points(np.array(fr_database).T)
oracle.reachability_radius = np.max(
np.linalg.norm(np.array(oracle.ir_database)[:, :2], axis=1))
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