def change_policy(self, condition, policy, new_ref_ja, new_ref_ee):
# Save all of these because we're about to initialize with something newww
old_K, old_k = policy.K, policy.k
# This was how many timesteps there were before, not including the padding!
old_T = old_K.shape[0] - self.padding
with open('old_policy.txt', 'w') as f:
noise = np.zeros((old_K.shape[0], self.dU))
f.write(str(policy_to_msg(policy, noise)))
old_pol_covar, old_chol_pol_covar = policy.pol_covar, policy.chol_pol_covar
old_inv_pol_covar = policy.inv_pol_covar
old_T = min(
old_T, self.T) # Make sure to choose the smaller one I think (???)
# Now we change the initial values to match what we learned before
policy.__init__(*init_pd_ref(self._hyperparams['init_traj_distr'],
new_ref_ja, new_ref_ee))
policy.K[:
old_T, :, :], policy.k[:
old_T, :] = old_K[:
old_T, :, :], old_k[:
old_T, :]
policy.pol_covar[:
old_T, :, :], policy.chol_pol_covar[:
old_T, :, :] = old_pol_covar[:
old_T, :, :], old_chol_pol_covar[:
old_T, :, :]
policy.inv_pol_covar[:old_T, :, :] = old_inv_pol_covar[:old_T, :, :]
# Writing the new policy to a text file just so we can examine
with open('new_policy.txt', 'w') as f:
noise = np.zeros((self.T, self.dU))
f.write(str(policy_to_msg(policy, noise)))
def sample(self,
policy,
condition,
reset=True,
verbose=True,
save=True,
noisy=True):
"""
Reset and execute a policy and collect a sample.
Args:
policy: A Policy object.
condition: Which condition setup to run.
verbose: Unused for this agent.
save: Whether or not to store the trial into the samples.
noisy: Whether or not to use noise during sampling.
Returns:
sample: A Sample object.
"""
if TfPolicy is not None: # user has tf installed.
if isinstance(policy, TfPolicy):
self._init_tf(policy.dU)
if reset:
self.reset(condition)
# Generate noise.
if noisy:
noise = generate_noise(self.T, self.dU, self._hyperparams)
else:
noise = np.zeros((self.T, self.dU))
# Execute trial.
trial_command = TrialCommand()
trial_command.id = self._get_next_seq_id()
trial_command.controller = policy_to_msg(policy, noise)
trial_command.T = self.T
trial_command.id = self._get_next_seq_id()
trial_command.frequency = self._hyperparams['frequency']
ee_points = self._hyperparams['end_effector_points']
trial_command.ee_points = ee_points.reshape(ee_points.size).tolist()
trial_command.ee_points_tgt = \
self._hyperparams['ee_points_tgt'][condition].tolist()
trial_command.state_datatypes = self._hyperparams['state_include']
trial_command.obs_datatypes = self._hyperparams['state_include']
if self.use_tf is False:
sample_msg = self._trial_service.publish_and_wait(
trial_command, timeout=self._hyperparams['trial_timeout'])
sample = msg_to_sample(sample_msg, self)
if save:
self._samples[condition].append(sample)
return sample
else:
self._trial_service.publish(trial_command)
sample_msg = self.run_trial_tf(
policy, time_to_run=self._hyperparams['trial_timeout'])
sample = msg_to_sample(sample_msg, self)
if save:
self._samples[condition].append(sample)
return sample
def get_lin_gauss_test(T=50):
dX = 14
x0 = np.zeros(dX)
x0[0] = 1.0
lgpol = init_pd({'init_var': 0.01}, x0, 7, 7, dX, T)
print 'T:', lgpol.T
print 'dX:', lgpol.dX
#Conver lgpol to message
noise = np.zeros((T, 7))
controller_params = policy_to_msg(lgpol, noise)
return controller_params
def get_lin_gauss_test(T=50):
dX = 14
x0 = np.zeros(dX)
x0[0] = 1.0
lgpol = init_pd({'init_var': 0.01}, x0, 7, 7, dX, T)
print 'T:', lgpol.T
print 'dX:', lgpol.dX
#Conver lgpol to message
noise = np.zeros((T, 7))
controller_params = policy_to_msg(lgpol, noise)
return controller_params
def sample(self, policy, condition, verbose=True, save=True, noisy=True):
"""
Reset and execute a policy and collect a sample.
Args:
policy: A Policy object.
condition: Which condition setup to run.
verbose: Unused for this agent.
save: Whether or not to store the trial into the samples.
noisy: Whether or not to use noise during sampling.
Returns:
sample: A Sample object.
"""
if TfPolicy is not None: # user has tf installed.
if isinstance(policy, TfPolicy):
self._init_tf(policy.dU)
self.reset(condition)
# Generate noise.
if noisy:
noise = generate_noise(self.T, self.dU, self._hyperparams)
else:
noise = np.zeros((self.T, self.dU))
# Execute trial.
trial_command = TrialCommand()
trial_command.id = self._get_next_seq_id()
trial_command.controller = policy_to_msg(policy, noise)
trial_command.T = self.T
trial_command.id = self._get_next_seq_id()
trial_command.frequency = self._hyperparams['frequency']
ee_points = self._hyperparams['end_effector_points']
trial_command.ee_points = ee_points.reshape(ee_points.size).tolist()
trial_command.ee_points_tgt = \
self._hyperparams['ee_points_tgt'][condition].tolist()
trial_command.state_datatypes = self._hyperparams['state_include']
trial_command.obs_datatypes = self._hyperparams['state_include']
if self.use_tf is False:
sample_msg = self._trial_service.publish_and_wait(
trial_command, timeout=self._hyperparams['trial_timeout']
)
sample = msg_to_sample(sample_msg, self)
if save:
self._samples[condition].append(sample)
return sample
else:
self._trial_service.publish(trial_command)
sample_msg = self.run_trial_tf(policy, time_to_run=self._hyperparams['trial_timeout'])
sample = msg_to_sample(sample_msg, self)
if save:
self._samples[condition].append(sample)
return sample
def get_lin_gauss_test(T=50):
dX = 4
x0 = np.zeros(dX)
x0[0] = 1.0
# lgpol = init_lqr({'x0' : x0, 'dU' : 2, 'dX' : dX, 'T' : T, 'dt' : 0.05})
lgpol = init_pd({\
'init_var' : 1.0, 'pos_gains' : 0.0,\
'dQ' : 2, 'dt' : 0.05, 'x0' : x0,\
'dU' : 2, 'dX' : 4, 'T' : T\
})
print 'T:', lgpol.T
print 'dX:', lgpol.dX
#Conver lgpol to message
noise = np.zeros((T, 2))
controller_params = policy_to_msg(lgpol, noise)
return controller_params
def sample(self,
policy,
condition,
reset=True,
verbose=True,
save=True,
noisy=True):
"""
Reset and execute a policy and collect a sample.
Args:
policy: A Policy object.
condition: Which condition setup to run.
verbose: Unused for this agent.
save: Whether or not to store the trial into the samples.
noisy: Whether or not to use noise during sampling.
Returns:
sample: A Sample object.
"""
if reset:
self.reset(condition)
# Generate noise.
if noisy:
noise = generate_noise(self.T, self.dU, self._hyperparams)
else:
noise = np.zeros((self.T, self.dU))
# noise = noise*0.01
# Execute trial.
trial_command = TrialCommand()
trial_command.id = self._get_next_seq_id()
trial_command.controller = policy_to_msg(policy, noise)
trial_command.T = self.T
trial_command.id = self._get_next_seq_id()
trial_command.frequency = self._hyperparams['frequency']
# ee_points and ee_points_tgt is uneccesary for mobile robot
trial_command.ee_points = []
trial_command.ee_points_tgt = []
trial_command.state_datatypes = self._hyperparams['state_include']
trial_command.obs_datatypes = self._hyperparams['obs_include']
sample_msg = self._trial_service.publish_and_wait(
trial_command, timeout=self._hyperparams['trial_timeout'])
sample = msg_to_sample(sample_msg, self)
if save:
self._samples[condition].append(sample)
return sample
def execute(self, policy):
"""
Reset and execute a policy and collect a sample.
Args:
policy: A Policy object.
Returns:
sample: A Sample object.
"""
if TfPolicy is not None: # user has tf installed.
if isinstance(policy, TfPolicy):
self._init_tf(policy.dU)
#self.reset(condition)
# Generate noise.
noise = generate_noise(self.T, self.dU, self._hyperparams)
# Execute trial.
trial_command = TrialCommand()
trial_command.id = self._get_next_seq_id()
trial_command.controller = policy_to_msg(policy, noise)
trial_command.T = self.T
trial_command.id = self._get_next_seq_id()
trial_command.frequency = self._hyperparams['frequency']
ee_points = self._hyperparams['end_effector_points']
trial_command.ee_points = ee_points.reshape(ee_points.size).tolist()
#### @todo: tgt command try ##########Brook####################################################
#trial_command.ee_points_tgt = self._tgt_subscribe.subscribe_and_wait()
trial_command.ee_points_tgt = \
self._hyperparams['ee_points_tgt'][0].tolist()
trial_command.state_datatypes = self._hyperparams['state_include']
trial_command.obs_datatypes = self._hyperparams['state_include']
if self.use_tf is False:
sample_msg = self._trial_service.publish_and_wait(
trial_command, timeout=self._hyperparams['trial_timeout'])
sample = msg_to_sample(sample_msg, self)
else:
self._trial_service.publish(trial_command)
sample_msg = self.run_trial_tf(
policy, time_to_run=self._hyperparams['trial_timeout'])
sample = msg_to_sample(sample_msg, self)
def execute(self, policy):
"""
Reset and execute a policy and collect a sample.
Args:
policy: A Policy object.
Returns:
sample: A Sample object.
"""
if TfPolicy is not None: # user has tf installed.
if isinstance(policy, TfPolicy):
self._init_tf(policy.dU)
#self.reset(condition)
# Generate noise.
noise = generate_noise(self.T, self.dU, self._hyperparams)
# Execute trial.
trial_command = TrialCommand()
trial_command.id = self._get_next_seq_id()
trial_command.controller = policy_to_msg(policy, noise)
trial_command.T = self.T
trial_command.id = self._get_next_seq_id()
trial_command.frequency = self._hyperparams['frequency']
ee_points = self._hyperparams['end_effector_points']
trial_command.ee_points = ee_points.reshape(ee_points.size).tolist()
#### @todo: tgt command try ##########Brook####################################################
#trial_command.ee_points_tgt = self._tgt_subscribe.subscribe_and_wait()
trial_command.ee_points_tgt = \
self._hyperparams['ee_points_tgt'][0].tolist()
trial_command.state_datatypes = self._hyperparams['state_include']
trial_command.obs_datatypes = self._hyperparams['state_include']
if self.use_tf is False:
sample_msg = self._trial_service.publish_and_wait(
trial_command, timeout=self._hyperparams['trial_timeout']
)
sample = msg_to_sample(sample_msg, self)
else:
self._trial_service.publish(trial_command)
sample_msg = self.run_trial_tf(policy, time_to_run=self._hyperparams['trial_timeout'])
sample = msg_to_sample(sample_msg, self)
def sample(self, policy, condition, verbose=True, save=True):
"""
Reset and execute a policy and collect a sample.
Args:
policy: A Policy object.
condition: Which condition setup to run.
verbose: Unused for this agent.
save: Whether or not to store the trial into the samples.
Returns:
sample: A Sample object.
"""
self.reset(condition)
# Generate noise.
noise = generate_noise(self.T, self.dU, self._hyperparams)
# Execute trial.
trial_command = TrialCommand()
trial_command.id = self._get_next_seq_id()
trial_command.controller = policy_to_msg(policy, noise)
trial_command.T = self.T
trial_command.id = self._get_next_seq_id()
trial_command.frequency = self._hyperparams['frequency']
ee_points = self._hyperparams['end_effector_points']
trial_command.ee_points = ee_points.reshape(ee_points.size).tolist()
trial_command.ee_points_tgt = \
self._hyperparams['ee_points_tgt'][condition].tolist()
trial_command.state_datatypes = self._hyperparams['state_include']
trial_command.obs_datatypes = self._hyperparams['state_include']
sample_msg = self._trial_service.publish_and_wait(
trial_command, timeout=self._hyperparams['trial_timeout']
)
sample = msg_to_sample(sample_msg, self)
if save:
self._samples[condition].append(sample)
return sample
def sample(self, policy, condition, verbose=True, save=True, noisy=True):
"""
Reset and execute a policy and collect a sample.
Args:
policy: A Policy object.
condition: Which condition setup to run.
verbose: Unused for this agent.
save: Whether or not to store the trial into the samples.
Returns:
sample: A Sample object.
"""
# There are different trajectories based on if reset or not
if self.reset_time:
trajectories = self.reset_trajectories
else:
trajectories = self.trajectories
if condition not in trajectories: # If this hasn't been initialized yet
if self.reset_time:
self.init_reset_traj(condition, policy)
else:
self.compute_reference_trajectory(condition, policy)
self.samples_taken[condition] += 1 # Increment number of samples taken
# Every some many samples, take more of the trajectory
# Unless we have reached the end or we are using varying T
if self.samples_taken[condition] % (self.num_samples * self.iter_per_seg) == 0 and self.iter_count != 0 \
and (self.T != self.final_T or self.varying_T):
self.update_T_then_policy(policy, condition)
self.T = self.cur_T[
condition] # Make sure the T is correct for the condition we are on
ref_traj_info = trajectories[condition]
# Length of the trajectory
traj_length = len(self.full_ref_ee[condition])
# This is how long the current trajectory we're using is - self.T
if self.T == self.final_T: # If we have gotten to the whole trajectory
ref_traj = self.trajectories[condition][
'ee'] # Current reference trajectory
else: # Otherwise pad the reference trajectory as well
ref_traj = self.trajectories[condition]['ee'][:self.T -
self.padding]
ref_traj.extend([
self.trajectories[condition]['ee'][self.T - self.padding - 1]
] * self.padding)
print('The length of the trajectory we are currently using is ' +
str(self.T))
print 'Sampling, condition', condition
self.reset(condition)
#added from agent_ros.py of public gps codebase
if TfPolicy is not None: # user has tf installed.
if isinstance(policy, TfPolicy):
print('well this got called')
self._init_tf(policy.dU)
# Generate noise.
if noisy:
noise = generate_noise(self.T, self.dU, self._hyperparams)
else:
noise = np.zeros((self.T, self.dU))
# Execute trial.
trial_command = TrialCommand()
#trial_command.id = self._get_next_seq_id()
trial_command.controller = policy_to_msg(policy, noise)
if self.samples_taken[
condition] % self.num_samples == 0 and self.samples_taken[
condition] != 0:
self.iter_count += 1 # This is the full count
self.pickle_self() # Pickle self and send to data files lmaooo
with open(
'iter' + str(self.iter_count) + '_cond' + str(condition) +
'.txt', 'w') as f:
the_noise = np.zeros((self.T, self.dU))
f.write(str(policy_to_msg(policy, the_noise)))
trial_command.T = self.T
trial_command.id = self._get_next_seq_id()
trial_command.frequency = self._hyperparams['frequency']
ee_points = self._hyperparams['end_effector_points']
trial_command.ee_points = ee_points.reshape(ee_points.size).tolist()
trial_command.ee_points_tgt = ref_traj[-1]
trial_command.state_datatypes = self._hyperparams['state_include']
trial_command.obs_datatypes = self._hyperparams['state_include']
if self.use_tf is False:
sample_msg = self._trial_service.publish_and_wait(
trial_command, timeout=self._hyperparams['trial_timeout'])
else:
self._trial_service.publish(trial_command)
sample_msg = self.run_trial_tf(
policy, time_to_run=self._hyperparams['trial_timeout'])
sample = msg_to_sample(sample_msg, self)
sample.set('target_traj_ee_points',
[points - ref_traj[-1] for points in ref_traj])
sample.set(REF_OFFSETS, ref_traj_info['offsets'][:self.T])
sample.set(REF_TRAJ,
np.array([ref_traj_info['offsets'].flatten()] * self.T))
if save:
self._samples[condition].append(sample)
if self.varying_T: # Only save this if you are gonna use varying T
self.saved_samples[condition].append(
sample) # Save it here too just in case
self.reset(condition) # Might as well reset for the heck of it
return sample
def sample(self, policy, condition, verbose=True, save=True, noisy=True):
"""
Reset and execute a policy and collect a sample.
Args:
policy: A Policy object.
condition: Which condition setup to run.
verbose: Unused for this agent.
save: Whether or not to store the trial into the samples.
noisy: Whether or not to use noise during sampling.
Returns:
sample: A Sample object.
"""
if TfPolicy is not None: # user has tf installed (try import)
if isinstance(policy, TfPolicy): # False, policy = Linear Gaussian policy
self._init_tf(policy.dU)
# Reset the agent for a particular experiment condition
self.reset(condition)
# Generate noise.
if noisy:
noise = generate_noise(self.T, self.dU, self._hyperparams)
else:
noise = np.zeros((self.T, self.dU))
# Execute trial.
trial_command = TrialCommand() # ROS message
trial_command.id = self._get_next_seq_id()
trial_command.controller = policy_to_msg(policy, noise) # ControllerParams
trial_command.T = self.T # Trajectory length
trial_command.id = self._get_next_seq_id()
trial_command.frequency = self._hyperparams['frequency'] # Controller frequency
ee_points = self._hyperparams['end_effector_points'] # 3*n_points array containing offsets
trial_command.ee_points = ee_points.reshape(ee_points.size).tolist()
trial_command.ee_points_tgt = \
self._hyperparams['ee_points_tgt'][condition].tolist() # 3*n_points array containing the desired ee_points for this trial
trial_command.state_datatypes = self._hyperparams['state_include'] # Which data types to include in state
trial_command.obs_datatypes = self._hyperparams['state_include'] # Which data types to include in observation
# ------------- Local Policy -------------
# use_tf is False
if self.use_tf is False:
# self._trial_service = ServiceEmulator(
# self._hyperparams['trial_command_topic'], TrialCommand,
# self._hyperparams['sample_result_topic'], SampleResult
# )
# Publish a message and wait for the response
sample_msg = self._trial_service.publish_and_wait(
trial_command, timeout=self._hyperparams['trial_timeout']
)
sample = msg_to_sample(sample_msg, self)
# Saving the samples for tf[?]
if save:
self._samples[condition].append(sample)
return sample
else:
self._trial_service.publish(trial_command)
# Run an async controller from a policy.
# The async controller receives observations from ROS subscribers
# and then uses them to publish actions
sample_msg = self.run_trial_tf(policy,
time_to_run=self._hyperparams['trial_timeout'])
sample = msg_to_sample(sample_msg, self)
if save:
self._samples[condition].append(sample)
return sample
def sample(self, policy, condition, verbose=True, save=True, noisy=True):
"""
Reset and execute a policy and collect a sample.
Args:
policy: A Policy object.
condition: Which condition setup to run.
verbose: Unused for this agent.
save: Whether or not to store the trial into the samples.
noisy: Whether or not to use noise during sampling.
Returns:
sample: A Sample object.
"""
if TfPolicy is not None: # user has tf installed.
if isinstance(policy, TfPolicy):
self._init_tf(policy.dU)
self.reset(condition)
# Generate noise.
if noisy:
noise = generate_noise(self.T, self.dU, self._hyperparams)
else:
noise = np.zeros((self.T, self.dU))
# Execute trial.
trial_command = TrialCommand()
trial_command.id = self._get_next_seq_id()
trial_command.controller = policy_to_msg(policy, noise)
trial_command.T = self.T
trial_command.id = self._get_next_seq_id()
trial_command.frequency = self._hyperparams['frequency']
ee_points = self._hyperparams['end_effector_points']
trial_command.ee_points = ee_points.reshape(ee_points.size).tolist()
trial_command.ee_points_tgt = \
self._hyperparams['ee_points_tgt'][condition].tolist()
trial_command.state_datatypes = self._hyperparams['state_include']
trial_command.obs_datatypes = self._hyperparams['obs_include']
if self.use_tf is False or not isinstance(policy, TfPolicy):
print 'Not using TF controller'
sample_msg = self._trial_service.publish_and_wait(
trial_command, timeout=self._hyperparams['trial_timeout']
)
sample = msg_to_sample(sample_msg, self)
if save:
self._samples[condition].append(sample)
return sample
else:
'''
print 'Using TF controller'
self._trial_service.publish(trial_command)
sample_msg = self.run_trial_tf(policy, condition, time_to_run=self._hyperparams['trial_timeout'])
pdb.set_trace()
sample = msg_to_sample(sample_msg, self)
if save:
self._samples[condition].append(sample)
return sample
'''
self.trial_manager.prep(policy, condition)
self._trial_service.publish(trial_command, wait=True)
self.trial_manager.run(self._hyperparams['trial_timeout'])
while self._trial_service._waiting:
print 'Waiting for sample to come in'
rospy.sleep(1.0)
sample_msg = self._trial_service._subscriber_msg
sample = msg_to_sample(sample_msg, self)
sample.set(NOISE, noise)
sample.set(TIMESTEP, np.arange(self.T).reshape((self.T,1)))
return sample
