def ir_base_trans(oracle, manip_trans, transform, default_trans): manip_trans2D = trans_from_base_values([manip_trans[0, 3], manip_trans[1, 3], get_manip_angle(oracle.robot, manip_trans)]) transform_trans2D = trans_from_base_values(transform) base_trans2D = np.dot(manip_trans2D, transform_trans2D) base_trans = default_trans.copy() base_trans[:2, :2] = base_trans2D[:2, :2] base_trans[:2, 3] = base_trans2D[:2, 3] return base_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 = trans_from_base_values([manip_trans[0, 3], manip_trans[1, 3], get_manip_angle(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