def pub_head_registration(ud): cheek_pose_base_link = self.tf_list.transformPose( "/base_link", ud.cheek_pose) # find the center of the ellipse given a cheek click cheek_transformation = np.mat( tf_trans.euler_matrix(2.6 * np.pi / 6, 0, 0, 'szyx')) cheek_transformation[0:3, 3] = np.mat([-0.08, -0.04, 0]).T cheek_pose = PoseConverter.to_homo_mat(cheek_pose_base_link) #b_B_c[0:3,0:3] = np.eye(3) norm_xy = cheek_pose[0:2, 2] / np.linalg.norm(cheek_pose[0:2, 2]) head_rot = np.arctan2(norm_xy[1], norm_xy[0]) cheek_pose[0:3, 0:3] = tf_trans.euler_matrix(0, 0, head_rot, 'sxyz')[0:3, 0:3] self.cheek_pub.publish( PoseConverter.to_pose_stamped_msg("/base_link", cheek_pose)) ell_center = cheek_pose * cheek_transformation self.ell_center_pub.publish( PoseConverter.to_pose_stamped_msg("/base_link", ell_center)) # create an ellipsoid msg and command it ep = EllipsoidParams() ep.e_frame.transform = PoseConverter.to_tf_msg(ell_center) ep.height = 0.924 ep.E = 0.086 self.ep_pub.publish(ep) return 'succeeded'
def generate_trajectory(ud): b_B_s = PoseConverter.to_homo_mat(ud.start_click) b_B_e = PoseConverter.to_homo_mat(ud.end_click) s_B_e = (b_B_s ** -1) * b_B_e b_normal = b_B_s[:3, 2] / np.linalg.norm(b_B_s[:3, 2]) s_vel = np.mat([s_B_e[0, 3], s_B_e[1, 3], 0]).T s_vel = s_vel / np.linalg.norm(s_vel) b_vel = b_B_s[:3, :3].T * s_vel b_ortho = np.mat(np.cross(b_normal.T, b_vel.T)).T b_ortho = b_ortho / np.linalg.norm(b_ortho) b_R_traj = np.vstack([b_vel.T, b_ortho.T, b_normal.T]) b_p_start = b_B_s[:3, 3] b_p_end = b_B_e[:3, 3] b_p_end = 3 #TODO TODO self.start_frame_pub.publish(PoseConverter.to_pose_stamped_msg(ud.start_click.header.frame_id, (b_p_start, b_R_traj))) self.end_frame_pub.publish(PoseConverter.to_pose_stamped_msg(ud.start_click.header.frame_id, (b_p_end, b_R_traj))) ud.start_traj_frame = (b_p_start, b_R_traj) ud.end_traj_frame = (b_p_end, b_R_traj) return 'succeeded'
def generate_trajectory(ud): b_B_s = PoseConverter.to_homo_mat(ud.start_click) b_B_e = PoseConverter.to_homo_mat(ud.end_click) s_B_e = (b_B_s**-1) * b_B_e b_normal = b_B_s[:3, 2] / np.linalg.norm(b_B_s[:3, 2]) s_vel = np.mat([s_B_e[0, 3], s_B_e[1, 3], 0]).T s_vel = s_vel / np.linalg.norm(s_vel) b_vel = b_B_s[:3, :3].T * s_vel b_ortho = np.mat(np.cross(b_normal.T, b_vel.T)).T b_ortho = b_ortho / np.linalg.norm(b_ortho) b_R_traj = np.vstack([b_vel.T, b_ortho.T, b_normal.T]) b_p_start = b_B_s[:3, 3] b_p_end = b_B_e[:3, 3] b_p_end = 3 #TODO TODO self.start_frame_pub.publish( PoseConverter.to_pose_stamped_msg( ud.start_click.header.frame_id, (b_p_start, b_R_traj))) self.end_frame_pub.publish( PoseConverter.to_pose_stamped_msg( ud.start_click.header.frame_id, (b_p_end, b_R_traj))) ud.start_traj_frame = (b_p_start, b_R_traj) ud.end_traj_frame = (b_p_end, b_R_traj) return 'succeeded'
def execute_trajectory(self, ell_f, gripper_rot, duration=5.): ell_f[1] = np.mod(ell_f[1], 2 * np.pi) num_samps = int(duration / self.time_step) t_vals = min_jerk_traj(num_samps) self.arm.reset_ep() self.reset_ell_ep() ell_init = np.mat(self.ell_ep).T ell_final = np.mat(ell_f).T if np.fabs(2 * np.pi + ell_final[1, 0] - ell_init[1, 0]) < np.pi: ell_final[1, 0] += 2 * np.pi print "wrapping; ell_f:", ell_f elif np.fabs(-2 * np.pi + ell_final[1, 0] - ell_init[1, 0]) < np.pi: ell_final[1, 0] -= 2 * np.pi print "wrapping; ell_f:", ell_f print "init", ell_init, "final", ell_final ell_traj = np.array(ell_init) + np.array( np.tile(ell_final - ell_init, (1, num_samps))) * np.array(t_vals) # find the current ellipsoid frame cur_time = rospy.Time.now() self.tf_list.waitForTransform("/torso_lift_link", "/ellipse_frame", cur_time, rospy.Duration(3)) ell_frame_mat = PoseConverter.to_homo_mat( self.tf_list.lookupTransform("/torso_lift_link", "/ellipse_frame", cur_time)) print ell_frame_mat print ell_traj.shape ell_pose_traj = [ self.robot_ellipsoidal_pose(ell_traj[0, i], ell_traj[1, i], ell_traj[2, i], gripper_rot, ell_frame_mat) for i in range(ell_traj.shape[1]) ] # replace the rotation matricies with a simple cartesian slerp cart_interp_traj = self.arm.interpolate_ep(self.arm.get_ep(), ell_pose_traj[-1], t_vals) fixed_traj = [(ell_pose_traj[i][0], cart_interp_traj[i][1]) for i in range(num_samps)] self.start_pub.publish( PoseConverter.to_pose_stamped_msg("/torso_lift_link", cart_interp_traj[0])) self.end_pub.publish( PoseConverter.to_pose_stamped_msg("/torso_lift_link", cart_interp_traj[-1])) #ep_traj_control = EPTrajectoryControl(self.arm, cart_interp_traj) print_trajectory_stats(ell_traj, fixed_traj, t_vals) ep_traj_control = EPTrajectoryControl(self.arm, fixed_traj) self.action_preempted = False self.ell_traj_behavior.stop_epc = False monitor_timer = rospy.Timer(rospy.Duration(0.1), self._check_preempt) self.ell_traj_behavior.epc_motion(ep_traj_control, self.time_step) monitor_timer.shutdown() if self.action_preempted: self.reset_ell_ep() else: self.ell_ep = ell_f
def execute_trajectory(self, ell_f, gripper_rot, duration=5.): ell_f[1] = np.mod(ell_f[1], 2 * np.pi) num_samps = int(duration / self.time_step) t_vals = min_jerk_traj(num_samps) self.arm.reset_ep() self.reset_ell_ep() ell_init = np.mat(self.ell_ep).T ell_final = np.mat(ell_f).T if np.fabs(2 * np.pi + ell_final[1,0] - ell_init[1,0]) < np.pi: ell_final[1,0] += 2 * np.pi print "wrapping; ell_f:", ell_f elif np.fabs(-2 * np.pi + ell_final[1,0] - ell_init[1,0]) < np.pi: ell_final[1,0] -= 2 * np.pi print "wrapping; ell_f:", ell_f print "init", ell_init, "final", ell_final ell_traj = np.array(ell_init) + np.array(np.tile(ell_final - ell_init, (1, num_samps))) * np.array(t_vals) # find the current ellipsoid frame cur_time = rospy.Time.now() self.tf_list.waitForTransform("/torso_lift_link", "/ellipse_frame", cur_time, rospy.Duration(3)) ell_frame_mat = PoseConverter.to_homo_mat( self.tf_list.lookupTransform("/torso_lift_link", "/ellipse_frame", cur_time)) print ell_frame_mat print ell_traj.shape ell_pose_traj = [self.robot_ellipsoidal_pose(ell_traj[0,i], ell_traj[1,i], ell_traj[2,i], gripper_rot, ell_frame_mat) for i in range(ell_traj.shape[1])] # replace the rotation matricies with a simple cartesian slerp cart_interp_traj = self.arm.interpolate_ep(self.arm.get_ep(), ell_pose_traj[-1], t_vals) fixed_traj = [(ell_pose_traj[i][0], cart_interp_traj[i][1]) for i in range(num_samps)] self.start_pub.publish(PoseConverter.to_pose_stamped_msg("/torso_lift_link", cart_interp_traj[0])) self.end_pub.publish(PoseConverter.to_pose_stamped_msg("/torso_lift_link", cart_interp_traj[-1])) #ep_traj_control = EPTrajectoryControl(self.arm, cart_interp_traj) print_trajectory_stats(ell_traj, fixed_traj, t_vals) ep_traj_control = EPTrajectoryControl(self.arm, fixed_traj) self.action_preempted = False self.ell_traj_behavior.stop_epc = False monitor_timer = rospy.Timer(rospy.Duration(0.1), self._check_preempt) self.ell_traj_behavior.epc_motion(ep_traj_control, self.time_step) monitor_timer.shutdown() if self.action_preempted: self.reset_ell_ep() else: self.ell_ep = ell_f
def pose_saver(tf_frame, bag_file, pickle_file): arm = create_pr2_arm('l', PR2ArmJTransposeTask) tf_list = tf.TransformListener() named_poses = NamedPoses() named_poses.pose_arr.header.frame_id = tf_frame pose_dict = {} while True: if rospy.is_shutdown(): return pose_name = raw_input("Type the name of the pose and hit enter to save it (Type 'q' to quit) ") if pose_name == 'q': break arm_pose = PoseConverter.to_pose_stamped_msg("/torso_lift_link", arm.get_end_effector_pose()) tf_pose = tf_list.transformPose(tf_frame, arm_pose) named_poses.pose_arr.poses.append(tf_pose) named_poses.names.append(pose_name) pose_dict[pose_name] = tf_pose if bag_file != "": bag = rosbag.Bag(bag_file, 'w') bag.write("/named_poses", named_poses) bag.close() if pickle_file != "": f = file(pickle_file, 'w') pickle.dump(pose_dict, f) f.close()
def pose_saver(tf_frame, bag_file, pickle_file): arm = create_pr2_arm('l', PR2ArmJTransposeTask) tf_list = tf.TransformListener() named_poses = NamedPoses() named_poses.pose_arr.header.frame_id = tf_frame pose_dict = {} while True: if rospy.is_shutdown(): return pose_name = raw_input( "Type the name of the pose and hit enter to save it (Type 'q' to quit) " ) if pose_name == 'q': break arm_pose = PoseConverter.to_pose_stamped_msg( "/torso_lift_link", arm.get_end_effector_pose()) tf_pose = tf_list.transformPose(tf_frame, arm_pose) named_poses.pose_arr.poses.append(tf_pose) named_poses.names.append(pose_name) pose_dict[pose_name] = tf_pose if bag_file != "": bag = rosbag.Bag(bag_file, 'w') bag.write("/named_poses", named_poses) bag.close() if pickle_file != "": f = file(pickle_file, 'w') pickle.dump(pose_dict, f) f.close()
def reset_ell_ep(self): ee_pose = PoseConverter.to_pose_stamped_msg("/torso_lift_link", self.arm.get_end_effector_pose()) cur_time = rospy.Time.now() ee_pose.header.stamp = cur_time self.tf_list.waitForTransform("/torso_lift_link", "/ellipse_frame", cur_time, rospy.Duration(3)) ell_pose = self.tf_list.transformPose("/ellipse_frame", ee_pose) pos, quat = PoseConverter.to_pos_quat(ell_pose) self.ell_ep = list(self.ell_space.pos_to_ellipsoidal(*pos))
def ar_sub(self, msg): cur_ar_ps = PoseConverter.to_pose_stamped_msg(msg.header.frame_id, msg.pose.pose) cur_ar_ps.header.stamp = msg.header.stamp try: cur_ar_in_base = self.tf_list.transformPose("/base_link", cur_ar_ps) except tf.Exception as e: return self.cur_ar_pose = PoseConverter.to_homo_mat(cur_ar_in_base) self.ar_pose_updated = True
def reset_ell_ep(self): ee_pose = PoseConverter.to_pose_stamped_msg( "/torso_lift_link", self.arm.get_end_effector_pose()) cur_time = rospy.Time.now() ee_pose.header.stamp = cur_time self.tf_list.waitForTransform("/torso_lift_link", "/ellipse_frame", cur_time, rospy.Duration(3)) ell_pose = self.tf_list.transformPose("/ellipse_frame", ee_pose) pos, quat = PoseConverter.to_pos_quat(ell_pose) self.ell_ep = list(self.ell_space.pos_to_ellipsoidal(*pos))
def ar_sub(self, msg): cur_ar_ps = PoseConverter.to_pose_stamped_msg(msg.header.frame_id, msg.pose.pose) cur_ar_ps.header.stamp = msg.header.stamp try: cur_ar_in_base = self.tf_list.transformPose( "/base_link", cur_ar_ps) except tf.Exception as e: return self.cur_ar_pose = PoseConverter.to_homo_mat(cur_ar_in_base) self.ar_pose_updated = True
def get_head_pose_srv(self, req): if req.name not in head_poses: pose = (np.mat([-9999, -9999, -9999]).T, np.mat(np.zeros((3, 3)))) else: pose = self.get_head_pose(req.name, req.gripper_rot) if USE_ELLIPSE_FRAME: frame = "/ellipse_frame" else: frame = "/base_link" pose_stamped = PoseConverter.to_pose_stamped_msg(frame, pose) # self.tmp_pub.publish(pose_stamped) return pose_stamped
def get_head_pose_srv(self, req): if req.name not in head_poses: pose = (np.mat([-9999, -9999, -9999]).T, np.mat(np.zeros((3, 3)))) else: pose = self.get_head_pose(req.name, req.gripper_rot) if USE_ELLIPSE_FRAME: frame = "/ellipse_frame" else: frame = "/base_link" pose_stamped = PoseConverter.to_pose_stamped_msg(frame, pose) #self.tmp_pub.publish(pose_stamped) return pose_stamped
def pub_head_registration(ud): cheek_pose_base_link = self.tf_list.transformPose("/base_link", ud.cheek_pose) # find the center of the ellipse given a cheek click cheek_transformation = np.mat(tf_trans.euler_matrix(2.6 * np.pi/6, 0, 0, 'szyx')) cheek_transformation[0:3, 3] = np.mat([-0.08, -0.04, 0]).T cheek_pose = PoseConverter.to_homo_mat(cheek_pose_base_link) #b_B_c[0:3,0:3] = np.eye(3) norm_xy = cheek_pose[0:2, 2] / np.linalg.norm(cheek_pose[0:2, 2]) head_rot = np.arctan2(norm_xy[1], norm_xy[0]) cheek_pose[0:3,0:3] = tf_trans.euler_matrix(0, 0, head_rot, 'sxyz')[0:3,0:3] self.cheek_pub.publish(PoseConverter.to_pose_stamped_msg("/base_link", cheek_pose)) ell_center = cheek_pose * cheek_transformation self.ell_center_pub.publish(PoseConverter.to_pose_stamped_msg("/base_link", ell_center)) # create an ellipsoid msg and command it ep = EllipsoidParams() ep.e_frame.transform = PoseConverter.to_tf_msg(ell_center) ep.height = 0.924 ep.E = 0.086 self.ep_pub.publish(ep) return 'succeeded'
def publish_vector(self, m_id): new_m = copy.deepcopy(self.m) new_m.id = m_id u, v, p = 1.0, np.random.uniform(0, np.pi), np.random.uniform(0, 2 * np.pi) pos = self.sspace.spheroidal_to_cart((u, v, p)) new_m.points.append(Point(*pos)) df_du = self.sspace.partial_u((u, v, p)) df_du *= 0.1 / np.linalg.norm(df_du) new_m.points.append(Point(*(pos+df_du))) self.ma.markers.append(new_m) self.vis_pub.publish(self.ma) rot_gripper = np.pi/4. pos, rot = self.sspace.spheroidal_to_pose((u,v,p), rot_gripper) ps_msg = PoseConverter.to_pose_stamped_msg("torso_lift_link", pos, rot) self.pose_pub.publish(ps_msg)
def publish_vector(self, m_id): new_m = copy.deepcopy(self.m) new_m.id = m_id u, v, p = 1.0, np.random.uniform(0, np.pi), np.random.uniform( 0, 2 * np.pi) pos = self.sspace.spheroidal_to_cart((u, v, p)) new_m.points.append(Point(*pos)) df_du = self.sspace.partial_u((u, v, p)) df_du *= 0.1 / np.linalg.norm(df_du) new_m.points.append(Point(*(pos + df_du))) self.ma.markers.append(new_m) self.vis_pub.publish(self.ma) rot_gripper = np.pi / 4. pos, rot = self.sspace.spheroidal_to_pose((u, v, p), rot_gripper) ps_msg = PoseConverter.to_pose_stamped_msg("torso_lift_link", pos, rot) self.pose_pub.publish(ps_msg)
def set_ep(self, cep, duration, delay=0.0): cep_pose_stmp = PoseConverter.to_pose_stamped_msg( 'torso_lift_link', cep) cep_pose_stmp.header.stamp = rospy.Time.now() self.command_pose_pub.publish(cep_pose_stmp)
def set_ep(self, cep, duration, delay=0.0): cep_pose_stmp = PoseConverter.to_pose_stamped_msg('torso_lift_link', cep) cep_pose_stmp.header.stamp = rospy.Time.now() self.command_pose_pub.publish(cep_pose_stmp)