1#The example to run the raibert controller in a Minitaur gym env.
2
3#blaze run :minitaur_raibert_controller_example  -- --log_path=/tmp/logs
4
5from __future__ import absolute_import
6from __future__ import division
7from __future__ import print_function
8
9import tf.compat.v1 as tf
10from pybullet_envs.minitaur.envs import minitaur_raibert_controller
11from pybullet_envs.minitaur.envs import minitaur_gym_env
12
13flags = tf.app.flags
14FLAGS = tf.app.flags.FLAGS
15
16flags.DEFINE_float("motor_kp", 1.0, "The position gain of the motor.")
17flags.DEFINE_float("motor_kd", 0.015, "The speed gain of the motor.")
18flags.DEFINE_float("control_latency", 0.006, "The latency between sensor measurement and action"
19                   " execution the robot.")
20flags.DEFINE_string("log_path", ".", "The directory to write the log file.")
21
22
23def speed(t):
24  max_speed = 0.35
25  t1 = 3
26  if t < t1:
27    return t / t1 * max_speed
28  else:
29    return -max_speed
30
31
32def main(argv):
33  del argv
34  env = minitaur_gym_env.MinitaurGymEnv(urdf_version=minitaur_gym_env.RAINBOW_DASH_V0_URDF_VERSION,
35                                        control_time_step=0.006,
36                                        action_repeat=6,
37                                        pd_latency=0.0,
38                                        control_latency=FLAGS.control_latency,
39                                        motor_kp=FLAGS.motor_kp,
40                                        motor_kd=FLAGS.motor_kd,
41                                        remove_default_joint_damping=True,
42                                        leg_model_enabled=False,
43                                        render=True,
44                                        on_rack=False,
45                                        accurate_motor_model_enabled=True,
46                                        log_path=FLAGS.log_path)
47  env.reset()
48
49  controller = minitaur_raibert_controller.MinitaurRaibertTrottingController(env.minitaur)
50
51  tstart = env.minitaur.GetTimeSinceReset()
52  for _ in range(1000):
53    t = env.minitaur.GetTimeSinceReset() - tstart
54    controller.behavior_parameters = (minitaur_raibert_controller.BehaviorParameters(
55        desired_forward_speed=speed(t)))
56    controller.update(t)
57    env.step(controller.get_action())
58
59  #env.close()
60
61
62if __name__ == "__main__":
63  tf.app.run(main)
64