summaryrefslogtreecommitdiff
path: root/examples/pybullet/gym/pybullet_envs/minitaur/actuatornet/minitaur_raibert_controller_example.py
diff options
context:
space:
mode:
Diffstat (limited to 'examples/pybullet/gym/pybullet_envs/minitaur/actuatornet/minitaur_raibert_controller_example.py')
-rw-r--r--examples/pybullet/gym/pybullet_envs/minitaur/actuatornet/minitaur_raibert_controller_example.py62
1 files changed, 29 insertions, 33 deletions
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/actuatornet/minitaur_raibert_controller_example.py b/examples/pybullet/gym/pybullet_envs/minitaur/actuatornet/minitaur_raibert_controller_example.py
index 0be299a75..40e6b4799 100644
--- a/examples/pybullet/gym/pybullet_envs/minitaur/actuatornet/minitaur_raibert_controller_example.py
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/actuatornet/minitaur_raibert_controller_example.py
@@ -15,9 +15,8 @@ FLAGS = tf.app.flags.FLAGS
flags.DEFINE_float("motor_kp", 1.0, "The position gain of the motor.")
flags.DEFINE_float("motor_kd", 0.015, "The speed gain of the motor.")
-flags.DEFINE_float(
- "control_latency", 0.006, "The latency between sensor measurement and action"
- " execution the robot.")
+flags.DEFINE_float("control_latency", 0.006, "The latency between sensor measurement and action"
+ " execution the robot.")
flags.DEFINE_string("log_path", ".", "The directory to write the log file.")
@@ -31,37 +30,34 @@ def speed(t):
def main(argv):
- del argv
- env = minitaur_gym_env.MinitaurGymEnv(
- urdf_version=minitaur_gym_env.RAINBOW_DASH_V0_URDF_VERSION,
- control_time_step=0.006,
- action_repeat=6,
- pd_latency=0.0,
- control_latency=FLAGS.control_latency,
- motor_kp=FLAGS.motor_kp,
- motor_kd=FLAGS.motor_kd,
- remove_default_joint_damping=True,
- leg_model_enabled=False,
- render=True,
- on_rack=False,
- accurate_motor_model_enabled=True,
- log_path=FLAGS.log_path)
- env.reset()
+ del argv
+ env = minitaur_gym_env.MinitaurGymEnv(urdf_version=minitaur_gym_env.RAINBOW_DASH_V0_URDF_VERSION,
+ control_time_step=0.006,
+ action_repeat=6,
+ pd_latency=0.0,
+ control_latency=FLAGS.control_latency,
+ motor_kp=FLAGS.motor_kp,
+ motor_kd=FLAGS.motor_kd,
+ remove_default_joint_damping=True,
+ leg_model_enabled=False,
+ render=True,
+ on_rack=False,
+ accurate_motor_model_enabled=True,
+ log_path=FLAGS.log_path)
+ env.reset()
+
+ controller = minitaur_raibert_controller.MinitaurRaibertTrottingController(env.minitaur)
+
+ tstart = env.minitaur.GetTimeSinceReset()
+ for _ in range(1000):
+ t = env.minitaur.GetTimeSinceReset() - tstart
+ controller.behavior_parameters = (minitaur_raibert_controller.BehaviorParameters(
+ desired_forward_speed=speed(t)))
+ controller.update(t)
+ env.step(controller.get_action())
+
+ #env.close()
- controller = minitaur_raibert_controller.MinitaurRaibertTrottingController(
- env.minitaur)
-
- tstart = env.minitaur.GetTimeSinceReset()
- for _ in range(1000):
- t = env.minitaur.GetTimeSinceReset() - tstart
- controller.behavior_parameters = (
- minitaur_raibert_controller.BehaviorParameters(
- desired_forward_speed=speed(t)))
- controller.update(t)
- env.step(controller.get_action())
-
- #env.close()
if __name__ == "__main__":
tf.app.run(main)
-