def SinePolicyExample(log_path=None):
"""An example of minitaur walking with a sine gait.
Args:
log_path: The directory that the log files are written to. If log_path is
None, no logs will be written.
"""
environment = minitaur_gym_env.MinitaurGymEnv(
urdf_version=minitaur_gym_env.DERPY_V0_URDF_VERSION,
render=True,
motor_velocity_limit=np.inf,
pd_control_enabled=True,
hard_reset=False,
on_rack=False,
log_path=log_path)
sum_reward = 0
steps = 20000
amplitude_1_bound = 0.5
amplitude_2_bound = 0.5
speed = 40
for step_counter in range(steps):
time_step = 0.01
t = step_counter * time_step
amplitude1 = amplitude_1_bound
amplitude2 = amplitude_2_bound
steering_amplitude = 0
if t < 10:
steering_amplitude = 0.5
elif t < 20:
steering_amplitude = -0.5
else:
steering_amplitude = 0
# Applying asymmetrical sine gaits to different legs can steer the minitaur.
a1 = math.sin(t * speed) * (amplitude1 + steering_amplitude)
a2 = math.sin(t * speed + math.pi) * (amplitude1 - steering_amplitude)
a3 = math.sin(t * speed) * amplitude2
a4 = math.sin(t * speed + math.pi) * amplitude2
action = [a1, a2, a2, a1, a3, a4, a4, a3]
_, reward, done, _ = environment.step(action)
sum_reward += reward
if done:
tf.logging.info("Return is {}".format(sum_reward))
environment.reset()
def ResetPoseExample(log_path=None):
"""An example that the minitaur stands still using the reset pose."""
steps = 10000
environment = minitaur_gym_env.MinitaurGymEnv(
urdf_version=minitaur_gym_env.DERPY_V0_URDF_VERSION,
render=True,
leg_model_enabled=False,
motor_velocity_limit=np.inf,
pd_control_enabled=True,
accurate_motor_model_enabled=True,
motor_overheat_protection=True,
hard_reset=False,
log_path=log_path)
action = [math.pi / 2] * 8
for _ in range(steps):
_, _, done, _ = environment.step(action)
if done:
break
def SineStandExample(log_path=None):
"""An example of minitaur standing and squatting on the floor.
To validate the accurate motor model we command the robot and sit and stand up
periodically in both simulation and experiment. We compare the measured motor
trajectories, torques and gains. The results are at:
https://colab.corp.google.com/v2/notebook#fileId=0BxTIAnWh1hb_ZnkyYWtNQ1RYdkU&scrollTo=ZGFMl84kKqRx
Args:
log_path: The directory that the log files are written to. If log_path is
None, no logs will be written.
"""
environment = minitaur_gym_env.MinitaurGymEnv(
urdf_version=minitaur_gym_env.RAINBOW_DASH_V0_URDF_VERSION,
render=True,
leg_model_enabled=False,
motor_velocity_limit=np.inf,
motor_overheat_protection=True,
accurate_motor_model_enabled=True,
motor_kp=1.20,
motor_kd=0.02,
on_rack=False,
log_path=log_path)
steps = 1000
amplitude = 0.5
speed = 3
actions_and_observations = []
for step_counter in range(steps):
# Matches the internal timestep.
time_step = 0.01
t = step_counter * time_step
current_row = [t]
action = [math.sin(speed * t) * amplitude + math.pi / 2] * 8
current_row.extend(action)
observation, _, _, _ = environment.step(action)
current_row.extend(observation.tolist())
actions_and_observations.append(current_row)
if FLAGS.output_filename is not None:
WriteToCSV(FLAGS.output_filename, actions_and_observations)
def MotorOverheatExample(log_path=None):
"""An example of minitaur motor overheat protection is triggered.
The minitaur is leaning forward and the motors are getting obove threshold
torques. The overheat protection will be triggered in ~1 sec.
Args:
log_path: The directory that the log files are written to. If log_path is
None, no logs will be written.
"""
environment = minitaur_gym_env.MinitaurGymEnv(
urdf_version=minitaur_gym_env.DERPY_V0_URDF_VERSION,
render=True,
leg_model_enabled=False,
motor_velocity_limit=np.inf,
motor_overheat_protection=True,
accurate_motor_model_enabled=True,
motor_kp=1.20,
motor_kd=0.00,
on_rack=False,
log_path=log_path)
action = [2.0] * 8
for i in range(8):
action[i] = 2.0 - 0.5 * (-1 if i % 2 == 0 else 1) * (-1 if i < 4 else 1)
steps = 500
actions_and_observations = []
for step_counter in range(steps):
# Matches the internal timestep.
time_step = 0.01
t = step_counter * time_step
current_row = [t]
current_row.extend(action)
observation, _, _, _ = environment.step(action)
current_row.extend(observation.tolist())
actions_and_observations.append(current_row)
if FLAGS.output_filename is not None:
WriteToCSV(FLAGS.output_filename, actions_and_observations)
def main(argv):
del argv
try:
env = minitaur_gym_env.MinitaurGymEnv(
urdf_version=minitaur_gym_env.DERPY_V0_URDF_VERSION,
control_time_step=0.004,
action_repeat=1,
pd_latency=0.003,
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,
torque_control_enabled=CONTROL_TORQUE,
log_path=FLAGS.log_path)
env.reset()
t_reg = 0
param_init() # param_init
controller = minitaur_raibert_controller.MinitaurRaibertTrottingController(
env.minitaur)
tstart = env.minitaur.GetTimeSinceReset()
while 1:
t = env.minitaur.GetTimeSinceReset() - tstart
dt = t - t_reg
# print(dt)
# -----------------------------------state measurement
temp = env.get_minitaur_motor_angles() * 57.3
leg[0].m_angle[0] = temp[0]
leg[0].m_angle[1] = temp[1]
leg[1].m_angle[0] = temp[2]
leg[1].m_angle[1] = temp[3]
leg[2].m_angle[0] = temp[5]
leg[2].m_angle[1] = temp[4]
leg[3].m_angle[0] = temp[7]
leg[3].m_angle[1] = temp[6]
temp = env.get_minitaur_motor_velocities() * 57.3
leg[0].m_angle_spd[0] = temp[0]
leg[0].m_angle_spd[1] = temp[1]
leg[1].m_angle_spd[0] = temp[2]
leg[1].m_angle_spd[1] = temp[3]
leg[2].m_angle_spd[0] = temp[5]
leg[2].m_angle_spd[1] = temp[4]
leg[3].m_angle_spd[0] = temp[7]
leg[3].m_angle_spd[1] = temp[6]
# forward est epos
en_att_jacobi = 1
off_att_jacobi = 6.6
leg[0].mangle_to_epose(brain.att[0] * en_att_jacobi +
off_att_jacobi)
leg[1].mangle_to_epose(brain.att[0] * en_att_jacobi +
off_att_jacobi)
leg[2].mangle_to_epose(brain.att[0] * en_att_jacobi +
off_att_jacobi)
leg[3].mangle_to_epose(brain.att[0] * en_att_jacobi +
off_att_jacobi)
# invert_calculate for test only
leg[0].epose_to_mangle(leg[0].pos_est[0], leg[0].pos_est[1])
leg[1].epose_to_mangle(leg[1].pos_est[0], leg[1].pos_est[1])
leg[2].epose_to_mangle(leg[2].pos_est[0], leg[2].pos_est[1])
leg[3].epose_to_mangle(leg[3].pos_est[0], leg[3].pos_est[1])
leg[0].cal_jacobi(brain.att[0] * en_att_jacobi + off_att_jacobi)
leg[1].cal_jacobi(brain.att[0] * en_att_jacobi + off_att_jacobi)
leg[2].cal_jacobi(brain.att[0] * en_att_jacobi + off_att_jacobi)
leg[3].cal_jacobi(brain.att[0] * en_att_jacobi + off_att_jacobi)
temp = env.get_minitaur_motor_torques()
leg[0].m_torque_est[0] = temp[0]
leg[0].m_torque_est[1] = temp[1]
leg[1].m_torque_est[0] = temp[2]
leg[1].m_torque_est[1] = temp[3]
leg[2].m_torque_est[0] = temp[5]
leg[2].m_torque_est[1] = temp[4]
leg[3].m_torque_est[0] = temp[7]
leg[3].m_torque_est[1] = temp[6]
temp = env.minitaur.GetBaseRollPitchYaw() * 57.3
brain.att[0] = -temp[1] # pitch
brain.att[1] = -temp[0] # roll
brain.att[2] = -temp[2] # yaw
temp = env.minitaur.GetBaseRollPitchYawRate()
brain.rate[0] = -temp[1] # pitch rate
brain.rate[1] = -temp[0] # roll rate
brain.rate[2] = -temp[2] # yaw rate
# print('att: ',brain.att)
# -------------------------------state machine
main_sfm(dt)
# ----------------------------------controll
if CONTROL_TORQUE == False:
env.step(
controller.set_motor_angles_test(
leg[0].m_angle_cmd[0], leg[0].m_angle_cmd[1],
leg[1].m_angle_cmd[0], leg[1].m_angle_cmd[1],
leg[2].m_angle_cmd[1], leg[2].m_angle_cmd[0],
leg[3].m_angle_cmd[1],
leg[3].m_angle_cmd[0])) # motor angle test
if CONTROL_TORQUE == True:
# print('MODE: ', brain.control_mode)
for i in range(0, 4):
leg[i].vmc_control_test(dt)
if brain.control_mode[i] == 'TORQUE_POS':
m_torque_cmd = [0, 0]
(m_torque_cmd[0],
m_torque_cmd[1]) = leg[i].force_to_mtorque(
leg[i].force_cmd[0], leg[i].force_cmd[1])
leg[i].m_angle_cmd[
0] = leg[i].m_angle_cmd[0] + m_torque_cmd[0]
leg[i].m_angle_cmd[
1] = leg[i].m_angle_cmd[1] + m_torque_cmd[1]
(leg[i].m_torque_cmd[0],
leg[i].m_torque_cmd[1]) = leg[0].motor_pos_control()
elif brain.control_mode[i] == 'TORQUE':
(leg[i].m_torque_cmd[0],
leg[i].m_torque_cmd[1]) = leg[i].force_to_mtorque(
leg[i].force_cmd[0], leg[i].force_cmd[1])
elif brain.control_mode[
i] == 'MOTOR_POS': # motor control mine
(leg[i].m_torque_cmd[0],
leg[i].m_torque_cmd[1]) = leg[i].motor_pos_control()
# torque output to motor
env.step([
leg[0].m_torque_cmd[0], leg[0].m_torque_cmd[1],
leg[1].m_torque_cmd[0], leg[1].m_torque_cmd[1],
leg[2].m_torque_cmd[1], leg[2].m_torque_cmd[0],
leg[3].m_torque_cmd[1], leg[3].m_torque_cmd[0]
]) # motor torque test
t_reg = t
finally:
env.close()
Leg_Param(id=1, torque_flag=[1, 1], m_kp=INIT_KP, m_kd=INIT_KD), # BL
Leg_Param(id=2, torque_flag=[1, 1], m_kp=INIT_KP, m_kd=INIT_KD), # FR
Leg_Param(id=3, torque_flag=[1, 1], m_kp=INIT_KP, m_kd=INIT_KD)
] # BR 1 3
brain = Brain_Param()
plot = Plot()
env = minitaur_gym_env.MinitaurGymEnv(
# urdf_version = minitaur_gym_env.DEFAULT_URDF_VERSION,
# urdf_version = minitaur_gym_env.DERPY_V0_URDF_VERSION,
urdf_version=minitaur_gym_env.RAINBOW_DASH_V0_URDF_VERSION,
control_time_step=0.004,
action_repeat=1,
pd_latency=0.000,
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,
torque_control_enabled=CONTROL_TORQUE,
log_path=FLAGS.log_path)
env.reset()
controller = minitaur_raibert_controller.MinitaurRaibertTrottingController(
env.minitaur)
def param_init():