def __init__(self, max_angle, time_step=0.02, gui=False):
self.gui = gui
self.max_angle = max_angle
if self.gui:
self.minitaur_env = 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,
reflection=False)
else:
self.minitaur_env = minitaur_gym_env.MinitaurGymEnv(
urdf_version=minitaur_gym_env.DERPY_V0_URDF_VERSION,
render=False,
motor_velocity_limit=np.inf,
pd_control_enabled=True,
hard_reset=False,
on_rack=False)
self.minitaur_env.minitaur.time_step = time_step
self.p = self.minitaur_env._pybullet_client
# self.minitaur_env.minitaur.SetFootFriction(1.0)
# self.minitaur_env.minitaur.SetFootRestitution(0.1)
# self.prim_lib = np.load('prim_lib.npy')
# textureId = self.p.loadTexture("heightmaps/table.png")
# self.p.changeVisualShape(self.minitaur_env.ground_id, -1, textureUniqueId=textureId)
self.terraintextureId = self.p.loadTexture("heightmaps/oak-wood.png")
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()
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())
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)
time.sleep(1. / 100.)
sum_reward += reward
if done:
tf.logging.info("Return is {}".format(sum_reward))
environment.reset()
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)
time.sleep(1. / 100.)
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)
time.sleep(1. / 100.)
if FLAGS.output_filename is not None:
WriteToCSV(FLAGS.output_filename, actions_and_observations)
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)
time.sleep(1. / 100.)
if done:
break
def hzd_policy(vircons=None,
phasevars=None,
log_file=None,
sim_cycles=4,
feedforward=False):
"""An example of minitaur locomoting with a virtual constraints.
Args:
vircons: A numpy array with bezier coefficients to generate optimal leg
trajectories.
phasevars: A numpy array of end time values for a stepping trajectory.
log_file: The directory into which log files are written. If log_path is
None, no data logging takes place.
sim_cycles: number of trotting steps to simulate.
feedforward: Boolean to use augmented bezier coefficients. The original
bezier coeffcients capture only the optimal leg trajectory information
(without corresponding torque info). Augmented bezier coefficients are
generated by combining the optimal torque information into the leg
trajectory info. Using this improves tracking performance.
"""
time_step = 0.01
sum_reward = 0
num_outputs = len(vircons[0])
init_motor_pos = []
for output in range(num_outputs):
coeffs = vircons[0, output, :]
output_val_at_0, _ = bezier_interpolation(coeffs, 0)
init_motor_pos.append(float(output_val_at_0))
if feedforward:
init_motor_pos = list(np.multiply(MOTOR_DIRECTION, init_motor_pos))
else:
init_motor_pos = list(np.multiply(LEGSPACE_DIRECTION, init_motor_pos))
init_motor_pos = leg_pose_to_motor_angles(init_motor_pos)
init_motor_pos = np.array(init_motor_pos, dtype='float32')
environment = minitaur_gym_env.MinitaurGymEnv(
urdf_version=minitaur_gym_env.RAINBOW_DASH_V0_URDF_VERSION,
render=True,
motor_velocity_limit=np.inf,
accurate_motor_model_enabled=True,
remove_default_joint_damping=False,
pd_control_enabled=True,
leg_model_enabled=False,
hard_reset=False,
on_rack=False,
motor_kp=1.0,
motor_kd=0.005,
control_latency=0.001,
pd_latency=0.001,
log_path=None,
env_randomizer=None)
environment.reset(initial_motor_angles=init_motor_pos, reset_duration=1.0)
time.sleep(3.)
cycle_time = float(sum(phasevars))
logging.info('Gait Cycle Time is %f', cycle_time)
steps = int((sim_cycles * cycle_time) / time_step)
actions_and_observations = []
for step_counter in range(steps):
t = step_counter * time_step
current_row = [t]
t_in_cycle = t % cycle_time
if 0 <= t_in_cycle <= float(phasevars[0]):
tau = normalize(t_in_cycle, float(phasevars[0]), 0)
phase = 0
elif float(phasevars[0]) <= t_in_cycle <= cycle_time:
tau = normalize(t_in_cycle, cycle_time, float(phasevars[0]))
phase = 1
else:
ValueError('Tau must lie between 0 and ' + str(cycle_time))
action = []
action_vel = []
for output in range(num_outputs):
coeffs = vircons[phase, output, :]
output_val_at_tau, output_vel_at_tau = bezier_interpolation(
coeffs, tau)
action.append(float(output_val_at_tau))
action_vel.append(float(output_vel_at_tau))
if feedforward:
action = list(np.multiply(MOTOR_DIRECTION, action))
action_vel = list(np.multiply(MOTOR_DIRECTION, action_vel))
else:
action = leg_pose_to_motor_angles(
list(np.multiply(LEGSPACE_DIRECTION, action)))
action_vel = leg_pose_to_motor_angles(
list(np.multiply(LEGSPACE_DIRECTION, action_vel)))
current_row.extend(action)
current_row.extend(action_vel)
observation, reward, done, _ = environment.step(action)
observation[16:24] = [
observation[16 + i] * MOTOR_DIRECTION[i] for i in range(NUM_MOTORS)
]
current_row.extend(observation.tolist())
base_angles = environment.minitaur.GetBaseRollPitchYaw()
current_row.extend(base_angles.tolist())
actions_and_observations.append(current_row)
sum_reward += reward
if done or (step_counter == (steps - 1)):
logging.info('End of Sim')
if log_file is not None:
write_csv(log_file, actions_and_observations)
tf.logging.info('Return is {}'.format(sum_reward))
environment.reset()
def gait_library_policy(gaits,
log_file=None,
steps_per_gait=4,
feedforward=False):
"""An example of minitaur locomoting using hzd gai libraries.
Args:
gaits: A dictionary of fileids indexed by steplength value.
log_file: The directory into which log files are written. If log_path is
None, no data logging takes place.
steps_per_gait: no of steps the robot will walk at a particular steplength
before switching to the next one.
feedforward: boolen to use augmented bezier coefficients. The original
bezier coeffcients capture only the optimal leg trajectory information
(without corresponding torque info). Augmented bezier coefficients are
generated by combining the optimal torque information into the leg
trajectory info. Using this improves tracking performance.
"""
## number of distinct phases in the gait cycle
num_phases = 2
for steplen, steplenfile in gaits.items():
## Load Bezier Parameters and Phase Timings Data
vircons, phasevars = load_gait_txt(steplenfile, num_phases=num_phases)
gaits[steplen] = [vircons, phasevars]
steplens = list(gaits.keys())
time_step = 0.01
sum_reward = 0
num_outputs = len(gaits['0.00'][0][0])
init_motor_pos = []
for output in range(num_outputs):
coeffs = gaits['0.00'][0][0, output, :]
output_val_at_0, _ = bezier_interpolation(coeffs, 0)
init_motor_pos.append(float(output_val_at_0))
if feedforward:
init_motor_pos = list(np.multiply(MOTOR_DIRECTION, init_motor_pos))
else:
init_motor_pos = list(np.multiply(LEGSPACE_DIRECTION, init_motor_pos))
init_motor_pos = leg_pose_to_motor_angles(init_motor_pos)
init_motor_pos = np.array(init_motor_pos, dtype='float32')
logging.info(init_motor_pos * (180 / math.pi))
environment = minitaur_gym_env.MinitaurGymEnv(
urdf_version=minitaur_gym_env.RAINBOW_DASH_V0_URDF_VERSION,
render=True,
motor_velocity_limit=np.inf,
accurate_motor_model_enabled=True,
remove_default_joint_damping=False,
pd_control_enabled=True,
leg_model_enabled=False,
hard_reset=False,
on_rack=False,
motor_kp=1,
motor_kd=0.005,
control_latency=0.001,
pd_latency=0.001,
log_path=None,
env_randomizer=None)
environment.reset(initial_motor_angles=init_motor_pos, reset_duration=1.0)
time.sleep(3.)
cycle_times = [float(sum(gaits[steplen][1][:])) for steplen in gaits]
cycle_timesum = np.cumsum(cycle_times)
gait_id = [i for i in range(len(steplens))]
steps = int((steps_per_gait * sum(cycle_times)) / time_step)
actions_and_observations = []
cycles = 0
last_phase = 0
for step_counter in range(steps):
t = step_counter * time_step
current_row = [t]
cycle_time = cycle_times[cycles]
t_in_cycle = (t - cycle_timesum[cycles]) % (
cycle_time) if cycles > 0 else t % (cycle_time)
if 0 <= t_in_cycle <= float(gaits[steplens[gait_id[cycles]]][1][0]):
tau = normalize(t_in_cycle,
float(gaits[steplens[gait_id[cycles]]][1][0]), 0)
phase = 0
if last_phase == 1:
logging.info('Gait Cycle Time was %f', cycle_time)
cycles += 1
if cycles == len(steplens):
break
last_phase = phase
elif float(gaits[steplens[gait_id[cycles]]][1]
[0]) <= t_in_cycle <= cycle_time:
tau = normalize(t_in_cycle, cycle_time,
float(gaits[steplens[gait_id[cycles]]][1][0]))
phase = 1
last_phase = 1
else:
ValueError('Tau must lie between 0 and ' + str(cycle_time))
action = []
action_vel = []
for output in range(num_outputs):
coeffs = gaits[steplens[gait_id[cycles]]][0][phase, output, :]
output_val_at_tau, output_vel_at_tau = bezier_interpolation(
coeffs, tau)
action.append(float(output_val_at_tau))
action_vel.append(float(output_vel_at_tau))
if feedforward:
action = list(np.multiply(MOTOR_DIRECTION, action))
action_vel = list(np.multiply(MOTOR_DIRECTION, action_vel))
else:
action = leg_pose_to_motor_angles(
list(np.multiply(LEGSPACE_DIRECTION, action)))
action_vel = leg_pose_to_motor_angles(
list(np.multiply(LEGSPACE_DIRECTION, action_vel)))
current_row.extend(action)
current_row.extend(action_vel)
observation, reward, done, _ = environment.step(action)
observation[16:24] = [
observation[16 + i] * MOTOR_DIRECTION[i] for i in range(NUM_MOTORS)
]
current_row.extend(observation.tolist())
base_angles = environment.minitaur.GetBaseRollPitchYaw()
current_row.extend(base_angles.tolist())
actions_and_observations.append(current_row)
sum_reward += reward
if done or (step_counter == (steps - 1)) or (cycles == steps_per_gait *
len(steplens)):
logging.info('End of Sim')
if log_file is not None:
write_csv(log_file, actions_and_observations)
def HZDPolicy(vircons=None,
phasevars=None,
log_file=None,
sim_cycles=4,
feedforward=False,
randomize=False):
"""An example of minitaur locomoting with a virtual constraints (a.k.a hybrid zero dynamics - based gait).
Args:
vircons: A numpy array with bezier coefficients to generate optimal leg trajectories.
phasevars: A numpy array of end time values for a stepping trajectory.
sim_cycles: number of trotting steps to simulate.
feedforward: boolen to use augmented bezier coefficients.
The original bezier coeffcients capture only the optimal leg trajectory information (without corresponding torque info).
Augmented bezier coefficients are generated by combining the optimal torque information into the leg trajectory info.
Using this improves tracking performance.
randomize: boolean to invoke the RandomizerEnv() and randomizing the model parameters. Turn on for robustness testing.
"""
time_step = 0.01
sum_reward = 0
num_outputs = len(vircons[0])
INIT_MOTOR_POS = []
for output in range(num_outputs):
coeffs = vircons[0, output, :]
output_val_at_0, _ = bezier_interpolation(coeffs, 0)
INIT_MOTOR_POS.append(float(output_val_at_0))
# INIT_MOTOR_POS = list( np.multiply(LEGSPACE_DIRECTION, INIT_MOTOR_POS) )
# INIT_MOTOR_POS = leg_pose_to_motor_angles(INIT_MOTOR_POS)
if feedforward:
INIT_MOTOR_POS = list(np.multiply(MOTOR_DIRECTION, INIT_MOTOR_POS))
else:
INIT_MOTOR_POS = list(np.multiply(LEGSPACE_DIRECTION, INIT_MOTOR_POS))
INIT_MOTOR_POS = leg_pose_to_motor_angles(INIT_MOTOR_POS)
INIT_MOTOR_POS = np.array(INIT_MOTOR_POS, dtype='float32')
print(INIT_MOTOR_POS * (180 / math.pi))
randomizer = (
minitaur_env_randomizer.MinitaurEnvRandomizer()) if randomize else None
environment = minitaur_gym_env.MinitaurGymEnv(
urdf_version=minitaur_gym_env.RAINBOW_DASH_V0_URDF_VERSION,
render=True,
motor_velocity_limit=np.inf,
accurate_motor_model_enabled=True,
remove_default_joint_damping=False,
pd_control_enabled=True,
leg_model_enabled=False,
hard_reset=False,
on_rack=False,
motor_kp=1.0,
motor_kd=0.005,
# motor_ki= 0.25, # 0, #
control_latency=0.001,
pd_latency=0.001,
log_path=None,
env_randomizer=randomizer)
# environment.minitaur.SetFootFriction(0.1)
# environment.minitaur.SetFootRestitution(0.6)
# if randomize:
# randomizer = minitaur_env_randomizer_from_config.MinitaurEnvRandomizerFromConfig()
# randomizer.randomize_env(environment)
environment.reset(initial_motor_angles=INIT_MOTOR_POS, reset_duration=1.0)
time.sleep(3.)
cycle_time = float(sum(phasevars))
print('Gait Cycle Time is ' + str(cycle_time))
steps = int((sim_cycles * cycle_time) / time_step)
actions_and_observations = []
for step_counter in range(steps):
t = step_counter * time_step
current_row = [t]
t_in_cycle = t % cycle_time
if 0 <= t_in_cycle <= float(phasevars[0]):
tau = normalize(t_in_cycle, float(phasevars[0]), 0)
phase = 0
elif float(phasevars[0]) <= t_in_cycle <= cycle_time:
tau = normalize(t_in_cycle, cycle_time, float(phasevars[0]))
phase = 1
else:
ValueError('Tau must lie between 0 and ' + str(cycle_time))
action = []
action_vel = []
for output in range(num_outputs):
coeffs = vircons[phase, output, :]
output_val_at_tau, output_vel_at_tau = bezier_interpolation(
coeffs, tau)
action.append(float(output_val_at_tau))
action_vel.append(float(output_vel_at_tau))
# action = leg_pose_to_motor_angles( list( np.multiply(LEGSPACE_DIRECTION, action) ) )
# action_vel = leg_pose_to_motor_angles( list( np.multiply(LEGSPACE_DIRECTION, action_vel) ) )
if feedforward:
action = list(np.multiply(MOTOR_DIRECTION, action))
action_vel = list(np.multiply(MOTOR_DIRECTION, action_vel))
else:
action = leg_pose_to_motor_angles(
list(np.multiply(LEGSPACE_DIRECTION, action)))
action_vel = leg_pose_to_motor_angles(
list(np.multiply(LEGSPACE_DIRECTION, action_vel)))
current_row.extend(action)
current_row.extend(action_vel)
# print(action)
observation, reward, done, _ = environment.step(action)
observation[16:24] = [
observation[16 + i] * environment.minitaur._motor_direction[i]
for i in range(NUM_MOTORS)
]
current_row.extend(observation.tolist())
base_angles = environment.minitaur.GetBaseRollPitchYaw()
current_row.extend(base_angles.tolist())
actions_and_observations.append(current_row)
#time.sleep(1./10.)
sum_reward += reward
if done or (step_counter == (steps - 1)):
print('End of Sim')
if log_file is not None:
filename = os.path.join(currentdir, log_file)
write_csv(filename, actions_and_observations)
tf.logging.info("Return is {}".format(sum_reward))
environment.reset()
def GaitLibraryPolicy(gaits,
log_file=None,
steps_per_gait=4,
randomize=False,
feedforward=False):
"""An example of minitaur locomoting using a
virtual constraints (a.k.a hybrid zero dynamics - based gait) gait library.
Args:
gaits: A dictionary of fileids indexed by steplength value.
log_path: The directory into which log files are written. If log_path is None, no data logging takes place.
steps_per_gait: no of steps the robot will walk at a particular steplength before switching to the next one.
randomize: boolean to invoke the RandomizerEnv() and randomizing the model parameters. Turn on for robustness testing.
feedforward: boolen to use augmented bezier coefficients.
The original bezier coeffcients capture only the optimal leg trajectory information (without corresponding torque info).
Augmented bezier coefficients are generated by combining the optimal torque information into the leg trajectory info.
Using this improves tracking performance.
"""
## number of distinct phases in the gait cycle
num_phases = 2
for steplen, steplenfile in gaits.items():
## Load Bezier Parameters and Phase Timings Data
vircons, phasevars = load_gait_txt(steplenfile, num_phases=num_phases)
gaits[steplen] = [vircons, phasevars]
steplens = list(gaits.keys())
time_step = 0.01
sum_reward = 0
num_outputs = len(gaits['0.00'][0][0])
INIT_MOTOR_POS = []
for output in range(num_outputs):
coeffs = gaits['0.00'][0][0, output, :]
output_val_at_0, _ = bezier_interpolation(coeffs, 0)
INIT_MOTOR_POS.append(float(output_val_at_0))
if feedforward:
INIT_MOTOR_POS = list(np.multiply(MOTOR_DIRECTION, INIT_MOTOR_POS))
else:
INIT_MOTOR_POS = list(np.multiply(LEGSPACE_DIRECTION, INIT_MOTOR_POS))
INIT_MOTOR_POS = leg_pose_to_motor_angles(INIT_MOTOR_POS)
INIT_MOTOR_POS = np.array(INIT_MOTOR_POS, dtype='float32')
print(INIT_MOTOR_POS * (180 / math.pi))
randomizer = (
minitaur_env_randomizer.MinitaurEnvRandomizer()) if randomize else None
environment = minitaur_gym_env.MinitaurGymEnv(
urdf_version=minitaur_gym_env.RAINBOW_DASH_V0_URDF_VERSION,
render=True,
motor_velocity_limit=np.inf,
accurate_motor_model_enabled=True,
remove_default_joint_damping=False,
pd_control_enabled=True,
leg_model_enabled=False,
hard_reset=False,
on_rack=False,
motor_kp=1,
motor_kd=0.005,
# motor_ki= 0,
control_latency=0.001,
pd_latency=0.001,
log_path=None,
env_randomizer=randomizer)
# environment.minitaur.SetFootFriction(0.1)
# environment.minitaur.SetFootRestitution(0.6)
environment.reset(initial_motor_angles=INIT_MOTOR_POS, reset_duration=1.0)
time.sleep(3.)
cycle_times = [
float(sum(gaits[steplen][1][:])) for steplen in gaits
for step in range(steps_per_gait)
]
cycle_timesum = np.cumsum(cycle_times)
gait_id = [
i for i in range(len(steplens)) for step in range(steps_per_gait)
]
steps = int((steps_per_gait * sum(cycle_times)) / time_step)
actions_and_observations = []
cycles = 0
for step_counter in range(steps):
t = step_counter * time_step
current_row = [t]
cycle_time = cycle_times[cycles]
t_in_cycle = (t - cycle_timesum[cycles]) % (
cycle_time) if cycles > 0 else t % (cycle_time)
if 0 <= t_in_cycle <= float(gaits[steplens[gait_id[cycles]]][1][0]):
tau = normalize(t_in_cycle,
float(gaits[steplens[gait_id[cycles]]][1][0]), 0)
phase = 0
# print('Tau is {}s'.format(tau))
# print(t, t_in_cycle, tau)
elif float(gaits[steplens[gait_id[cycles]]][1]
[0]) <= t_in_cycle <= cycle_time:
tau = normalize(t_in_cycle, cycle_time,
float(gaits[steplens[gait_id[cycles]]][1][0]))
phase = 1
# print('Tau is {}s'.format(tau))
# print(t, t_in_cycle, tau)
if math.isclose(tau, 1, rel_tol=5e-2):
print('Gait Cycle Time was ' + str(cycle_time))
cycles += 1
if cycles == steps_per_gait * len(steplens):
break
else:
ValueError('Tau must lie between 0 and ' + str(cycle_time))
action = []
action_vel = []
for output in range(num_outputs):
coeffs = gaits[steplens[gait_id[cycles]]][0][phase, output, :]
output_val_at_tau, output_vel_at_tau = bezier_interpolation(
coeffs, tau)
action.append(float(output_val_at_tau))
action_vel.append(float(output_vel_at_tau))
if feedforward:
action = list(np.multiply(MOTOR_DIRECTION, action))
action_vel = list(np.multiply(MOTOR_DIRECTION, action_vel))
else:
action = leg_pose_to_motor_angles(
list(np.multiply(LEGSPACE_DIRECTION, action)))
action_vel = leg_pose_to_motor_angles(
list(np.multiply(LEGSPACE_DIRECTION, action_vel)))
current_row.extend(action)
current_row.extend(action_vel)
# print(action)
observation, reward, done, _ = environment.step(action)
observation[16:24] = [
observation[16 + i] * environment.minitaur._motor_direction[i]
for i in range(NUM_MOTORS)
]
current_row.extend(observation.tolist())
base_angles = environment.minitaur.GetBaseRollPitchYaw()
current_row.extend(base_angles.tolist())
actions_and_observations.append(current_row)
# time.sleep(1./50.)
sum_reward += reward
if done or (step_counter
== (steps - 1)) or (cycles == steps_per_gait * len(steplens)):
print('End of Sim')
if log_file is not None:
filename = os.path.join(currentdir, log_file)
write_csv(filename, actions_and_observations)
