def train(env, policy, hp, parentPipes, args):
args.logdir = "experiments/" + args.logdir
logger = DataLog()
total_steps = 0
best_return = -99999999
working_dir = os.getcwd()
if os.path.isdir(args.logdir) == False:
os.mkdir(args.logdir)
previous_dir = os.getcwd()
os.chdir(args.logdir)
if os.path.isdir('iterations') == False: os.mkdir('iterations')
if os.path.isdir('logs') == False: os.mkdir('logs')
hp.to_text('hyperparameters')
log_dir = os.getcwd()
os.chdir(working_dir)
for step in range(hp.nb_steps):
if hp.domain_Rand:
env.Set_Randomization(default=False)
else:
env.randomize_only_inclines()
#Cirriculum learning
if (step > hp.curilearn):
avail_deg = [7, 9, 11, 11]
env.incline_deg = avail_deg[random.randint(0, 3)]
else:
avail_deg = [5, 7, 9]
env.incline_deg = avail_deg[random.randint(0, 2)]
# Initializing the perturbations deltas and the positive/negative rewards
deltas = policy.sample_deltas()
positive_rewards = [0] * hp.nb_directions
negative_rewards = [0] * hp.nb_directions
if (parentPipes):
process_count = len(parentPipes)
if parentPipes:
p = 0
while (p < hp.nb_directions):
temp_p = p
n_left = hp.nb_directions - p #Number of processes required to complete the search
for k in range(min([process_count, n_left])):
parentPipe = parentPipes[k]
parentPipe.send(
[_EXPLORE, [policy, hp, "positive", deltas[temp_p]]])
temp_p = temp_p + 1
temp_p = p
for k in range(min([process_count, n_left])):
positive_rewards[temp_p], step_count = parentPipes[k].recv(
)
total_steps = total_steps + step_count
temp_p = temp_p + 1
temp_p = p
for k in range(min([process_count, n_left])):
parentPipe = parentPipes[k]
parentPipe.send(
[_EXPLORE, [policy, hp, "negative", deltas[temp_p]]])
temp_p = temp_p + 1
temp_p = p
for k in range(min([process_count, n_left])):
negative_rewards[temp_p], step_count = parentPipes[k].recv(
)
total_steps = total_steps + step_count
temp_p = temp_p + 1
p = p + process_count
print('total steps till now: ', total_steps,
'Processes done: ', p)
else:
# Getting the positive rewards in the positive directions
for k in range(hp.nb_directions):
positive_rewards[k] = explore(env, policy, "positive",
deltas[k], hp)
# Getting the negative rewards in the negative/opposite directions
for k in range(hp.nb_directions):
negative_rewards[k] = explore(env, policy, "negative",
deltas[k], hp)
# Sorting the rollouts by the max(r_pos, r_neg) and selecting the best directions
scores = {
k: max(r_pos, r_neg)
for k, (
r_pos,
r_neg) in enumerate(zip(positive_rewards, negative_rewards))
}
order = sorted(scores.keys(),
key=lambda x: -scores[x])[:int(hp.nb_best_directions)]
rollouts = [(positive_rewards[k], negative_rewards[k], deltas[k])
for k in order]
# Gathering all the positive/negative rewards to compute the standard deviation of these rewards
all_rewards = np.array([x[0]
for x in rollouts] + [x[1] for x in rollouts])
sigma_r = all_rewards.std(
) # Standard deviation of only rewards in the best directions is what it should be
# Updating our policy
policy.update(rollouts, sigma_r, args)
#Start evaluating after only second stage
if step >= hp.curilearn:
# policy evaluation after specified iterations
if step % hp.evalstep == 0:
reward_evaluation = policyevaluation(env, policy, hp)
logger.log_kv('steps', step)
logger.log_kv('return', reward_evaluation)
if (reward_evaluation > best_return):
best_policy = policy.theta
best_return = reward_evaluation
np.save(log_dir + "/iterations/best_policy.npy",
best_policy)
print('Step:', step, 'Reward:', reward_evaluation)
policy_path = log_dir + "/iterations/" + "policy_" + str(step)
np.save(policy_path, policy.theta)
logger.save_log(log_dir + "/logs/")
make_train_plots_ars(log=logger.log,
keys=['steps', 'return'],
save_loc=log_dir + "/logs/")
class StochliteEnv(mujoco_env.MujocoEnv, utils.EzPickle):
def __init__(
self,
render=False,
on_rack=False,
gait='trot',
phase=[0, no_of_points, no_of_points, 0], #[FR, FL, BR, BL]
action_dim=20,
end_steps=1000,
stairs=False,
downhill=False,
seed_value=100,
wedge=False,
IMU_Noise=False,
deg=5): # deg = 5
self._is_stairs = stairs
self._is_wedge = wedge
self._is_render = render
self._on_rack = on_rack
self.rh_along_normal = 0.24
self.seed_value = seed_value
random.seed(self.seed_value)
if self._is_render:
self._pybullet_client = bullet_client.BulletClient(
connection_mode=pybullet.GUI)
else:
self._pybullet_client = bullet_client.BulletClient()
self._theta = 0
self._frequency = 2.5 # originally 2.5, changing for stability
self.termination_steps = end_steps
self.downhill = downhill
#PD gains
self._kp = 400
self._kd = 10
self.dt = 0.01
self._frame_skip = 50
self._n_steps = 0
self._action_dim = action_dim
self._obs_dim = 11
self.action = np.zeros(self._action_dim)
self._last_base_position = [0, 0, 0]
self.last_yaw = 0
self._distance_limit = float("inf")
self.current_com_height = 0.25 # 0.243
#wedge_parameters
self.wedge_start = 0.5
self.wedge_halflength = 2
if gait is 'trot':
phase = [0, no_of_points, no_of_points, 0]
elif gait is 'walk':
phase = [0, no_of_points, 3 * no_of_points / 2, no_of_points / 2]
self._walkcon = walking_controller.WalkingController(gait_type=gait,
phase=phase)
self.inverse = False
self._cam_dist = 1.0
self._cam_yaw = 0.0
self._cam_pitch = 0.0
self.avg_vel_per_step = 0
self.avg_omega_per_step = 0
self.linearV = 0
self.angV = 0
self.prev_vel = [0, 0, 0]
self.x_f = 0
self.y_f = 0
self.clips = 7
self.friction = 0.6
self.ori_history_length = 3
self.ori_history_queue = deque(
[0] * 3 * self.ori_history_length,
maxlen=3 * self.ori_history_length) #observation queue
self.step_disp = deque([0] * 100, maxlen=100)
self.stride = 5
self.incline_deg = deg
self.incline_ori = 0
self.prev_incline_vec = (0, 0, 1)
self.terrain_pitch = []
self.add_IMU_noise = IMU_Noise
self.support_plane_estimated_pitch = 0
self.support_plane_estimated_roll = 0
self.pertub_steps = 0
self.x_f = 0
self.y_f = 0
self.state_pos = StatePositionIndex()
self.reset_pos = InitPosition()
self.state_vel = StateVelocityIndex()
self.actuaor = ActuatorIndex()
self.limits = JoinLimit()
mujoco_env.MujocoEnv.__ini__(self, stoch23.xml, 1)
utils.EzPickle.__init__(self)
## Gym env related mandatory variables
self._obs_dim = 3 * self.ori_history_length + 2 #[r,p,y]x previous time steps, suport plane roll and pitch
observation_high = np.array([np.pi / 2] * self._obs_dim)
observation_low = -observation_high
self.observation_space = spaces.Box(observation_low, observation_high)
action_high = np.array([1] * self._action_dim)
self.action_space = spaces.Box(-action_high, action_high)
self.commands = np.array(
[0, 0, 0]
) #Joystick commands consisting of cmd_x_velocity, cmd_y_velocity, cmd_ang_velocity
self.max_linear_xvel = 0.35 # calculation is < 0.2 m steplength times the frequency 2.5 Hz
self.max_linear_yvel = 0 #0.25, made zero for only x direction # calculation is < 0.14 m times the frequency 2.5 Hz
self.max_ang_vel = 0 #6, made zero for only x direction # less than one complete rotation in one second
self.max_steplength = 0.2 # by the kinematic limits of the robot
self.max_x_shift = 0.1 #plus minus 0.1 m
self.max_y_shift = 0.14 # max 30 degree abduction
self.max_z_shift = 0.1 # plus minus 0.1 m
self.max_incline = 15 # in deg
self.hard_reset()
self.Set_Randomization(default=True, idx1=2, idx2=2)
self.logger = DataLog()
if (self._is_stairs):
boxHalfLength = 0.1
boxHalfWidth = 1
boxHalfHeight = 0.015
sh_colBox = self._pybullet_client.createCollisionShape(
self._pybullet_client.GEOM_BOX,
halfExtents=[boxHalfLength, boxHalfWidth, boxHalfHeight])
boxOrigin = 0.3
n_steps = 15
self.stairs = []
for i in range(n_steps):
step = self._pybullet_client.createMultiBody(
baseMass=0,
baseCollisionShapeIndex=sh_colBox,
basePosition=[
boxOrigin + i * 2 * boxHalfLength, 0,
boxHalfHeight + i * 2 * boxHalfHeight
],
baseOrientation=[0.0, 0.0, 0.0, 1])
self.stairs.append(step)
self._pybullet_client.changeDynamics(step,
-1,
lateralFriction=0.8)
def hard_reset(self):
'''
Function to
1) Set simulation parameters which remains constant throughout the experiments
2) load urdf of plane, wedge and robot in initial conditions
'''
self._pybullet_client.resetSimulation()
self._pybullet_client.setPhysicsEngineParameter(
numSolverIterations=int(300))
self._pybullet_client.setTimeStep(self.dt / self._frame_skip)
self.plane = self._pybullet_client.loadURDF(
"%s/plane.urdf" % pybullet_data.getDataPath())
self._pybullet_client.changeVisualShape(self.plane,
-1,
rgbaColor=[1, 1, 1, 0.9])
self._pybullet_client.setGravity(0, 0, -9.8)
if self._is_wedge:
wedge_halfheight_offset = 0.01
self.wedge_halfheight = wedge_halfheight_offset + 1.5 * math.tan(
math.radians(self.incline_deg)) / 2.0
self.wedgePos = [0, 0, self.wedge_halfheight]
self.wedgeOrientation = self._pybullet_client.getQuaternionFromEuler(
[0, 0, self.incline_ori])
if not (self.downhill):
wedge_model_path = "gym_sloped_terrain/envs/Wedges/uphill/urdf/wedge_" + str(
self.incline_deg) + ".urdf"
self.INIT_ORIENTATION = self._pybullet_client.getQuaternionFromEuler(
[
math.radians(self.incline_deg) *
math.sin(self.incline_ori),
-math.radians(self.incline_deg) *
math.cos(self.incline_ori), 0
])
self.robot_landing_height = wedge_halfheight_offset + 0.28 + math.tan(
math.radians(self.incline_deg)) * abs(self.wedge_start)
# self.INIT_POSITION = [self.INIT_POSITION[0], self.INIT_POSITION[1], self.robot_landing_height]
self.INIT_POSITION = [-0.1, 0, self.robot_landing_height]
else:
wedge_model_path = "gym_sloped_terrain/envs/Wedges/downhill/urdf/wedge_" + str(
self.incline_deg) + ".urdf"
self.robot_landing_height = wedge_halfheight_offset + 0.28 + math.tan(
math.radians(self.incline_deg)) * 1.5
self.INIT_POSITION = [0, 0, self.robot_landing_height
] # [0.5, 0.7, 0.3] #[-0.5,-0.5,0.3]
self.INIT_ORIENTATION = [0, 0, 0, 1]
self.wedge = self._pybullet_client.loadURDF(
wedge_model_path, self.wedgePos, self.wedgeOrientation)
self.SetWedgeFriction(0.7)
model_path = 'gym_sloped_terrain/envs/robots/stochlite/stochlite_description/urdf/stochlite_urdf.urdf'
self.stochlite = self._pybullet_client.loadURDF(
model_path, self.INIT_POSITION, self.INIT_ORIENTATION)
self._joint_name_to_id, self._motor_id_list = self.BuildMotorIdList()
num_legs = 4
for i in range(num_legs):
self.ResetLeg(i, add_constraint=True)
self.ResetPoseForAbd()
if self._on_rack:
self._pybullet_client.createConstraint(
self.stochlite, -1, -1, -1, self._pybullet_client.JOINT_FIXED,
[0, 0, 0], [0, 0, 0], [0, 0, 0.4])
self._pybullet_client.resetBasePositionAndOrientation(
self.stochlite, self.INIT_POSITION, self.INIT_ORIENTATION)
self._pybullet_client.resetBaseVelocity(self.stochlite, [0, 0, 0],
[0, 0, 0])
self._pybullet_client.resetDebugVisualizerCamera(
self._cam_dist, self._cam_yaw, self._cam_pitch, [0, 0, 0])
self.SetFootFriction(self.friction)
# self.SetLinkMass(0,0)
# self.SetLinkMass(11,0)
def reset_standing_position(self):
num_legs = 4
for i in range(num_legs):
self.ResetLeg(i, add_constraint=False, standstilltorque=10)
self.ResetPoseForAbd()
# Conditions for standstill
for i in range(300):
self._pybullet_client.stepSimulation()
for i in range(num_legs):
self.ResetLeg(i, add_constraint=False, standstilltorque=0)
def reset(self):
'''
This function resets the environment
Note : Set_Randomization() is called before reset() to either randomize or set environment in default conditions.
'''
self._theta = 0
self._last_base_position = [0, 0, 0]
self.commands = [0, 0, 0]
self.last_yaw = 0
self.inverse = False
if self._is_wedge:
self._pybullet_client.removeBody(self.wedge)
wedge_halfheight_offset = 0.01
self.wedge_halfheight = wedge_halfheight_offset + 1.5 * math.tan(
math.radians(self.incline_deg)) / 2.0
self.wedgePos = [0, 0, self.wedge_halfheight]
self.wedgeOrientation = self._pybullet_client.getQuaternionFromEuler(
[0, 0, self.incline_ori])
if not (self.downhill):
wedge_model_path = "gym_sloped_terrain/envs/Wedges/uphill/urdf/wedge_" + str(
self.incline_deg) + ".urdf"
self.INIT_ORIENTATION = self._pybullet_client.getQuaternionFromEuler(
[
math.radians(self.incline_deg) *
math.sin(self.incline_ori),
-math.radians(self.incline_deg) *
math.cos(self.incline_ori), 0
])
self.robot_landing_height = wedge_halfheight_offset + 0.28 + math.tan(
math.radians(self.incline_deg)) * abs(self.wedge_start)
# self.INIT_POSITION = [self.INIT_POSITION[0], self.INIT_POSITION[1], self.robot_landing_height]
self.INIT_POSITION = [-1, 0, self.robot_landing_height]
else:
wedge_model_path = "gym_sloped_terrain/envs/Wedges/downhill/urdf/wedge_" + str(
self.incline_deg) + ".urdf"
self.robot_landing_height = wedge_halfheight_offset + 0.28 + math.tan(
math.radians(self.incline_deg)) * 1.5
self.INIT_POSITION = [0, 0, self.robot_landing_height
] # [0.5, 0.7, 0.3] #[-0.5,-0.5,0.3]
self.INIT_ORIENTATION = [0, 0, 0, 1]
self.wedge = self._pybullet_client.loadURDF(
wedge_model_path, self.wedgePos, self.wedgeOrientation)
self.SetWedgeFriction(0.7)
self._pybullet_client.resetBasePositionAndOrientation(
self.stochlite, self.INIT_POSITION, self.INIT_ORIENTATION)
self._pybullet_client.resetBaseVelocity(self.stochlite, [0, 0, 0],
[0, 0, 0])
self.reset_standing_position()
self._pybullet_client.resetDebugVisualizerCamera(
self._cam_dist, self._cam_yaw, self._cam_pitch, [0, 0, 0])
self._n_steps = 0
return self.GetObservation()
def updateCommands(self, num_plays, episode_length):
ratio = num_plays / episode_length
if num_plays < 0.2 * episode_length:
self.commands = [0, 0, 0]
# elif num_plays < 0.6 * episode_length:
# self.commands = [0.5 * self.max_linear_xvel, 0.5 * self.max_linear_yvel, 0.5 * self.max_ang_vel]
else:
# self.commands = [self.max_linear_xvel, self.max_linear_yvel, self.max_ang_vel]
self.commands = np.array([
self.max_linear_xvel, self.max_linear_yvel, self.max_ang_vel
]) * ratio
def apply_Ext_Force(self,
x_f,
y_f,
link_index=1,
visulaize=False,
life_time=0.01):
'''
function to apply external force on the robot
Args:
x_f : external force in x direction
y_f : external force in y direction
link_index : link index of the robot where the force need to be applied
visulaize : bool, whether to visulaize external force by arrow symbols
life_time : life time of the visualization
'''
force_applied = [x_f, y_f, 0]
self._pybullet_client.applyExternalForce(
self.stochlite,
link_index,
forceObj=[x_f, y_f, 0],
posObj=[0, 0, 0],
flags=self._pybullet_client.LINK_FRAME)
f_mag = np.linalg.norm(np.array(force_applied))
if (visulaize and f_mag != 0.0):
point_of_force = self._pybullet_client.getLinkState(
self.stochlite, link_index)[0]
lam = 1 / (2 * f_mag)
dummy_pt = [
point_of_force[0] - lam * force_applied[0],
point_of_force[1] - lam * force_applied[1],
point_of_force[2] - lam * force_applied[2]
]
self._pybullet_client.addUserDebugText(str(round(f_mag, 2)) + " N",
dummy_pt,
[0.13, 0.54, 0.13],
textSize=2,
lifeTime=life_time)
self._pybullet_client.addUserDebugLine(point_of_force,
dummy_pt, [0, 0, 1],
3,
lifeTime=life_time)
def SetLinkMass(self, link_idx, mass=0):
'''
Function to add extra mass to front and back link of the robot
Args:
link_idx : link index of the robot whose weight to need be modified
mass : value of extra mass to be added
Ret:
new_mass : mass of the link after addition
Note : Presently, this function supports addition of masses in the front and back link only (0, 11)
'''
link_mass = self._pybullet_client.getDynamicsInfo(
self.stochlite, link_idx)[0]
if (link_idx == 0):
link_mass = mass # mass + 1.1
self._pybullet_client.changeDynamics(self.stochlite,
0,
mass=link_mass)
elif (link_idx == 11):
link_mass = mass # mass + 1.1
self._pybullet_client.changeDynamics(self.stochlite,
11,
mass=link_mass)
return link_mass
def getlinkmass(self, link_idx):
'''
function to retrieve mass of any link
Args:
link_idx : link index of the robot
Ret:
m[0] : mass of the link
'''
m = self._pybullet_client.getDynamicsInfo(self.stochlite, link_idx)
return m[0]
def Set_Randomization(self,
default=True,
idx1=0,
idx2=0,
idx3=2,
idx0=0,
idx11=0,
idxc=2,
idxp=0,
deg=5,
ori=0): # deg = 5, changed for stochlite
'''
This function helps in randomizing the physical and dynamics parameters of the environment to robustify the policy.
These parameters include wedge incline, wedge orientation, friction, mass of links, motor strength and external perturbation force.
Note : If default argument is True, this function set above mentioned parameters in user defined manner
'''
if default:
frc = [0.55, 0.6, 0.8]
# extra_link_mass=[0,0.05,0.1,0.15]
cli = [5.2, 6, 7, 8]
# pertub_range = [0, -30, 30, -60, 60]
self.pertub_steps = 150
self.x_f = 0
# self.y_f = pertub_range[idxp]
self.incline_deg = deg + 2 * idx1
# self.incline_ori = ori + PI/12*idx2
self.new_fric_val = frc[idx3]
self.friction = self.SetFootFriction(self.new_fric_val)
# self.FrontMass = self.SetLinkMass(0,extra_link_mass[idx0])
# self.BackMass = self.SetLinkMass(11,extra_link_mass[idx11])
self.clips = cli[idxc]
else:
avail_deg = [5, 7, 9, 11] # [5,7,9,11], changed for stochlite
# extra_link_mass=[0,.05,0.1,0.15]
# pertub_range = [0, -30, 30, -60, 60]
cli = [5, 6, 7, 8]
self.pertub_steps = 150 #random.randint(90,200) #Keeping fixed for now
self.x_f = 0
# self.y_f = pertub_range[random.randint(0,2)]
self.incline_deg = avail_deg[random.randint(0, 3)]
# self.incline_ori = (PI/12)*random.randint(0, 4) #resolution of 15 degree, changed for stochlite
# self.new_fric_val = np.round(np.clip(np.random.normal(0.6,0.08),0.55,0.8),2)
self.friction = self.SetFootFriction(0.8) #(self.new_fric_val)
# i=random.randint(0,3)
# self.FrontMass = self.SetLinkMass(0,extra_link_mass[i])
# i=random.randint(0,3)
# self.BackMass = self.SetLinkMass(11,extra_link_mass[i])
self.clips = np.round(np.clip(np.random.normal(6.5, 0.4), 5, 8), 2)
def randomize_only_inclines(self,
default=True,
idx1=0,
idx2=0,
deg=5,
ori=0): # deg = 5, changed for stochlite
'''
This function only randomizes the wedge incline and orientation and is called during training without Domain Randomization
'''
if default:
self.incline_deg = deg + 2 * idx1
# self.incline_ori = ori + PI / 12 * idx2
else:
avail_deg = [5, 7, 9, 11] # [5, 7, 9, 11]
self.incline_deg = avail_deg[random.randint(0, 3)]
# self.incline_ori = (PI / 12) * random.randint(0, 4) # resolution of 15 degree
def boundYshift(self, x, y):
'''
This function bounds Y shift with respect to current X shift
Args:
x : absolute X-shift
y : Y-Shift
Ret :
y : bounded Y-shift
'''
if x > 0.5619:
if y > 1 / (0.5619 - 1) * (x - 1):
y = 1 / (0.5619 - 1) * (x - 1)
return y
def getYXshift(self, yx):
'''
This function bounds X and Y shifts in a trapezoidal workspace
'''
y = yx[:4]
x = yx[4:]
for i in range(0, 4):
y[i] = self.boundYshift(abs(x[i]), y[i])
y[i] = y[i] * 0.038
x[i] = x[i] * 0.0418
yx = np.concatenate([y, x])
return yx
def transform_action(self, action):
'''
Transform normalized actions to scaled offsets
Args:
action : 20 dimensional 1D array of predicted action values from policy in following order :
[(step lengths of FR, FL, BR, BL), (steer angles of FR, FL, BR, BL),
(Y-shifts of FR, FL, BR, BL), (X-shifts of FR, FL, BR, BL),
(Z-shifts of FR, FL, BR, BL)]
Ret :
action : scaled action parameters
Note : The convention of Cartesian axes for leg frame in the codebase follow this order, Y points up, X forward and Z right.
While in research paper we follow this order, Z points up, X forward and Y right.
'''
'''
The previous action transform
action = np.clip(action, -1, 1)
action[:4] = (action[:4] + 1)/2 # Step lengths are positive always
action[:4] = action[:4] *2 * 0.068 # Max step length = 2x0.068
action[4:8] = action[4:8] * PI/2 # PHI can be [-pi/2, pi/2]
action[8:12] = (action[8:12]+1)/2 # Y-shifts are positive always
action[8:16] = self.getYXshift(action[8:16])
action[16:20] = action[16:20]*0.035 # Max allowed Z-shift due to abduction limits is 3.5cm
action[17] = -action[17]
action[19] = -action[19]
'''
# Note: just check if the getYXShift function is required
#This is to switch the z_shift and y_shift
#this is being done as we are taking the
# cordinate system for the walking controler
# where all the cordinate system are using right
# hand rule
# action_temp = action[8:12]
# replasing the y_shift with z_shift
# The order is fr,fl,br,bl
# Ensure that in the walking controller the order
# of the legs are taken care of
# we are negating the values as the convention for
# the walking controller is with right hand rule
# forwards: +x, upwards +z, left: +y
# action[8] = -action[16]
# action[9] = action[17]
# action[10] = -action[18]
# action[11] = action[19]
# #replacing the z_shift with the y_shift
# action[16] = action_temp[0]
# action[17] = action_temp[1]
# action[18] = action_temp[2]
# action[19] = action_temp[3]
'''
Action changed
action[:4] -> step_length fl fr bl br
action[4:8] -> steer angle
action[8:12] -> x_shift fl fr bl br
action[12:16] -> y_shift fl fr bl br
action[16:20] -> z_shift fl fr bl br
'''
step_length_offset = self.max_steplength * math.sqrt(
self.commands[0]**2 +
self.commands[1]**2) / math.sqrt(self.max_linear_xvel**2 +
self.max_linear_yvel**2)
sp_pitch_offset = self.max_x_shift * np.degrees(
self.support_plane_estimated_pitch) / self.max_incline
# sp_roll_offset = self.max_z_shift * np.degrees(self.support_plane_estimated_roll) / self.max_incline
# print("Sp pitch", np.degrees(self.support_plane_estimated_pitch))
# print("pitch offset", sp_pitch_offset)
action = np.clip(action, -1, 1)
action[:4] = (
action[:4] + 1
) / 2 * 0.4 * self.max_steplength #+ 0.15 # Step lengths are positive always
action[:
4] = action[:4] + step_length_offset # Max step length = 2x0.1 =0.2
action[8:12] = action[8:12] + sp_pitch_offset #- 0.08
action[12:16] = action[
12:
16] * self.max_y_shift + 0.04 # np.clip(action[12:16], -0.14, 0.14) #the Y_shift can be +/- 0.14 from the leg zero position, max abd angle = +/- 30 deg
action[12] = -action[12]
action[14] = -action[14]
action[16:20] = action[16:20] * self.max_z_shift
# action[16] = action[16] * self.max_z_shift + sp_roll_offset
# action[17] = action[17] * self.max_z_shift - sp_roll_offset # Z_shift can be +/- 0.1 from z center
# action[18] = action[18] * self.max_z_shift + sp_roll_offset
# action[19] = action[19] * self.max_z_shift - sp_roll_offset
# print("in env", action)
return action
def get_foot_contacts(self):
'''
Retrieve foot contact information with the supporting ground and any special structure (wedge/stairs).
Ret:
foot_contact_info : 8 dimensional binary array, first four values denote contact information of feet [FR, FL, BR, BL] with the ground
while next four with the special structure.
'''
foot_ids = [5, 2, 11, 8]
foot_contact_info = np.zeros(8)
for leg in range(4):
contact_points_with_ground = self._pybullet_client.getContactPoints(
self.plane, self.stochlite, -1, foot_ids[leg])
# print(contact_points_with_ground)
if len(contact_points_with_ground) > 0:
foot_contact_info[leg] = 1
# print("Contact", foot_contact_info[leg])
if self._is_wedge:
contact_points_with_wedge = self._pybullet_client.getContactPoints(
self.wedge, self.stochlite, -1, foot_ids[leg])
if len(contact_points_with_wedge) > 0:
foot_contact_info[leg + 4] = 1
# print("Contact", foot_contact_info[leg])
if self._is_stairs:
for steps in self.stairs:
contact_points_with_stairs = self._pybullet_client.getContactPoints(
steps, self.stochlite, -1, foot_ids[leg])
if len(contact_points_with_stairs) > 0:
foot_contact_info[leg + 4] = 1
# print(foot_contact_info)
# foot_contact_info = [1, 1, 1, 1]
return foot_contact_info
def step(self, action):
'''
function to perform one step in the environment
Args:
action : array of action values
Ret:
ob : observation after taking step
reward : reward received after taking step
done : whether the step terminates the env
{} : any information of the env (will be added later)
'''
action = self.transform_action(action)
self.do_simulation(action, n_frames=self._frame_skip)
ob = self.GetObservation()
reward, done = self._get_reward()
return ob, reward, done, {}
def CurrentVelocities(self):
'''
Returns robot's linear and angular velocities
Ret:
radial_v : linear velocity
current_w : angular velocity
'''
current_w = self.GetBaseAngularVelocity()[2]
current_v = self.GetBaseLinearVelocity()
radial_v = math.sqrt(current_v[0]**2 + current_v[1]**2)
return radial_v, current_w
def do_simulation(self, action, n_frames):
'''
Converts action parameters to corresponding motor commands with the help of a elliptical trajectory controller
'''
omega = 2 * no_of_points * self._frequency
self.action = action
ii = 0
leg_m_angle_cmd = self._walkcon.run_elliptical_Traj_Stochlite(
self._theta, action)
self._theta = constrain_theta(omega * self.dt + self._theta)
m_angle_cmd_ext = np.array(leg_m_angle_cmd)
m_vel_cmd_ext = np.zeros(12)
force_visualizing_counter = 0
for _ in range(n_frames):
ii = ii + 1
applied_motor_torque = self._apply_pd_control(
m_angle_cmd_ext, m_vel_cmd_ext)
self._pybullet_client.stepSimulation()
if self._n_steps >= self.pertub_steps and self._n_steps <= self.pertub_steps + self.stride:
force_visualizing_counter += 1
if (force_visualizing_counter % 7 == 0):
self.apply_Ext_Force(self.x_f,
self.y_f,
visulaize=True,
life_time=0.1)
else:
self.apply_Ext_Force(self.x_f, self.y_f, visulaize=False)
contact_info = self.get_foot_contacts()
pos, ori = self.GetBasePosAndOrientation()
Rot_Mat = self._pybullet_client.getMatrixFromQuaternion(ori)
Rot_Mat = np.array(Rot_Mat)
Rot_Mat = np.reshape(Rot_Mat, (3, 3))
plane_normal, self.support_plane_estimated_roll, self.support_plane_estimated_pitch = normal_estimator.vector_method_Stochlite(
self.prev_incline_vec, contact_info, self.GetMotorAngles(),
Rot_Mat)
self.prev_incline_vec = plane_normal
log_dir = os.getcwd()
self.logger.log_kv("Robot_roll", pos[0])
self.logger.log_kv("Robot_pitch", pos[1])
self.logger.log_kv("SP_roll", self.support_plane_estimated_roll)
self.logger.log_kv("SP_pitch", self.support_plane_estimated_pitch)
self.logger.save_log(log_dir + '/experiments/logs_sensors')
# print("estimate", self.support_plane_estimated_roll, self.support_plane_estimated_pitch)
# print("incline", self.incline_deg)
self._n_steps += 1
def render(self, mode="rgb_array", close=False):
if mode != "rgb_array":
return np.array([])
base_pos, _ = self.GetBasePosAndOrientation()
view_matrix = self._pybullet_client.computeViewMatrixFromYawPitchRoll(
cameraTargetPosition=base_pos,
distance=self._cam_dist,
yaw=self._cam_yaw,
pitch=self._cam_pitch,
roll=0,
upAxisIndex=2)
proj_matrix = self._pybullet_client.computeProjectionMatrixFOV(
fov=60,
aspect=float(RENDER_WIDTH) / RENDER_HEIGHT,
nearVal=0.1,
farVal=100.0)
(_, _, px, _, _) = self._pybullet_client.getCameraImage(
width=RENDER_WIDTH,
height=RENDER_HEIGHT,
viewMatrix=view_matrix,
projectionMatrix=proj_matrix,
renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
rgb_array = np.array(px).reshape(RENDER_WIDTH, RENDER_HEIGHT, 4)
rgb_array = rgb_array[:, :, :3]
return rgb_array
def _termination(self, pos, orientation):
'''
Check termination conditions of the environment
Args:
pos : current position of the robot's base in world frame
orientation : current orientation of robot's base (Quaternions) in world frame
Ret:
done : return True if termination conditions satisfied
'''
done = False
RPY = self._pybullet_client.getEulerFromQuaternion(orientation)
if self._n_steps >= self.termination_steps:
done = True
else:
if abs(RPY[0]) > math.radians(30):
print('Oops, Robot about to fall sideways! Terminated')
done = True
if abs(RPY[1]) > math.radians(35):
print('Oops, Robot doing wheely! Terminated')
done = True
if pos[2] > 0.7:
print('Robot was too high! Terminated')
done = True
return done
def _get_reward(self):
'''
Calculates reward achieved by the robot for RPY stability, torso height criterion and forward distance moved on the slope:
Ret:
reward : reward achieved
done : return True if environment terminates
'''
wedge_angle = self.incline_deg * PI / 180
robot_height_from_support_plane = 0.25 # walking height of stochlite
pos, ori = self.GetBasePosAndOrientation()
RPY_orig = self._pybullet_client.getEulerFromQuaternion(ori)
RPY = np.round(RPY_orig, 4)
current_height = round(pos[2], 5)
self.current_com_height = current_height
standing_penalty = 3
desired_height = (robot_height_from_support_plane
) / math.cos(wedge_angle) + math.tan(wedge_angle) * (
(pos[0]) * math.cos(self.incline_ori) + 0.5)
#Need to re-evaluate reward functions for slopes, value of reward function after considering error should be greater than 0.5, need to tune
roll_reward = np.exp(-45 *
((RPY[0] - self.support_plane_estimated_roll)**2))
pitch_reward = np.exp(
-45 * ((RPY[1] - self.support_plane_estimated_pitch)**2))
yaw_reward = np.exp(
-40 * (RPY[2]**2)
) # np.exp(-35 * (RPY[2] ** 2)) increasing reward for yaw correction
height_reward = np.exp(-800 * (desired_height - current_height)**2)
x = pos[0]
y = pos[1]
yaw = RPY[2]
x_l = self._last_base_position[0]
y_l = self._last_base_position[1]
self._last_base_position = pos
self.last_yaw = RPY[2]
# step_distance_x = (x - x_l)
# step_distance_y = abs(y - y_l)
x_velocity = self.GetBaseLinearVelocity()[0] #(x - x_l)/self.dt
y_velocity = self.GetBaseLinearVelocity()[1] #(y - y_l)/self.dt
ang_velocity = self.GetBaseAngularVelocity()[
2] #(yaw - self.last_yaw)/self.dt
cmd_translation_vel_reward = np.exp(
-50 * ((x_velocity - self.commands[0])**2 +
(y_velocity - self.commands[1])**2))
cmd_rotation_vel_reward = np.exp(-50 *
(ang_velocity - self.commands[2])**2)
done = self._termination(pos, ori)
if done:
reward = 0
else:
reward = round(yaw_reward, 4) + round(pitch_reward, 4) + round(roll_reward, 4)\
+ round(height_reward,4) + round(cmd_translation_vel_reward, 4) + round(cmd_rotation_vel_reward, 4)#- 100 * round(step_distance_x, 4) - 100 * round(step_distance_y, 4)\
# reward_info = [0, 0, 0]
# reward_info[0] = self.commands[0]
# reward_info[1] = x_velocity
# reward_info[2] = reward_info[2] + reward
'''
#Penalize for standing at same position for continuous 150 steps
self.step_disp.append(step_distance_x)
if(self._n_steps>150):
if(sum(self.step_disp)<0.035):
reward = reward-standing_penalty
'''
# Testing reward function
# reward_info = [0, 0, 0, 0, 0, 0, 0, 0, 0]
# done = self._termination(pos, ori)
# if done:
# reward = 0
# else:
# reward_info[0] = round(roll_reward, 4)
# reward_info[1] = round(pitch_reward, 4)
# reward_info[2] = round(yaw_reward, 4)
# reward_info[3] = round(height_reward, 4)
# reward_info[4] = 100 * round(step_distance_x, 4)
# reward_info[5] = -50 * round(step_distance_y, 4)
# reward_info[6] = self.support_plane_estimated_roll
# reward_info[7] = self.support_plane_estimated_pitch
# reward = round(yaw_reward, 4) + round(pitch_reward, 4) + round(roll_reward, 4)\
# + round(height_reward,4) + 100 * round(step_distance_x, 4) - 50 * round(step_distance_y, 4)
# reward_info[8] = reward
return reward, done
def _apply_pd_control(self, motor_commands, motor_vel_commands):
'''
Apply PD control to reach desired motor position commands
Ret:
applied_motor_torque : array of applied motor torque values in order [FLH FLK FRH FRK BLH BLK BRH BRK FLA FRA BLA BRA]
'''
qpos_act = self.GetMotorAngles()
qvel_act = self.GetMotorVelocities()
applied_motor_torque = self._kp * (
motor_commands - qpos_act) + self._kd * (motor_vel_commands -
qvel_act)
applied_motor_torque = np.clip(np.array(applied_motor_torque),
-self.clips, self.clips)
applied_motor_torque = applied_motor_torque.tolist()
for motor_id, motor_torque in zip(self._motor_id_list,
applied_motor_torque):
self.SetMotorTorqueById(motor_id, motor_torque)
return applied_motor_torque
def add_noise(self, sensor_value, SD=0.04):
'''
Adds sensor noise of user defined standard deviation in current sensor_value
'''
noise = np.random.normal(0, SD, 1)
sensor_value = sensor_value + noise[0]
return sensor_value
def GetObservation(self):
'''
This function returns the current observation of the environment for the interested task
Ret:
obs : [R(t-2), P(t-2), Y(t-2), R(t-1), P(t-1), Y(t-1), R(t), P(t), Y(t), estimated support plane (roll, pitch) ]
'''
pos, ori = self.GetBasePosAndOrientation()
RPY = self._pybullet_client.getEulerFromQuaternion(ori)
RPY = np.round(RPY, 5)
for val in RPY:
if (self.add_IMU_noise):
val = self.add_noise(val)
self.ori_history_queue.append(val)
obs = np.concatenate((self.ori_history_queue, [
self.support_plane_estimated_roll,
self.support_plane_estimated_pitch
])).ravel()
return obs
def GetMotorAngles(self):
'''
This function returns the current joint angles in order [FLH FLK FRH FRK BLH BLK BRH BRK FLA FRA BLA BRA ]
'''
motor_ang = [
self._pybullet_client.getJointState(self.stochlite, motor_id)[0]
for motor_id in self._motor_id_list
]
# print("motor angles", motor_ang)
return motor_ang
def GetMotorVelocities(self):
'''
This function returns the current joint velocities in order [FLH FLK FRH FRK BLH BLK BRH BRK FLA FRA BLA BRA ]
'''
motor_vel = [
self._pybullet_client.getJointState(self.stochlite, motor_id)[1]
for motor_id in self._motor_id_list
]
return motor_vel
def GetBasePosAndOrientation(self):
'''
This function returns the robot torso position(X,Y,Z) and orientation(Quaternions) in world frame
'''
position, orientation = (
self._pybullet_client.getBasePositionAndOrientation(
self.stochlite))
return position, orientation
def GetBaseAngularVelocity(self):
'''
This function returns the robot base angular velocity in world frame
Ret: list of 3 floats
'''
basevelocity = self._pybullet_client.getBaseVelocity(self.stochlite)
return basevelocity[1]
def GetBaseLinearVelocity(self):
'''
This function returns the robot base linear velocity in world frame
Ret: list of 3 floats
'''
basevelocity = self._pybullet_client.getBaseVelocity(self.stochlite)
return basevelocity[0]
def SetFootFriction(self, foot_friction):
'''
This function modify coefficient of friction of the robot feet
Args :
foot_friction : desired friction coefficient of feet
Ret :
foot_friction : current coefficient of friction
'''
FOOT_LINK_ID = [2, 5, 8, 11]
for link_id in FOOT_LINK_ID:
self._pybullet_client.changeDynamics(self.stochlite,
link_id,
lateralFriction=foot_friction)
return foot_friction
def SetWedgeFriction(self, friction):
'''
This function modify friction coefficient of the wedge
Args :
foot_friction : desired friction coefficient of the wedge
'''
self._pybullet_client.changeDynamics(self.wedge,
-1,
lateralFriction=friction)
def SetMotorTorqueById(self, motor_id, torque):
'''
function to set motor torque for respective motor_id
'''
self._pybullet_client.setJointMotorControl2(
bodyIndex=self.stochlite,
jointIndex=motor_id,
controlMode=self._pybullet_client.TORQUE_CONTROL,
force=torque)
def BuildMotorIdList(self):
'''
function to map joint_names with respective motor_ids as well as create a list of motor_ids
Ret:
joint_name_to_id : Dictionary of joint_name to motor_id
motor_id_list : List of joint_ids for respective motors in order [FLH FLK FRH FRK BLH BLK BRH BRK FLA FRA BLA BRA ]
'''
num_joints = self._pybullet_client.getNumJoints(self.stochlite)
joint_name_to_id = {}
for i in range(num_joints):
joint_info = self._pybullet_client.getJointInfo(self.stochlite, i)
# print(joint_info[0], joint_info[1])
joint_name_to_id[joint_info[1].decode("UTF-8")] = joint_info[0]
# print(joint_info)
# print(joint_info[1].decode("UTF-8"))
MOTOR_NAMES = [
"fl_hip_joint", "fl_knee_joint", "fr_hip_joint", "fr_knee_joint",
"bl_hip_joint", "bl_knee_joint", "br_hip_joint", "br_knee_joint",
"fl_abd_joint", "fr_abd_joint", "bl_abd_joint", "br_abd_joint"
]
motor_id_list = [
joint_name_to_id[motor_name] for motor_name in MOTOR_NAMES
]
return joint_name_to_id, motor_id_list
def ResetLeg(self, leg_id, add_constraint, standstilltorque=10):
'''
function to reset hip and knee joints' state
Args:
leg_id : denotes leg index
add_constraint : bool to create constraints in lower joints of five bar leg mechanisim
standstilltorque : value of initial torque to set in hip and knee motors for standing condition
'''
self._pybullet_client.resetJointState(
self.stochlite,
self._joint_name_to_id["fl_hip_joint"], # motor
targetValue=0.772001,
targetVelocity=0)
self._pybullet_client.setJointMotorControl2(
bodyIndex=self.stochlite,
jointIndex=(self._joint_name_to_id["fl_hip_joint"]),
controlMode=self._pybullet_client.VELOCITY_CONTROL,
force=0,
targetVelocity=0)
self._pybullet_client.resetJointState(
self.stochlite,
self._joint_name_to_id["fr_hip_joint"],
targetValue=0.772001,
targetVelocity=0)
self._pybullet_client.setJointMotorControl2(
bodyIndex=self.stochlite,
jointIndex=(self._joint_name_to_id["fr_hip_joint"]),
controlMode=self._pybullet_client.VELOCITY_CONTROL,
force=0,
targetVelocity=0)
self._pybullet_client.resetJointState(
self.stochlite,
self._joint_name_to_id["bl_hip_joint"], # motor
targetValue=0.772001,
targetVelocity=0)
self._pybullet_client.setJointMotorControl2(
bodyIndex=self.stochlite,
jointIndex=(self._joint_name_to_id["bl_hip_joint"]),
controlMode=self._pybullet_client.VELOCITY_CONTROL,
force=0,
targetVelocity=0)
self._pybullet_client.resetJointState(
self.stochlite,
self._joint_name_to_id["br_hip_joint"],
targetValue=0.772001,
targetVelocity=0)
self._pybullet_client.setJointMotorControl2(
bodyIndex=self.stochlite,
jointIndex=(self._joint_name_to_id["br_hip_joint"]),
controlMode=self._pybullet_client.VELOCITY_CONTROL,
force=0,
targetVelocity=0)
self._pybullet_client.resetJointState(
self.stochlite,
self._joint_name_to_id["fl_knee_joint"], # motor
targetValue=-1.4129,
targetVelocity=0)
self._pybullet_client.setJointMotorControl2(
bodyIndex=self.stochlite,
jointIndex=(self._joint_name_to_id["fl_knee_joint"]),
controlMode=self._pybullet_client.VELOCITY_CONTROL,
force=0,
targetVelocity=0)
self._pybullet_client.resetJointState(
self.stochlite,
self._joint_name_to_id["fr_knee_joint"],
targetValue=-1.4129,
targetVelocity=0)
self._pybullet_client.setJointMotorControl2(
bodyIndex=self.stochlite,
jointIndex=(self._joint_name_to_id["fr_knee_joint"]),
controlMode=self._pybullet_client.VELOCITY_CONTROL,
force=0,
targetVelocity=0)
self._pybullet_client.resetJointState(
self.stochlite,
self._joint_name_to_id["bl_knee_joint"], # motor
targetValue=-1.4129,
targetVelocity=0)
self._pybullet_client.setJointMotorControl2(
bodyIndex=self.stochlite,
jointIndex=(self._joint_name_to_id["bl_knee_joint"]),
controlMode=self._pybullet_client.VELOCITY_CONTROL,
force=0,
targetVelocity=0)
self._pybullet_client.resetJointState(
self.stochlite,
self._joint_name_to_id["br_knee_joint"],
targetValue=-1.4129,
targetVelocity=0)
self._pybullet_client.setJointMotorControl2(
bodyIndex=self.stochlite,
jointIndex=(self._joint_name_to_id["br_knee_joint"]),
controlMode=self._pybullet_client.VELOCITY_CONTROL,
force=0,
targetVelocity=0)
def ResetPoseForAbd(self):
'''
Reset initial conditions of abduction joints
'''
self._pybullet_client.resetJointState(
self.stochlite,
self._joint_name_to_id["fl_abd_joint"],
targetValue=0,
targetVelocity=0)
self._pybullet_client.resetJointState(
self.stochlite,
self._joint_name_to_id["fr_abd_joint"],
targetValue=0,
targetVelocity=0)
self._pybullet_client.resetJointState(
self.stochlite,
self._joint_name_to_id["bl_abd_joint"],
targetValue=0,
targetVelocity=0)
self._pybullet_client.resetJointState(
self.stochlite,
self._joint_name_to_id["br_abd_joint"],
targetValue=0,
targetVelocity=0)
# Set control mode for each motor and initial conditions
self._pybullet_client.setJointMotorControl2(
bodyIndex=self.stochlite,
jointIndex=(self._joint_name_to_id["fl_abd_joint"]),
controlMode=self._pybullet_client.VELOCITY_CONTROL,
force=0,
targetVelocity=0)
self._pybullet_client.setJointMotorControl2(
bodyIndex=self.stochlite,
jointIndex=(self._joint_name_to_id["fr_abd_joint"]),
controlMode=self._pybullet_client.VELOCITY_CONTROL,
force=0,
targetVelocity=0)
self._pybullet_client.setJointMotorControl2(
bodyIndex=self.stochlite,
jointIndex=(self._joint_name_to_id["bl_abd_joint"]),
controlMode=self._pybullet_client.VELOCITY_CONTROL,
force=0,
targetVelocity=0)
self._pybullet_client.setJointMotorControl2(
bodyIndex=self.stochlite,
jointIndex=(self._joint_name_to_id["br_abd_joint"]),
controlMode=self._pybullet_client.VELOCITY_CONTROL,
force=0,
targetVelocity=0)
def train(env, policy, normalizer, hp, parentPipes, args):
logger = DataLog()
total_steps = 0
best_return = -99999999
if os.path.isdir(args.logdir) == False:
os.mkdir(args.logdir)
previous_dir = os.getcwd()
os.chdir(args.logdir)
if os.path.isdir('iterations') == False: os.mkdir('iterations')
if os.path.isdir('logs') == False: os.mkdir('logs')
hp.to_text('hyperparameters')
for step in range(hp.nb_steps):
# Initializing the perturbations deltas and the positive/negative rewards
deltas = policy.sample_deltas()
positive_rewards = [0] * hp.nb_directions
negative_rewards = [0] * hp.nb_directions
if (parentPipes):
process_count = len(parentPipes)
if parentPipes:
p = 0
while (p < hp.nb_directions):
temp_p = p
n_left = hp.nb_directions - p #Number of processes required to complete the search
for k in range(min([process_count, n_left])):
parentPipe = parentPipes[k]
parentPipe.send([
_EXPLORE,
[normalizer, policy, hp, "positive", deltas[temp_p]]
])
temp_p = temp_p + 1
temp_p = p
for k in range(min([process_count, n_left])):
positive_rewards[temp_p], step_count = parentPipes[k].recv(
)
total_steps = total_steps + step_count
temp_p = temp_p + 1
temp_p = p
for k in range(min([process_count, n_left])):
parentPipe = parentPipes[k]
parentPipe.send([
_EXPLORE,
[normalizer, policy, hp, "negative", deltas[temp_p]]
])
temp_p = temp_p + 1
temp_p = p
for k in range(min([process_count, n_left])):
negative_rewards[temp_p], step_count = parentPipes[k].recv(
)
total_steps = total_steps + step_count
temp_p = temp_p + 1
p = p + process_count
# print('mp step has worked, ', p)
print('total steps till now: ', total_steps,
'Processes done: ', p)
else:
# Getting the positive rewards in the positive directions
for k in range(hp.nb_directions):
positive_rewards[k] = explore(env, policy, "positive",
deltas[k], hp)
# Getting the negative rewards in the negative/opposite directions
for k in range(hp.nb_directions):
negative_rewards[k] = explore(env, policy, "negative",
deltas[k], hp)
# Sorting the rollouts by the max(r_pos, r_neg) and selecting the best directions
scores = {
k: max(r_pos, r_neg)
for k, (
r_pos,
r_neg) in enumerate(zip(positive_rewards, negative_rewards))
}
order = sorted(scores.keys(),
key=lambda x: -scores[x])[:int(hp.nb_best_directions)]
rollouts = [(positive_rewards[k], negative_rewards[k], deltas[k])
for k in order]
# Gathering all the positive/negative rewards to compute the standard deviation of these rewards
all_rewards = np.array([x[0]
for x in rollouts] + [x[1] for x in rollouts])
sigma_r = all_rewards.std(
) # Standard deviation of only rewards in the best directions is what it should be
# Updating our policy
policy.update(rollouts, sigma_r, args)
# Printing the final reward of the policy after the update
reward_evaluation = explore(env, policy, None, None, hp)
logger.log_kv('steps', step)
logger.log_kv('return', reward_evaluation)
if (reward_evaluation > best_return):
best_policy = policy.theta
best_return = reward_evaluation
np.save("iterations/best_policy.npy", best_policy)
print('Step:', step, 'Reward:', reward_evaluation)
policy_path = "iterations/" + "policy_" + str(step)
np.save(policy_path, policy.theta)
logger.save_log('logs/')
make_train_plots_ars(log=logger.log,
keys=['steps', 'return'],
save_loc='logs/')