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