コード例 #1
0
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
コード例 #2
0
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