def get_target_pose(self, target_offset): pos = self.target_model_pose.position rot = self.target_model_pose.orientation pos_arr = np.array([[pos.x, pos.y, pos.z - .93]]) rot_arr = Quaternion(x=rot.x, y=rot.y, z=rot.z, w=rot.w).rotation_matrix return np.ndarray.flatten( get_ee_points(target_offset, pos_arr, rot_arr).T)
def __get_ee_pose(self, joint_angles): EE_POINTS = np.array(self._hyperparams['end_effector_points']) pose = self.arm.kinematics.forward_position_kinematics( joint_angles) #[x, y, z, ax, ay, az, w] translation = np.array(pose[:3]).reshape(1, 3) rot = Quaternion(np.roll(pose[3:], 1)).rotation_matrix return np.ndarray.flatten( get_ee_points(EE_POINTS, np.array(translation).reshape((1, 3)), rot).T)
def request_goal_state(self, event=None): self.request_mode('goalstate') sample = self._agent.get_data(arm=self._actuator_type) ja = sample.get(JOINT_ANGLES) ee_pos = sample.get(END_EFFECTOR_POSITIONS) ee_rot = sample.get(END_EFFECTOR_ROTATIONS) ee_tgt = np.ndarray.flatten( get_ee_points(self._agent._hyperparams['ee_points'], ee_pos, ee_rot).T ) self._agent._hyperparams['ee_points_tgt'] = [ee_tgt] self._target_position = (ja, ee_pos, ee_rot) self._agent._target_ja = [ja]
rospy.init_node('gps_agent_ur_ros_node') # Set starting end effector position using TF tf = TransformListener() # Sleep for .1 secs to give the node a chance to kick off rospy.sleep(1) time = tf.getLatestCommonTime(WRIST_3_LINK, BASE_LINK) x0[joint_dim:(joint_dim + NUM_EE_POINTS * EE_POINTS.shape[1])] = get_position( tf, WRIST_3_LINK, BASE_LINK, time) # Initialize target end effector position ee_tgt = np.ndarray.flatten( get_ee_points(EE_POINTS, ee_pos_tgt, ee_rot_tgt).T) reset_condition = {JOINT_ANGLES: INITIAL_JOINTS, JOINT_VELOCITIES: []} x0s.append(x0) ee_tgts.append(ee_tgt) reset_conditions.append(reset_condition) if not os.path.exists(common['data_files_dir']): os.makedirs(common['data_files_dir']) agent = { 'type': AgentURROS, 'dt': TIMESTEP,
for i in xrange(common['conditions']): ja_x0, ee_pos_x0, ee_rot_x0 = load_pose_from_npz( common['target_filename'], 'trial_arm', str(i), 'initial' ) ja_aux, _, _ = load_pose_from_npz( common['target_filename'], 'auxiliary_arm', str(i), 'initial' ) _, ee_pos_tgt, ee_rot_tgt = load_pose_from_npz( common['target_filename'], 'trial_arm', str(i), 'target' ) x0 = np.zeros(32) x0[:7] = ja_x0 x0[14:(14+9)] = np.ndarray.flatten( get_ee_points(EE_POINTS, ee_pos_x0, ee_rot_x0).T ) ee_tgt = np.ndarray.flatten( get_ee_points(EE_POINTS, ee_pos_tgt, ee_rot_tgt).T ) aux_x0 = np.zeros(7) aux_x0[:] = ja_aux reset_condition = { TRIAL_ARM: { 'mode': JOINT_SPACE, 'data': x0[0:7], }, AUXILIARY_ARM: {
for i in xrange(common['conditions']): ja_x0, ee_pos_x0, ee_rot_x0 = load_pose_from_npz( common['target_filename'], 'trial_arm', str(i), 'initial' ) ja_aux, _, _ = load_pose_from_npz( common['target_filename'], 'auxiliary_arm', str(i), 'initial' ) _, ee_pos_tgt, ee_rot_tgt = load_pose_from_npz( common['target_filename'], 'trial_arm', str(i), 'target' ) x0 = np.zeros(32) x0[:7] = ja_x0 x0[14:(14+3*EE_POINTS.shape[0])] = np.ndarray.flatten( get_ee_points(EE_POINTS, ee_pos_x0, ee_rot_x0).T ) ee_tgt = np.ndarray.flatten( get_ee_points(EE_POINTS, ee_pos_tgt, ee_rot_tgt).T ) aux_x0 = np.zeros(7) aux_x0[:] = ja_aux reset_condition = { TRIAL_ARM: { 'mode': JOINT_SPACE, 'data': x0[0:7], }, AUXILIARY_ARM: {
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
ee_rot_tgts = np.array([[[-0.9987, -0.0500, 0.0094],\ [-0.0498, 0.9986, 0.0150],\ [-0.0101, 0.0145, -0.9998]],\ [[-0.9987, -0.0500, 0.0094],\ [-0.0498, 0.9986, 0.0150],\ [-0.0101, 0.0145, -0.9998]]]) tgt_state = np.loadtxt(EXP_DIR + 'target_state', delimiter=',') # TODO(chelsea/zoe) : Move this code to a utility function # Set up each condition. for i in xrange(common['conditions']): x0 = np.zeros(24) x0[:3] = ja_x0s[0] x0[6:(6+3*EE_POINTS.shape[0])] = np.ndarray.flatten( get_ee_points(EE_POINTS, ee_pos_x0s[0], ee_rot_x0s[0]).T ) ee_tgt = np.ndarray.flatten( get_ee_points(EE_POINTS, ee_pos_tgts[0], ee_rot_tgts[0]).T ) aux_x0 = np.zeros(3) aux_x0[:] = ja_x0s[0] reset_condition = { TRIAL_ARM: { 'mode': JOINT_SPACE, 'data': x0[0:3], }, AUXILIARY_ARM: {
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() }
) ja_aux, _, _ = load_pose_from_npz( common['target_filename'], 'auxiliary_arm', str(i), 'initial' ) _, ee_pos_tgt, ee_rot_tgt = load_pose_from_npz( common['target_filename'], 'trial_arm', str(i), 'target' ) ''' ee_rot_x0 = np.array([[0.6086, -0.72184, 0.3293052], [-0.546578, 0.37591, 0.92504], [-0.791526, -0.58105, 0.18935]]) x0 = np.zeros((SENSOR_DIMS[JOINT_ANGLES] * 2 + 3 * EE_POINTS.shape[0] * 2)) x0[:5] = [1.5596, 0.12270, -1.22824, -0.5233, -0.0] x0[10:(10 + 3 * EE_POINTS.shape[0])] = np.ndarray.flatten( get_ee_points(EE_POINTS, np.array([0.125645, -0.1788422, -0.36095773]), ee_rot_x0).T) ee_ja = [0.256174, 0.22396, -0.854427, -1.030, -0.86368] ee_rot_tgt = np.array([[0.6867, 0.5401067, 0.48645617], [-0.4588635, -0.19689, 0.866416], [0.56373678, -0.58105, 0.18935]]) ee_tgt = np.ndarray.flatten( get_ee_points(EE_POINTS, np.array([0.31013, -0.251094, 0.07471]), ee_rot_tgt).T) reset_condition = { TRIAL_ARM: { 'mode': JOINT_SPACE, 'data': x0[0:5], }, }
ee_pos_tgt = np.array(eeposes[i]).reshape(1, 3) ee_rot_quat = Quaternion(np.roll( eerot[i], 1)) # shift one position so that we have [w,x,y,z] ee_rot_tgt = ee_rot_quat.rotation_matrix.reshape((3, 3)) state_space = sum(SENSOR_DIMS.values()) - SENSOR_DIMS[ACTION] print "state_space", state_space # Initialized to start position and inital velocities are 0 x0 = np.zeros(state_space) x0[:DOFs] = init_q[i] # Initialize target end effector position ee_tgt = np.ndarray.flatten( get_ee_points(EE_POINTS, ee_pos_tgt, ee_rot_tgt).T).reshape((1, -1)) reset_condition = { JOINT_ANGLES: init_q[i], } x0s.append(x0) ee_tgts.append(ee_tgt) reset_conditions.append(reset_condition) if not os.path.exists(common['data_files_dir']): os.makedirs(common['data_files_dir']) agent = { 'type': AgentUR,