Esempio n. 1
0
def _test_stability(max_time=5, render=False, test_generator=None):
    """Tests the stability of the controller using speed profiles."""
    locomotion_controller_setup.load_sim_config(render=render)
    gin.parse_config(SCENARIO_SET_CONFIG)
    if FLAGS.add_random_push:
        locomotion_controller_setup.add_random_push_config()

    env = env_loader.load()
    controller = locomotion_controller_setup.setup_controller(env.robot,
                                                              gait=FLAGS.gait)

    for name, speed_profile in test_generator():
        env.reset()
        controller.reset()
        current_time = 0
        while current_time < max_time:
            current_time = env.get_time_since_reset()
            lin_speed, ang_speed = _generate_linear_angular_speed(
                current_time, speed_profile[0], speed_profile[1])
            _update_controller_params(controller, lin_speed, ang_speed)

            # Needed before every call to get_action().
            controller.update()
            hybrid_action = controller.get_action()

            _, _, done, _ = env.step(hybrid_action)
            if done:
                break
        print(
            f"Scene name: flat ground. Random push: {FLAGS.add_random_push}. "
            f"Survival time for {name} = {speed_profile[1]} is {current_time}")
Esempio n. 2
0
def main(argv):
  del argv  # Unused.

  # Parse the gym config and create the environment.
  for gin_file in CONFIG_FILES:
    gin.parse_config_file(gin_file)
  gin.bind_parameter("SimulationParameters.enable_rendering", True)
  gin.bind_parameter("terminal_conditions.maxstep_terminal_condition.max_step",
                     10000)
  env = env_loader.load()
  tg_intensity = _PMTG_INTENSITY_RANGE[0]
  sum_reward = 0
  env.reset()
  done = False
  # Use zero residual, only use the output of the trajectory generator.
  residual = [0] * _NUM_MOTORS
  # Since we fix residuals and all the parameters of the TG, this example
  # is practically an open loop controller. A learned policy would provide
  # different values for these parameters at every timestep.
  while not done:
    # Increase the intensity of the trajectory generator gradually
    # to illustrate increasingly larger steps.
    if tg_intensity < _PMTG_INTENSITY_RANGE[1]:
      tg_intensity += _PMTG_INTENSITY_STEP_SIZE
    tg_params = [
        _PMTG_DELTA_TIME_MULTIPLIER, tg_intensity, _PMTG_WALKING_HEIGHT,
        _PMTG_SWING_VS_STANCE
    ]
    action = residual + tg_params
    _, reward, done, _ = env.step(action)
    sum_reward += reward
def _build_env():
    """Builds the environment for the Laikago robot.

  Returns:
    The OpenAI gym environment.
  """
    gin.parse_config_file(CONFIG_FILE_SIM)
    gin.bind_parameter("SimulationParameters.enable_rendering",
                       ENABLE_RENDERING)
    env = env_loader.load()
    env.seed(ENV_RANDOM_SEED)

    return env
Esempio n. 4
0
def run_example(num_max_steps=_NUM_STEPS):
    """Runs the example.

  Args:
    num_max_steps: Maximum number of steps this example should run for.
  """
    env = env_loader.load()

    env.seed(_ENV_RANDOM_SEED)
    observation = env.reset()
    policy = static_gait_controller.StaticGaitController(env.robot)

    for _ in range(num_max_steps):
        action = policy.act(observation)
        _, _, done, _ = env.step(action)
        if done:
            break
Esempio n. 5
0
def create_laikago_env(args):
    CONFIG_DIR = pd.getDataPath() + "/configs_v2/"
    CONFIG_FILES = [
        os.path.join(CONFIG_DIR, "base/laikago_with_imu.gin"),
        os.path.join(CONFIG_DIR, "tasks/fwd_task_no_termination.gin"),
        os.path.join(CONFIG_DIR, "wrappers/pmtg_wrapper.gin"),
        os.path.join(CONFIG_DIR, "scenes/simple_scene.gin")
    ]

    #gin.bind_parameter("scene_base.SceneBase.data_root", pd.getDataPath()+"/")
    for gin_file in CONFIG_FILES:
        gin.parse_config_file(gin_file)
    gin.bind_parameter("SimulationParameters.enable_rendering", True)
    gin.bind_parameter(
        "terminal_conditions.maxstep_terminal_condition.max_step", 10000)
    env = env_loader.load()
    return env
Esempio n. 6
0
def _run_example(max_time=_MAX_TIME_SECONDS,
                 run_on_robot=False,
                 use_keyboard=False):
  """Runs the locomotion controller example."""
  if use_keyboard:
    kb = keyboard_utils.KeyboardInput()

  env = env_loader.load()
  env.reset()

  # To mitigate jittering from the python
  gc.collect()

  # Wait for the robot to be placed properly.
  if run_on_robot:
    input("Press Enter to continue when robot is ready.")

  lin_speed = np.array([0.0, 0.0, 0.0])
  ang_speed = 0.0

  controller = locomotion_controller_setup.setup_controller(
      env.robot, FLAGS.gait, run_on_robot, FLAGS.use_ground_truth_velocity)
  controller.reset()

  loop_start_time = env.get_time_since_reset()
  loop_elapsed_time = 0
  robot_log = {
      "timestamps": [],
      "motor_angles": [],
      "motor_velocities": [],
      "base_velocities": [],
      "foot_positions": [],
      "base_rollpitchyaw": [],
      "base_angular_velocities": [],
      "actions": []
  }
  try:
    while loop_elapsed_time < max_time:
      #if use_keyboard:
      #  lin_speed, ang_speed = _update_speed_from_kb(kb, lin_speed, ang_speed)
      #else:
      lin_speed, ang_speed = _generate_example_linear_angular_speed(
            loop_elapsed_time)

      # Needed before every call to get_action().
      _update_controller_params(controller, lin_speed, ang_speed)
      controller.update()
      hybrid_action = controller.get_action()

      # Log the robot data.
      robot_log["timestamps"].append(env.robot.GetTimeSinceReset())
      robot_log["motor_angles"].append(env.robot.motor_angles)
      robot_log["motor_velocities"].append(env.robot.motor_velocities)
      robot_log["base_velocities"].append(
          controller.state_estimator.com_velocity_body_yaw_aligned_frame)
      robot_log["foot_positions"].append(env.robot.foot_positions())
      robot_log["base_rollpitchyaw"].append(env.robot.base_roll_pitch_yaw)
      robot_log["base_angular_velocities"].append(
          env.robot.base_roll_pitch_yaw_rate)
      robot_log["actions"].append(hybrid_action)

      env.step(hybrid_action)
      loop_elapsed_time = env.get_time_since_reset() - loop_start_time

  finally:
    if FLAGS.run_on_robot:
      # Apply zero torques to the robot.
      env.robot.apply_action(
          [0] * env.robot.num_motors,
          motor_control_mode=robot_config.MotorControlMode.TORQUE)
    if FLAGS.log_path:
      pickle.dump(robot_log, gfile.Open(FLAGS.log_path + "/robot.log", "wb"))