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}")
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
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
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
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"))