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)