def _run_example(max_time=_MAX_TIME_SECONDS):
"""Runs the locomotion controller example."""
# env = env_builder.build_regular_env(
# a1.A1,
# motor_control_mode=robot_config.MotorControlMode.HYBRID,
# enable_rendering=True,
# on_rack=False,
# wrap_trajectory_generator=False)
# robot = env.robot
# p = env.pybullet_client
p = bullet_client.BulletClient(connection_mode=pybullet.GUI)
p.setPhysicsEngineParameter(numSolverIterations=30)
p.setTimeStep(0.001)
p.setGravity(0, 0, -9.8)
p.setPhysicsEngineParameter(enableConeFriction=0)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadURDF("plane.urdf")
robot = a1.A1(p,
motor_control_mode=robot_config.MotorControlMode.HYBRID,
enable_action_interpolation=False,
reset_time=2,
action_repeat=5)
controller = _setup_controller(robot)
controller.reset()
current_time = robot.GetTimeSinceReset()
com_vels, imu_rates, actions = [], [], []
while current_time < max_time:
start_time_robot = current_time
start_time_wall = time.time()
# Updates the controller behavior parameters.
lin_speed, ang_speed = (
0., 0., 0.), 0. #_generate_example_linear_angular_speed(current_time)
_update_controller_params(controller, lin_speed, ang_speed)
controller.update()
hybrid_action = controller.get_action()
com_vels.append(np.array(robot.GetBaseVelocity()).copy())
imu_rates.append(np.array(robot.GetBaseRollPitchYawRate()).copy())
actions.append(hybrid_action)
robot.Step(hybrid_action)
current_time = robot.GetTimeSinceReset()
expected_duration = current_time - start_time_robot
actual_duration = time.time() - start_time_wall
if actual_duration < expected_duration:
time.sleep(expected_duration - actual_duration)
if FLAGS.logdir:
logdir = os.path.join(FLAGS.logdir,
datetime.now().strftime('%Y_%m_%d_%H_%M_%S'))
os.makedirs(logdir)
np.savez(os.path.join(logdir, 'action.npz'),
action=actions,
com_vels=com_vels,
imu_rates=imu_rates)
logging.info("logged to: {}".format(logdir))
def main(argv):
"""Runs the locomotion controller example."""
del argv # unused
# Construct simulator
if FLAGS.show_gui and not FLAGS.use_real_robot:
p = bullet_client.BulletClient(connection_mode=pybullet.GUI)
else:
p = bullet_client.BulletClient(connection_mode=pybullet.DIRECT)
p.setPhysicsEngineParameter(numSolverIterations=30)
p.setTimeStep(0.001)
p.setGravity(0, 0, -9.8)
p.setPhysicsEngineParameter(enableConeFriction=0)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadURDF("plane.urdf")
# Construct robot class:
if FLAGS.use_real_robot:
from motion_imitation.robots import a1_robot
robot = a1_robot.A1Robot(
pybullet_client=p,
motor_control_mode=robot_config.MotorControlMode.HYBRID,
enable_action_interpolation=False,
time_step=0.002,
action_repeat=1)
else:
robot = a1.A1(p,
motor_control_mode=robot_config.MotorControlMode.HYBRID,
enable_action_interpolation=False,
reset_time=2,
time_step=0.002,
action_repeat=1)
controller = _setup_controller(robot)
controller.reset()
if FLAGS.use_gamepad:
gamepad = gamepad_reader.Gamepad()
command_function = gamepad.get_command
else:
command_function = _generate_example_linear_angular_speed
if FLAGS.logdir:
logdir = os.path.join(FLAGS.logdir,
datetime.now().strftime('%Y_%m_%d_%H_%M_%S'))
os.makedirs(logdir)
start_time = robot.GetTimeSinceReset()
current_time = start_time
com_vels, imu_rates, actions = [], [], []
while current_time - start_time < FLAGS.max_time_secs:
#time.sleep(0.0008) #on some fast computer, works better with sleep on real A1?
start_time_robot = current_time
start_time_wall = time.time()
# Updates the controller behavior parameters.
lin_speed, ang_speed, e_stop = command_function(current_time)
# print(lin_speed)
if e_stop:
logging.info("E-stop kicked, exiting...")
break
_update_controller_params(controller, lin_speed, ang_speed)
controller.update()
hybrid_action, _ = controller.get_action()
com_vels.append(np.array(robot.GetBaseVelocity()).copy())
imu_rates.append(np.array(robot.GetBaseRollPitchYawRate()).copy())
actions.append(hybrid_action)
robot.Step(hybrid_action)
current_time = robot.GetTimeSinceReset()
if not FLAGS.use_real_robot:
expected_duration = current_time - start_time_robot
actual_duration = time.time() - start_time_wall
if actual_duration < expected_duration:
time.sleep(expected_duration - actual_duration)
if FLAGS.use_gamepad:
gamepad.stop()
if FLAGS.logdir:
np.savez(os.path.join(logdir, 'action.npz'),
action=actions,
com_vels=com_vels,
imu_rates=imu_rates)
logging.info("logged to: {}".format(logdir))