def main(_):
traj = dict(np.load(FLAGS.traj_dir))
p = bullet_client.BulletClient(connection_mode=pybullet.DIRECT)
p.setPhysicsEngineParameter(numSolverIterations=30)
p.setTimeStep(0.001)
p.setGravity(0, 0, -10)
p.setPhysicsEngineParameter(enableConeFriction=0)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadURDF("plane.urdf")
robot = a1_robot.A1Robot(
p,
motor_control_mode=robot_config.MotorControlMode.HYBRID,
enable_action_interpolation=False,
reset_time=2)
# 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
input("Press Enter Key to Start...")
for action in traj['action'][:100]:
robot.Step(action)
time.sleep(0.01)
robot.Terminate()
def main(_):
logging.info(
"WARNING: this code executes low-level controller on the robot.")
logging.info("Make sure the robot is hang on rack before proceeding.")
input("Press enter to continue...")
# Construct sim env and real robot
p = bullet_client.BulletClient(connection_mode=pybullet.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
robot = a1_robot.A1Robot(pybullet_client=p, action_repeat=1)
# Move the motors slowly to initial position
robot.ReceiveObservation()
current_motor_angle = np.array(robot.GetMotorAngles())
desired_motor_angle = np.array([0., 0.9, -1.8] * 4)
for t in tqdm(range(300)):
blend_ratio = np.minimum(t / 200., 1)
action = (1 - blend_ratio
) * current_motor_angle + blend_ratio * desired_motor_angle
robot.Step(action, robot_config.MotorControlMode.POSITION)
time.sleep(0.005)
# Move the legs in a sinusoidal curve
for t in tqdm(range(1000)):
angle_hip = 0.9 + 0.2 * np.sin(2 * np.pi * FREQ * 0.01 * t)
angle_calf = -2 * angle_hip
action = np.array([0., angle_hip, angle_calf] * 4)
robot.Step(action, robot_config.MotorControlMode.POSITION)
time.sleep(0.007)
# print(robot.GetFootContacts())
print(robot.GetBaseVelocity())
robot.Terminate()
def _run_example():
"""Runs the locomotion controller example."""
p = bullet_client.BulletClient(connection_mode=pybullet.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
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)
controller = _setup_controller(robot)
controller.reset()
actions = []
states = []
start_time = robot.GetTimeSinceReset()
current_time = start_time
while current_time - start_time < FLAGS.max_time_secs:
# Updates the controller behavior parameters.
lin_speed, ang_speed = _generate_example_linear_angular_speed(
current_time)
_update_controller_params(controller, lin_speed, ang_speed)
# Needed before every call to get_action().
controller.update()
current_time = robot.GetTimeSinceReset()
hybrid_action, info = controller.get_action()
actions.append(hybrid_action)
robot.Step(hybrid_action)
states.append(
dict(timestamp=robot.GetTimeSinceReset(),
base_rpy=robot.GetBaseRollPitchYaw(),
motor_angles=robot.GetMotorAngles(),
base_vel=robot.GetBaseVelocity(),
base_vels_body_frame=controller.state_estimator.
com_velocity_body_frame,
base_rpy_rate=robot.GetBaseRollPitchYawRate(),
motor_vels=robot.GetMotorVelocities(),
contacts=robot.GetFootContacts(),
qp_sol=info['qp_sol']))
# robot.Reset()
robot.Terminate()
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)
pickle.dump(states, open(os.path.join(logdir, 'states.pkl'), 'wb'))
logging.info("logged to: {}".format(logdir))
def _run_example():
"""Runs the locomotion controller example."""
p = bullet_client.BulletClient(connection_mode=pybullet.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
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)
controller = _setup_controller(robot)
controller.reset()
actions = []
raw_states = []
timestamps, com_vels, imu_rates = [], [], []
start_time = robot.GetTimeSinceReset()
current_time = start_time
while current_time - start_time < FLAGS.max_time_secs:
# Updates the controller behavior parameters.
lin_speed, ang_speed = (0., 0., 0.), 0.
_update_controller_params(controller, lin_speed, ang_speed)
# Needed before every call to get_action().
controller.update()
hybrid_action = controller.get_action()
raw_states.append(copy.deepcopy(robot._raw_state)) # pylint:disable=protected-access
com_vels.append(robot.GetBaseVelocity().copy())
imu_rates.append(robot.GetBaseRollPitchYawRate().copy())
actions.append(hybrid_action)
robot.Step(hybrid_action)
current_time = robot.GetTimeSinceReset()
timestamps.append(current_time)
time.sleep(0.003)
robot.Reset()
robot.Terminate()
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,
timestamps=timestamps)
pickle.dump(raw_states, open(os.path.join(logdir, 'raw_states.pkl'), 'wb'))
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))