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
예제 #2
0
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