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 setup_pr2_init_pose(self): rospy.logout('Initializing the Robot..'+self.tool) self.head.set_pose([0.15,0.,0.]) self.torso.down() self.get_angles() duration=5. self.t_vals = eptc.min_jerk_traj(duration/self.time_step) self.r_ep =np.array([-1.397, 0.375, -1.740, -2.122, -1.966, -1.680, -2.491]) self.l_ep =np.array([1.397, 0.375, 1.740, -2.122, 1.966, -1.680, -3.926]) # self.r_ep =np.array([-1.397, 0.375, -1.740, -2.122, -1.966, -1.680, .651]) # self.l_ep =np.array([1.397, 0.375, 1.740, -2.122, 1.966, -1.680, -.784]) self.cs.carefree_switch('r', '%s_arm_controller') self.cs.carefree_switch('l', '%s_arm_controller') self.epc_move_arm(self.r_arm, self.r_angle, self.r_ep, duration) self.epc_move_arm(self.l_arm, self.l_angle, self.l_ep, duration) self.pr2_init = True
def epc_move_arm(self, arm, ep1, ep2, duration=10.): self.t_vals = eptc.min_jerk_traj(duration / self.time_step) traj = arm.interpolate_ep(ep1, ep2, self.t_vals) tc = eptc.EPTrajectoryControl(arm, traj) self.epc.epc_motion(tc, self.time_step)
def epc_move_arm(self, arm, ep1, ep2, duration=10.): self.t_vals = eptc.min_jerk_traj(duration/self.time_step) traj = arm.interpolate_ep(ep1, ep2, self.t_vals) tc = eptc.EPTrajectoryControl(arm, traj) self.epc.epc_motion(tc, self.time_step)