def __init__(self,
normalizer,
policy,
env,
smach=None,
TGP=None,
spot=None,
gui=False):
self.normalizer = normalizer
self.policy = policy
self.state_dim = self.policy.state_dim
self.action_dim = self.policy.action_dim
self.env = env
self.max_action = float(self.env.action_space.high[0])
self.successes = 0
self.phase = 0
self.desired_velocity = 0.5
self.desired_rate = 0.0
self.flip = 0
self.increment = 0
self.scaledown = True
self.type = "Stop"
self.smach = smach
if smach is not None:
self.BaseClearanceHeight = self.smach.ClearanceHeight
self.BasePenetrationDepth = self.smach.PenetrationDepth
self.TGP = TGP
self.spot = spot
if gui:
self.g_u_i = GUI(self.env.spot.quadruped)
else:
self.g_u_i = None
self.action_history = []
self.true_action_history = []
def main():
""" The main() function. """
print("STARTING SPOT TEST ENV")
seed = 0
max_timesteps = 4e6
file_name = "spot_ars_"
# Find abs path to this file
my_path = os.path.abspath(os.path.dirname(__file__))
results_path = os.path.join(my_path, "../results")
models_path = os.path.join(my_path, "../models")
if not os.path.exists(results_path):
os.makedirs(results_path)
if not os.path.exists(models_path):
os.makedirs(models_path)
env = spotGymEnv(render=True, on_rack=False)
# Set seeds
env.seed(seed)
torch.manual_seed(seed)
np.random.seed(seed)
state_dim = env.observation_space.shape[0]
print("STATE DIM: {}".format(state_dim))
action_dim = env.action_space.shape[0]
print("ACTION DIM: {}".format(action_dim))
max_action = float(env.action_space.high[0])
env.reset()
g_u_i = GUI()
spot = SpotModel()
T_bf = spot.WorldToFoot
print("STARTED SPOT TEST ENV")
t = 0
while t < (int(max_timesteps)):
# GUI: x, y, z | r, p , y
pos, orn, _, _, _, _ = g_u_i.UserInput()
# Get Roll, Pitch, Yaw
joint_angles = spot.IK(orn, pos, T_bf)
action = joint_angles.reshape(-1)
action = env.action_space.sample()
next_state, reward, done, _ = env.step(action)
# time.sleep(1.0)
t += 1
env.close()
print(joint_angles)
def main():
""" The main() function. """
print("STARTING SPOT TEST ENV")
seed = 0
max_timesteps = 4e6
# Find abs path to this file
my_path = os.path.abspath(os.path.dirname(__file__))
results_path = os.path.join(my_path, "../results")
models_path = os.path.join(my_path, "../models")
if not os.path.exists(results_path):
os.makedirs(results_path)
if not os.path.exists(models_path):
os.makedirs(models_path)
if ARGS.DebugRack:
on_rack = True
else:
on_rack = False
if ARGS.DebugPath:
draw_foot_path = True
else:
draw_foot_path = False
if ARGS.HeightField:
height_field = True
else:
height_field = False
env = spotBezierEnv(render=True,
on_rack=on_rack,
height_field=height_field,
draw_foot_path=draw_foot_path)
# Set seeds
env.seed(seed)
torch.manual_seed(seed)
np.random.seed(seed)
state_dim = env.observation_space.shape[0]
print("STATE DIM: {}".format(state_dim))
action_dim = env.action_space.shape[0]
print("ACTION DIM: {}".format(action_dim))
max_action = float(env.action_space.high[0])
state = env.reset()
g_u_i = GUI(env.spot.quadruped)
spot = SpotModel()
T_bf0 = spot.WorldToFoot
T_bf = copy.deepcopy(T_bf0)
bzg = BezierGait(dt=env._time_step)
bz_step = BezierStepper(dt=env._time_step, mode=0)
action = env.action_space.sample()
FL_phases = []
FR_phases = []
BL_phases = []
BR_phases = []
yaw = 0.0
print("STARTED SPOT TEST ENV")
t = 0
while t < (int(max_timesteps)):
bz_step.ramp_up()
pos, orn, StepLength, LateralFraction, YawRate, StepVelocity, ClearanceHeight, PenetrationDepth = bz_step.StateMachine(
)
pos, orn, StepLength, LateralFraction, YawRate, StepVelocity, ClearanceHeight, PenetrationDepth = g_u_i.UserInput(
)
yaw = env.return_yaw()
P_yaw = 5.0
if ARGS.AutoYaw:
YawRate += -yaw * P_yaw
# print("YAW RATE: {}".format(YawRate))
# TEMP
bz_step.StepLength = StepLength
bz_step.LateralFraction = LateralFraction
bz_step.YawRate = YawRate
bz_step.StepVelocity = StepVelocity
contacts = state[-4:]
FL_phases.append(env.spot.LegPhases[0])
FR_phases.append(env.spot.LegPhases[1])
BL_phases.append(env.spot.LegPhases[2])
BR_phases.append(env.spot.LegPhases[3])
# Get Desired Foot Poses
T_bf = bzg.GenerateTrajectory(StepLength, LateralFraction, YawRate,
StepVelocity, T_bf0, T_bf,
ClearanceHeight, PenetrationDepth,
contacts)
joint_angles = spot.IK(orn, pos, T_bf)
env.pass_joint_angles(joint_angles.reshape(-1))
# Get External Observations
env.spot.GetExternalObservations(bzg, bz_step)
# Step
state, reward, done, _ = env.step(action)
if done:
print("DONE")
if ARGS.AutoReset:
env.reset()
# plt.plot()
# plt.plot(FL_phases, label="FL")
# plt.plot(FR_phases, label="FR")
# plt.plot(BL_phases, label="BL")
# plt.plot(BR_phases, label="BR")
# plt.xlabel("dt")
# plt.ylabel("value")
# plt.title("Leg Phases")
# plt.legend()
# plt.show()
# time.sleep(1.0)
t += 1
env.close()
print(joint_angles)
def main():
""" The main() function. """
# Hold mp pipes
mp.freeze_support()
print("STARTING SPOT TRAINING ENV")
seed = 0
max_timesteps = 4e6
eval_freq = 1e1
save_model = True
file_name = "spot_ars_"
if ARGS.HeightField:
height_field = True
else:
height_field = False
if ARGS.NoContactSensing:
contacts = False
else:
contacts = True
# Find abs path to this file
my_path = os.path.abspath(os.path.dirname(__file__))
results_path = os.path.join(my_path, "../results")
if contacts:
models_path = os.path.join(my_path, "../models/contact")
else:
models_path = os.path.join(my_path, "../models/no_contact")
if not os.path.exists(results_path):
os.makedirs(results_path)
if not os.path.exists(models_path):
os.makedirs(models_path)
env = spotBezierEnv(render=False,
on_rack=False,
height_field=height_field,
draw_foot_path=False,
contacts=contacts)
# Set seeds
env.seed(seed)
np.random.seed(seed)
state_dim = env.observation_space.shape[0]
print("STATE DIM: {}".format(state_dim))
action_dim = env.action_space.shape[0]
print("ACTION DIM: {}".format(action_dim))
max_action = float(env.action_space.high[0])
env.reset()
g_u_i = GUI(env.spot.quadruped)
spot = SpotModel()
T_bf = spot.WorldToFoot
bz_step = BezierStepper(dt=env._time_step)
bzg = BezierGait(dt=env._time_step)
# Initialize Normalizer
normalizer = Normalizer(state_dim)
# Initialize Policy
policy = Policy(state_dim, action_dim)
# Initialize Agent with normalizer, policy and gym env
agent = ARSAgent(normalizer, policy, env, bz_step, bzg, spot)
agent_num = 0
if os.path.exists(models_path + "/" + file_name + str(agent_num) +
"_policy"):
print("Loading Existing agent")
agent.load(models_path + "/" + file_name + str(agent_num))
env.reset(agent.desired_velocity, agent.desired_rate)
episode_reward = 0
episode_timesteps = 0
episode_num = 0
# Create mp pipes
num_processes = policy.num_deltas
processes = []
childPipes = []
parentPipes = []
# Store mp pipes
for pr in range(num_processes):
parentPipe, childPipe = Pipe()
parentPipes.append(parentPipe)
childPipes.append(childPipe)
# Start multiprocessing
# Start multiprocessing
for proc_num in range(num_processes):
p = mp.Process(target=ParallelWorker,
args=(childPipes[proc_num], env, state_dim))
p.start()
processes.append(p)
print("STARTED SPOT TRAINING ENV")
t = 0
while t < (int(max_timesteps)):
# Maximum timesteps per rollout
episode_reward, episode_timesteps = agent.train_parallel(parentPipes)
t += episode_timesteps
# episode_reward = agent.train()
# +1 to account for 0 indexing.
# +0 on ep_timesteps since it will increment +1 even if done=True
print(
"Total T: {} Episode Num: {} Episode T: {} Reward: {:.2f} REWARD PER STEP: {:.2f}"
.format(t + 1, episode_num, episode_timesteps, episode_reward,
episode_reward / float(episode_timesteps)))
# Evaluate episode
if (episode_num + 1) % eval_freq == 0:
if save_model:
agent.save(models_path + "/" + str(file_name) +
str(episode_num))
# replay_buffer.save(t)
episode_num += 1
# Close pipes and hence envs
for parentPipe in parentPipes:
parentPipe.send([_CLOSE, "pay2"])
for p in processes:
p.join()