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 compute_reference_trajectory(self, condition, policy):
super(GearExperiment, self).reset(condition) # Call the super method
target = self._hyperparams['targets'][condition]
best_ref_ee, best_ref_ja = None, None # HAHAHAHAHA
best_dist = float("inf") # Infinity itself
best_plan = None
best_T = None # Set this up
self.T = self.final_T # Gotta use the final T or something like that
while True:
for attempt in range(
self.attempts): # Let's do asked attempts on plan
if self.use_saved_traj: # If there is a plan to read
best_plan = self.load_plan(self.plan_filename)
best_dist = self.get_dist(best_plan) # Best plan!
break # We can leave this place
else: # Otherwise just create a motion plan
plan = self.plan_end_effector(target['position'],
target['orientation'])
if plan is not None:
cur_dist = self.get_dist(
plan) # Get the distance of cur plan
#self.dists.append(cur_dist)
# If it beats it we need to use it! Woah!
if cur_dist < best_dist:
best_dist = cur_dist # This is the current best distance
best_plan = plan # This is the best plan
# Save the display trajectory and stuff
self.best_saved_traj[condition] = self.saved_traj
copy_plan = copy.deepcopy(best_plan) # Copy the plan
self.reverse_plan(copy_plan) # Reverse the plan
self.reset_plans[
condition] = copy_plan # This is the plan to get out of here!
# Only continue on with these calculations if necessary
plan_joints = [
np.array(point.positions)
for point in best_plan.joint_trajectory.points
]
best_ref_ja = interpolate(plan_joints, self.T_interpolation)
best_ref_ja.extend([best_ref_ja[-1]] *
(self.T - self.T_interpolation))
ref_poses = self.forward_kinematics(best_ref_ja, 'torso_lift_link')
best_ref_ee = []
ee_offsets = self._hyperparams['end_effector_points']
for pose in ref_poses:
position, orientation = np.array([pose[:3]]), pose[3:]
rotation_mat = quaternion_matrix(orientation)[:3, :3]
points = np.ndarray.flatten(
get_ee_points(ee_offsets, position, rotation_mat).T)
best_ref_ee.append(points)
# Publish it so it is the last thing you see before question
self.publishDisplayTrajectory(self.best_saved_traj[condition])
if not self.require_approval or yesno(
'Does this trajectory look ok?'):
print("this is the distance of the best one: " +
str(best_dist))
break
if self.use_saved_traj: # If we already have one
with open(self.pickled_traj_filename,
'r') as f: # Read from the pickled place
self.best_saved_traj[condition] = pickle.load(
f) # Load that pickled DisplayTrajectory!
self.publishDisplayTrajectory(
self.best_saved_traj[condition]) # Publish it so it is the last
#with open('the_distances.txt', 'w') as f:
# f.write('These are the distances: \n')
# f.write(str(self.dists))
# f.write('\nThis is the mean distance: ' + str(np.mean(np.array(self.dists))) + '\n')
# f.write('Success rate: ' + str(len(self.dists)) + ' / ' + str(self.attempts) + '\n')
self.save_plan(best_plan) # Save the trajectory
if self.varying_T: # If we are going for the varying T plan
start_ind = self.final_T # Start off with the full timestep length
if self.chosen_parts is None: # If there isn't a specific plan segment chunk
# For now just take one / segments of the references to initialize
start_ind = int(1.0 / self.segments * len(best_ref_ee))
else: # If there is, just use it
#closest_T = find_closest_T(best_ref_ja, best_ref_ee, B4_PEG_JA, B4_PEG_EE)
closest_T = 139
self.chosen_parts[
0] = closest_T - 10 # Just a little bit of buffer lmao
start_ind = self.chosen_parts[
0] # Set the starting ind to what we have chosen
print("This is the closest found T: " + str(closest_T))
# Provide copies of the reference ee and ja
self.full_ref_ee[condition] = list(np.copy(np.array(best_ref_ee)))
self.full_ref_ja[condition] = list(np.copy(np.array(best_ref_ja)))
self.T = start_ind # How many there is to use
# If we are just taking part of the trajectory
if start_ind != self.final_T: # Add padding
new_ref_ja, new_ref_ee = best_ref_ja[:
start_ind], best_ref_ee[:
start_ind]
# This is adding more timesteps to the last step in the segment
new_ref_ja.extend([best_ref_ja[start_ind - 1]] * self.padding)
new_ref_ee.extend([best_ref_ee[start_ind - 1]] * self.padding)
self.T += self.padding # Put in the padding lmao
else: # Otherwise, the goal already has padding
new_ref_ja, new_ref_ee = best_ref_ja, best_ref_ee
# Initialize using the beginning part of the trajectory
policy.__init__(*init_pd_ref(self._hyperparams['init_traj_distr'],
new_ref_ja, new_ref_ee))
with open('pickled_robot_traj_cond' + str(condition), 'w') as f:
pickle.dump(self.best_saved_traj[condition], f)
ref_offsets = np.array(best_ref_ee) - best_ref_ee[-1]
traj_info = {
'ja': np.array(best_ref_ja),
'ee': np.array(best_ref_ee),
'offsets': ref_offsets,
'flattened': ref_offsets.flatten()
}
self.cur_T[condition] = self.T # Update this as well
self.trajectories[condition] = traj_info
return traj_info
def compute_reference_trajectory(self, condition, policy):
self.reset(condition)
target = self._hyperparams['targets'][condition]
best_ref_ee = None # HAHAHAHAHA to store the best one
best_ref_ja = None # HAHAHAHAHA to store the best one
best_dist = float("inf") # Infinity itself
best_plan = None
plans_made = 0 # Count how many plans were actually created
while True:
for attempt in range(self.attempts): # Make this many attempts
plan = self.plan_end_effector(target['position'],
target['orientation'])
if plan is None: # Leave this iteration lmao
continue
plans_made += 1 # Increment plans made
cur_dist = self.get_dist(plan) # Get the current distance
# If it beats it we need to use it! Woah!
if cur_dist < best_dist:
plan_joints = [
np.array(point.positions)
for point in plan.joint_trajectory.points
]
ref_ja = interpolate(plan_joints, self.T_interpolation)
ref_ja.extend([ref_ja[-1]] *
(self.T - self.T_interpolation))
ref_poses = self.forward_kinematics(
ref_ja, 'torso_lift_link')
ref_ee = []
ee_offsets = self._hyperparams['end_effector_points']
for pose in ref_poses:
position, orientation = np.array([pose[:3]]), pose[3:]
rotation_mat = quaternion_matrix(orientation)[:3, :3]
points = np.ndarray.flatten(
get_ee_points(ee_offsets, position,
rotation_mat).T)
ref_ee.append(points)
best_dist = cur_dist
best_ref_ee, best_ref_ja = ref_ee, ref_ja
best_plan = plan
self.best_saved_traj = self.saved_traj # Save the trajectory and stuff
# Plot the very best plan that we found!
plot_trajectories([best_ref_ee])
if not self.require_approval or yesno(
'Does this trajectory look ok?'):
print("Of all the " + str(plans_made) + " plans made, ")
print("this is the distance of the best one: " +
str(best_dist))
break
if self.use_saved_traj: # If we already have one
with open(self.pickled_traj_filename,
'r') as f: # Read from the pickled place
self.best_saved_traj = pickle.load(
f) # Load that pickled DisplayTrajectory!
self.publishDisplayTrajectory(
self.best_saved_traj) # Publish it so it is the last
self.trajectories[condition] = best_ref_ee
policy.__init__(*init_pd_ref(self._hyperparams['init_traj_distr'],
best_ref_ja, best_ref_ee))
with open('pickled_robot_traj', 'w') as f:
pickle.dump(self.best_saved_traj, f)
ref_offsets = np.array(best_ref_ee) - best_ref_ee[-1]
return {
'ja': np.array(best_ref_ja),
'ee': np.array(best_ref_ee),
'offsets': ref_offsets,
'flattened': ref_offsets.flatten()
}