def TrajectoryGenerator(Tse_init, Tsc_init, Tsc_final, Tce_grasp, Tce_stand, k): """ k: The number of trajectory reference configurations per 0.01 seconds Ex: k=10, t=0.01; freq_controller = k/t = 1000Hz Tse_init: The initial configuration of the end-effector for the reference trajectory Tsc_init: The cube's initial configuration Tsc_final: The cube's desired final configuration Tce_grasp: The end-effector's configuration relative to the cube when it is grasping the cube Tce_stand: The end-effector's standoff configuration above the cube, before and after grasping, relative to the cube """ t = 2 N = t*k/0.01 # Move end-effector to cube Tse_is = np.dot(Tsc_init, Tce_stand) traj = mr.CartesianTrajectory(Tse_init, Tse_is, t, N, 5) for i in range(len(traj)): temp = shuffle(traj[i], 0) ref_traj.append(temp) # Drop end-effector to cube height Tse_ig = np.dot(Tsc_init, Tce_grasp) traj = mr.CartesianTrajectory(Tse_is, Tse_ig, t, N, 5) for i in range(len(traj)): temp = shuffle(traj[i], 0) ref_traj.append(temp) # Close gripper close_gripper() # Raise end-effector back up traj = mr.CartesianTrajectory(Tse_ig, Tse_is, t, N, 5) for i in range(len(traj)): temp = shuffle(traj[i], 1) ref_traj.append(temp) # Move to final position t = 4 N = t*k/0.01 Tse_fs = np.dot(Tsc_final, Tce_stand) traj = mr.CartesianTrajectory(Tse_is, Tse_fs, t, N, 5) for i in range(len(traj)): temp = shuffle(traj[i], 1) ref_traj.append(temp) # Drop end-effector to final cube height t = 2 N = t*k/0.01 Tse_fg = np.dot(Tsc_final, Tce_grasp) traj = mr.CartesianTrajectory(Tse_fs, Tse_fg, t, N, 5) for i in range(len(traj)): temp = shuffle(traj[i], 1) ref_traj.append(temp) # Open gripper open_gripper() print("Trajectory Generated")
def path_planning(self): """ Generate desriable waypoints for achieving target pose. - Example dt = 0.01 #Create a trajectory to follow thetaend = [pi / 2, pi, 1.5 * pi] Tf = 1 N = Tf / dt method = 5 dt = Tf / (N - 1.0) """ dt = self.sample_time current_pose = self.limb.endpoint_pose() X_current = self._get_tf_matrix(current_pose) X_goal = self.original_goal # X_goal[2][3] += 0.05 rospy.logwarn('=============== Current pose ===============') print(X_current) rospy.logwarn('=============== Goal goal ===============') print(X_goal) Tf = self.plan_time # Tf = np.random.uniform(self.rand_plan_min, self.rand_plan_max) N = int(Tf / dt) # ex: plantime = 7, dt = 0.01 -> N = 700 self.num_wp = N self.traj_list = r.CartesianTrajectory(X_current, X_goal, Tf=Tf, N=N, method=CUBIC) self.set_init_frame( ) # save the robot's pose frame at start of the trajectory. self.isPathPlanned = True
def Generate_segment_trajectory(self, T_start, T_end, time_scale, traj_type): ''' Generate segment of trajectory of the end effector Input : T_start : Starting configuration T_end : Goal configuration time_scale : Time scaling of this segment traj_type : Screw or Cartesian type Return: trajs : Trajectory of the segment in [np.array([Tse..]), np.array([next_Tse])] ''' # Duration and number of configuration of the trajectory is computed based on the maximum linear and angular velocity T_dist = np.dot(mr.TransInv(T_start), T_end) dist = math.hypot(T_dist[0, -1], T_dist[1, -1]) ang = max(self.euler_angles_from_rotation_matrix(T_dist[:-1, :-1])) duration = max(dist / self.max_lin_vel, ang / self.max_ang_vel) # Duration from rest to rest no_conf = int( (duration * self.k) / 0.01) # Number of configuration between the start and goal if (traj_type == "Screw"): trajs = mr.ScrewTrajectory(T_start, T_end, duration, no_conf, time_scale) elif (traj_type == "Cartesian"): trajs = mr.CartesianTrajectory(T_start, T_end, duration, no_conf, time_scale) return trajs
def move_to_pose(self, pose, Tf, N, inter_type, frame='end'): while self.joint_position_received == False: rospy.loginfo('Waiting for joint angles...') rospy.sleep(1) # Cartisian or joint interplation if inter_type == 'c': x_start = self.forward_kinematics(self.joint_position, frame) print('xstart', x_start) x_end = pose print('xend', x_end) x_traj = mr.CartesianTrajectory(x_start, x_end, Tf, N, 5) #print(x_traj) traj = [] for i in range(len(x_traj)): if i == 0: theta0 = self.joint_position else: theta0 = traj[i - 1] #print(x_traj[i]) #print(theta0) theta, _ = self.inverse_kinematics(x_traj[i], theta0, frame) #print(theta) traj.append(theta) elif inter_type == 'j': theta_d = self.inverse_kinematics(pose, self.joint_position, frame) traj = mr.JointTrajectory(self.joint_position, theta_d, Tf, N, 5) dt = Tf / (N - 1.0) self.move_joint_traj(traj, dt)
def createSegment1(T_se_initial, T_sc_initial, T_ce_standoff, k): # Need to compute the desired orientation R T_se_standoff = np.matmul(T_sc_initial.copy(), T_ce_standoff.copy()) totalSeconds = 100 N = float(totalSeconds) / float(k) X_start = T_se_initial.copy() X_end = T_se_standoff.copy() X_end = X_end.astype(float) # The total time in seconds Tf = totalSeconds # Method describes cubic or quintic scaling method = 5 path_states = mr.CartesianTrajectory(X_start, X_end, Tf, N, method) # Take the 3-D array and put it into a 2-D form path_states = process_array(path_states, 0) return path_states
def TrajectoryGenerator(Tse_i, Tsc_i, Tsc_f, Tce_g, Tce_s,k): #inital to standoff position Tse_s = np.dot(Tsc_i, Tce_s) N = 2*k/0.01 traj = mr.CartesianTrajectory(Tse_i, Tse_s, 2, N, 5) for i in range(int(N)): final.append([traj[i][0][0], traj[i][0][1], traj[i][0][2], traj[i][1][0], traj[i][1][1], traj[i][1][2], traj[i][2][0], traj[i][2][1], traj[i][2][2], traj[i][0][3], traj[i][1][3], traj[i][2][3], 0]) #standoff to grasp Tse_g = np.dot(Tsc_i, Tce_g) N = 2*k/0.01 traj = mr.CartesianTrajectory(Tse_s, Tse_g, 2, N, 5) for i in range(int(N)): final.append([traj[i][0][0], traj[i][0][1], traj[i][0][2], traj[i][1][0], traj[i][1][1], traj[i][1][2], traj[i][2][0], traj[i][2][1], traj[i][2][2], traj[i][0][3], traj[i][1][3], traj[i][2][3], 0]) #closing the grasp List = final[-1] closed = [] for i in range(len(List)-1): closed.append(List[i]) closed.append(1) for i in range(100): final.append(closed) #grasp to standoff N = 2*k/0.01 traj = mr.CartesianTrajectory(Tse_g, Tse_s, 2, N, 5) for i in range(int(N)): final.append([traj[i][0][0], traj[i][0][1], traj[i][0][2], traj[i][1][0], traj[i][1][1], traj[i][1][2], traj[i][2][0], traj[i][2][1], traj[i][2][2], traj[i][0][3], traj[i][1][3], traj[i][2][3], 1]) #standoff to final stand off N = 5*k/0.01 Tse_fs = np.dot(Tsc_f, Tce_s) traj = mr.CartesianTrajectory(Tse_s, Tse_fs, 3, N, 5) for i in range(int(N)): final.append([traj[i][0][0], traj[i][0][1], traj[i][0][2], traj[i][1][0], traj[i][1][1], traj[i][1][2], traj[i][2][0], traj[i][2][1], traj[i][2][2], traj[i][0][3], traj[i][1][3], traj[i][2][3], 1]) #final standoff to final grasp N = 2*k/0.01 Tse_fg = np.dot(Tsc_f, Tce_g) traj = mr.CartesianTrajectory(Tse_fs, Tse_fg, 2, N, 5) for i in range(int(N)): final.append([traj[i][0][0], traj[i][0][1], traj[i][0][2], traj[i][1][0], traj[i][1][1], traj[i][1][2], traj[i][2][0], traj[i][2][1], traj[i][2][2], traj[i][0][3], traj[i][1][3], traj[i][2][3], 1]) #opening the grasp List = final[-1] opened = [] for i in range(len(List)-1): opened.append(List[i]) opened.append(0) for i in range(100): final.append(opened) #final grasp to final standoff N = 2*k/0.01 traj = mr.CartesianTrajectory(Tse_fg, Tse_fs, 2, N, 5) for i in range(int(N)): final.append([traj[i][0][0], traj[i][0][1], traj[i][0][2], traj[i][1][0], traj[i][1][1], traj[i][1][2], traj[i][2][0], traj[i][2][1], traj[i][2][2], traj[i][0][3], traj[i][1][3], traj[i][2][3], 0])
def servo_vel(self, pose, time=4.0, steps=400, num_wp=5): """Servo the robot to the goal TODO: merge check_path_stop method """ current_pose = self._limb.endpoint_pose() X_current = self._get_tf_matrix(current_pose) X_goal = self._get_tf_matrix(pose) traj = mr.CartesianTrajectory(X_current, X_goal, Tf=5, N=num_wp, method=3) for idx, X in enumerate(traj): _pose = self._get_msg(X) self.ikVelPub.publish(_pose) self._check_traj(_pose, idx)
def TrajectoryGenerator(Tse_i,Tsc_i,Tsc_f,Tce_g,Tce_s,k): """ Tse_i: The initial configuration of the end-effector in the reference trajectory == Tse_initial. Tsc_i: The cube's initial configuration == Tsc_initial Tsc_f: The cube's desired final configuration == Tsc_final Tce_g: The end-effector's configuration relative to the cube when it is grasping the cube == Tce_grasp Tce_s: The end-effector's standoff configuration above the cube, before and after grasping, relative to the cube == Tce_standoff k: The number of trajectory reference configurations per 0.01 seconds """ Tse_si = np.dot(Tsc_i,Tce_s) # The end-effector's standoff configuration above the initial cube relative to the world frame N = 2*k/0.01 traj1 = mr.CartesianTrajectory(Tse_i,Tse_si,2,N,5) # Move from initial position to the initial standoff position for i in range(int(N)): param.append([traj1[i][0][0],traj1[i][0][1],traj1[i][0][2],traj1[i][1][0],traj1[i][1][1],traj1[i][1][2],\ traj1[i][2][0],traj1[i][2][1],traj1[i][2][2],traj1[i][0][3],traj1[i][1][3],traj1[i][2][3],0])
def Trajectory(Ti,Tf,iterations): thetalist0 = BestTheta0(Ti[0][3],Ti[1][3],Ti[2][3],206,189) AngleList = [] VelocityList = [] traj = mr.CartesianTrajectory(Ti,Tf,10,iterations,3) for i in range(iterations): if i == 0: Ik = mr.IKinSpace(Robot.Slist.T,Robot.M,traj[i],thetalist0, eomg = 0.00001, ev = 0.000001) Vel = JointsVelocity(Robot.Slist,Ik,Robot.Vs) VelocityList.append(Vel) AngleList.append(Ik[0]) else: Ik = mr.IKinSpace(Robot.Slist.T,Robot.M,traj[i],AngleList[i-1], eomg = 0.000001, ev = 0.000001) Vel = JointsVelocity(Robot.Slist,Ik,Robot.Vs) VelocityList.append(Vel) AngleList.append(Ik[0]) return AngleList
def p7(): print('P7') X_start = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) X_end = np.array([[0, 0, 1, 1], [1, 0, 0, 2], [0, 1, 0, 3], [0, 0, 0, 1]]) Tf = 10 N = 10 method = 5 traj = mr.CartesianTrajectory(X_start, X_end, Tf, N, method) ans = traj[-2] print('second to last via configuration:\n%s' %ans)
def move_to_pose(self, pose, jaw_angle, Tf, N, inter_type, frame='jaw'): while self.joint_angle_received == False: self.loginfo_wait_joint_angle() rospy.sleep(1) traj = [] # self.set_last_tar_joint_angles2cur() # Cartisian or joint interplation if inter_type == 'c': x_start = self.get_current_pos() if self.last_desired_pose is not None: x_start = self.last_desired_pose print('xstart', x_start) x_end = pose print('xend', x_end) x_traj = mr.CartesianTrajectory(x_start, x_end, Tf, N, 5) jaw_traj = mr.JointTrajectory([self.jaw_angle], [jaw_angle], Tf, N, 5) #print(x_traj) for i in range(len(x_traj)): if i == 0: theta0 = self.joint_angle[:-1] else: theta0 = traj[i - 1] #print(x_traj[i]) #print(theta0) theta_d = self.inverse_kinematics_rcm(x_traj[i]) motor_angle_d = self.joint_angle2motor_angle( theta_d, jaw_traj[i]) #print(theta) traj.append(motor_angle_d) elif inter_type == 'j': theta_d = self.inverse_kinematics_rcm(pose) print("theta_d: {}".format(theta_d)) motor_angle_d = self.joint_angle2motor_angle(theta_d, jaw_angle) print("motor_d: {}".format(motor_angle_d)) print("tar motor angle: {}".format(motor_angle_d)) traj = mr.JointTrajectory(self.motor_angle, motor_angle_d, Tf, N, 5) dt = Tf / (N - 1.0) self.move_joint_traj(traj, dt) self.last_desired_pose = pose
def createSegment8(current_state, goal_state, k): totalSeconds = 100 N = float(totalSeconds) / float(k) X_start = current_state.copy() X_end = goal_state.copy() # The total time in seconds Tf = totalSeconds # Method describes cubic or quintic scaling method = 5 path_states = mr.CartesianTrajectory(X_start, X_end, Tf, N, method) # Take the 3-D array and put it into a 2-D form path_states = process_array(path_states, 0) return path_states
def createSegment2(T_se_initial, T_sc_initial, T_ce_grasp, k): totalSeconds = 100 N = float(totalSeconds) / float(k) T_goal = np.matmul(T_sc_initial.copy(), T_ce_grasp.copy()) X_start = T_se_initial.copy() X_end = T_goal.copy() Tf = totalSeconds # Method describes cubic or quintic scaling method = 5 path_states = mr.CartesianTrajectory(X_start, X_end, Tf, N, method) # Take the 3-D array and put it into a 2-D form path_states = process_array(path_states, 0) return path_states
def createSegment6(current_state, T_sc_final, T_ce_grasp, k): goal_state = np.matmul(T_sc_final, T_ce_grasp) totalSeconds = 200 N = float(totalSeconds) / float(k) X_start = current_state.copy() X_end = goal_state.copy() # The total time in seconds Tf = totalSeconds # Method describes cubic or quintic scaling method = 5 path_states = mr.CartesianTrajectory(X_start, X_end, Tf, N, method) # Take the 3-D array and put it into a 2-D form path_states = process_array(path_states, 1) return path_states
import modern_robotics as mr import numpy as np import math print mr.QuinticTimeScaling(5, 3) Xstart = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) Xend = np.array([[0, 0, 1, 1], [1, 0, 0, 2], [0, 1, 0, 3], [0, 0, 0, 1]]) # tragectoryPoints = mr.ScrewTrajectory(Xstart, Xend, 10, 10, 3) tragectoryPoints = mr.CartesianTrajectory(Xstart, Xend, 10, 10, 5) with np.printoptions(precision=3, suppress=True): print tragectoryPoints[8]
Tsc_i: The cube's initial configuration == Tsc_initial Tsc_f: The cube's desired final configuration == Tsc_final Tce_g: The end-effector's configuration relative to the cube when it is grasping the cube == Tce_grasp Tce_s: The end-effector's standoff configuration above the cube, before and after grasping, relative to the cube == Tce_standoff k: The number of trajectory reference configurations per 0.01 seconds """ Tse_si = np.dot(Tsc_i,Tce_s) # The end-effector's standoff configuration above the initial cube relative to the world frame N = 2*k/0.01 traj1 = mr.CartesianTrajectory(Tse_i,Tse_si,2,N,5) # Move from initial position to the initial standoff position for i in range(int(N)): param.append([traj1[i][0][0],traj1[i][0][1],traj1[i][0][2],traj1[i][1][0],traj1[i][1][1],traj1[i][1][2],\ traj1[i][2][0],traj1[i][2][1],traj1[i][2][2],traj1[i][0][3],traj1[i][1][3],traj1[i][2][3],0]) Tse_gi = np.dot(Tsc_i,Tce_g) # The end-effector's configuration relative to the world when it is grasping the initial cube traj2 = mr.CartesianTrajectory(Tse_si,Tse_gi,2,N,5) # Move from initial standoff position to grasp position for i in range(int(N)): param.append([traj2[i][0][0],traj2[i][0][1],traj2[i][0][2],traj2[i][1][0],traj2[i][1][1],traj2[i][1][2],\ traj2[i][2][0],traj2[i][2][1],traj2[i][2][2],traj2[i][0][3],traj2[i][1][3],traj2[i][2][3],0]) temp = param[-1] temp[-1] = 1 for i in range(100): param.append(temp) traj3 = mr.CartesianTrajectory(Tse_gi,Tse_si,2,N,5) # Move from grasp position back to initial standoff position for i in range(int(N)): param.append([traj3[i][0][0],traj3[i][0][1],traj3[i][0][2],traj3[i][1][0],traj3[i][1][1],traj3[i][1][2],\ traj3[i][2][0],traj3[i][2][1],traj3[i][2][2],traj3[i][0][3],traj3[i][1][3],traj3[i][2][3],1]) Tse_sf = np.dot(Tsc_f,Tce_s) # The end-effector's standoff configuration above the final cube relative to the world frame N = 5*k/0.01
def TrajectoryGenerator(E, C, F, G, S): Xstart1 = np.copy(E) # Trajectory 1 Starting Point :- Tseinitial Xend2 = np.dot( C, G ) # Trajectory 2 Ending Point :- Tsg (Endeffector Poistion during Grasping) Xend1 = np.dot( C, S ) # Trajectory 1 Ending Point :- Tss (Endeffector Poistion during Standoff) Xend6 = np.dot(F, G) # Tsg at Cube Goal (grasp) Xend5 = np.dot(F, S) # Tss at Cube Goal(Standoff) N2 = 200 t2 = N2 / 100 # Time and Number of Iteration for each Trajectory N5 = 1600 t5 = N5 / 100 trac1 = mr.CartesianTrajectory(Xstart1, Xend1, 6, 600, 5) trac2 = mr.CartesianTrajectory(Xend1, Xend2, t2, N2, 5) trac4 = mr.CartesianTrajectory(Xend2, Xend1, t2, N2, 5) trac5 = mr.CartesianTrajectory(Xend1, Xend5, t5, N5, 5) # Finding Trajectory using MR Function trac6 = mr.CartesianTrajectory(Xend5, Xend6, t2, N2, 5) trac8 = mr.CartesianTrajectory(Xend6, Xend5, t2, N2, 5) matrix1 = np.zeros((600, 13)) matrix2 = np.zeros((N2, 13)) matrix3 = np.zeros((65, 13)) matrix4 = np.zeros((N2, 13)) matrix5 = np.zeros((N5, 13)) matrix6 = np.zeros((N2, 13)) matrix7 = np.zeros((65, 13)) matrix8 = np.zeros((N2, 13)) for i in range(600): matrix1[i][0] = trac1[i][0][0] matrix1[i][1] = trac1[i][0][1] matrix1[i][2] = trac1[i][0][2] matrix1[i][3] = trac1[i][1][0] matrix1[i][4] = trac1[i][1][1] matrix1[i][5] = trac1[i][1][2] matrix1[i][6] = trac1[i][2][0] matrix1[i][7] = trac1[i][2][1] matrix1[i][8] = trac1[i][2][2] matrix1[i][9] = trac1[i][0][3] matrix1[i][10] = trac1[i][1][3] matrix1[i][11] = trac1[i][2][3] matrix1[i][12] = 0 for i in range(N2): matrix2[i][0] = trac2[i][0][0] matrix2[i][1] = trac2[i][0][1] matrix2[i][2] = trac2[i][0][2] matrix2[i][3] = trac2[i][1][0] matrix2[i][4] = trac2[i][1][1] matrix2[i][5] = trac2[i][1][2] matrix2[i][6] = trac2[i][2][0] matrix2[i][7] = trac2[i][2][1] matrix2[i][8] = trac2[i][2][2] matrix2[i][9] = trac2[i][0][3] matrix2[i][10] = trac2[i][1][3] matrix2[i][11] = trac2[i][2][3] matrix2[i][12] = 0 for i in range(65): matrix3[i, 0:12] = matrix2[N2 - 1, 0:12] matrix3[i, 12] = 1 for i in range(N2): matrix4[i][0] = trac4[i][0][0] matrix4[i][1] = trac4[i][0][1] matrix4[i][2] = trac4[i][0][2] matrix4[i][3] = trac4[i][1][0] matrix4[i][4] = trac4[i][1][1] matrix4[i][5] = trac4[i][1][2] matrix4[i][6] = trac4[i][2][0] matrix4[i][7] = trac4[i][2][1] matrix4[i][8] = trac4[i][2][2] matrix4[i][9] = trac4[i][0][3] matrix4[i][10] = trac4[i][1][3] matrix4[i][11] = trac4[i][2][3] matrix4[i][12] = 1 for i in range(N5): matrix5[i][0] = trac5[i][0][0] matrix5[i][1] = trac5[i][0][1] matrix5[i][2] = trac5[i][0][2] matrix5[i][3] = trac5[i][1][0] matrix5[i][4] = trac5[i][1][1] matrix5[i][5] = trac5[i][1][2] matrix5[i][6] = trac5[i][2][0] matrix5[i][7] = trac5[i][2][1] matrix5[i][8] = trac5[i][2][2] matrix5[i][9] = trac5[i][0][3] matrix5[i][10] = trac5[i][1][3] matrix5[i][11] = trac5[i][2][3] matrix5[i][12] = 1 for i in range(N2): matrix6[i][0] = trac6[i][0][0] matrix6[i][1] = trac6[i][0][1] matrix6[i][2] = trac6[i][0][2] matrix6[i][3] = trac6[i][1][0] matrix6[i][4] = trac6[i][1][1] matrix6[i][5] = trac6[i][1][2] matrix6[i][6] = trac6[i][2][0] matrix6[i][7] = trac6[i][2][1] matrix6[i][8] = trac6[i][2][2] matrix6[i][9] = trac6[i][0][3] matrix6[i][10] = trac6[i][1][3] matrix6[i][11] = trac6[i][2][3] matrix6[i][12] = 1 for i in range(65): matrix7[i, 0:12] = matrix6[N2 - 1, 0:12] matrix7[i, 12] = 0 for i in range(N2): matrix8[i][0] = trac8[i][0][0] matrix8[i][1] = trac8[i][0][1] matrix8[i][2] = trac8[i][0][2] matrix8[i][3] = trac8[i][1][0] matrix8[i][4] = trac8[i][1][1] matrix8[i][5] = trac8[i][1][2] matrix8[i][6] = trac8[i][2][0] matrix8[i][7] = trac8[i][2][1] matrix8[i][8] = trac8[i][2][2] matrix8[i][9] = trac8[i][0][3] matrix8[i][10] = trac8[i][1][3] matrix8[i][11] = trac8[i][2][3] matrix8[i][12] = 0 finalmatrix = np.vstack( (matrix1, matrix2, matrix3, matrix4, matrix5, matrix6, matrix7, matrix8)) # Storing all 12 configuration and gripper state in Matrix return finalmatrix
def trajectoryGenerator(T_se_initial, T_sc_initial, T_sc_final, T_ce_grasp, T_ce_standoff, k): # --------------------------------- # segment 1: A trajectory to move the gripper from its initial configuration to a "standoff" configuration a few cm above the block # represent the frame of end-effector when standoff in space frame T_se_standoff_initial = T_sc_initial.dot(T_ce_standoff) # generate trajectory when approaching the standoff position in segment 1 T_se_segment_1 = mr.CartesianTrajectory(T_se_initial, T_se_standoff_initial, 5, 5 / 0.01, 3) # --------------------------------- # segment 2: A trajectory to move the gripper down to the grasp position # represent the frame of end-effecto r in space frame in segment 2 T_se_grasp = T_sc_initial.dot(T_ce_grasp) # generate trajectory when approaching the grasping position in segment 2 T_se_seg2 = mr.CartesianTrajectory(T_se_segment_1[-1], T_se_grasp, 2, 2 / 0.01, 3) # append the trajectory of segment 2 after segment 1 T_se_before = np.append(T_se_segment_1, T_se_seg2, axis=0) # --------------------------------- # segment 3: Closing of the gripper # append the trajectory of segment 3 by 63 times for i in np.arange(64): T_se_before = np.append(T_se_before, np.array([T_se_before[-1]]), axis=0) # --------------------------------- # segment 4: A trajectory to move the gripper back up to the "standoff" configuration # generate trajectory when back on the standoff position in segment 4 T_se_segment_4 = mr.CartesianTrajectory(T_se_grasp, T_se_standoff_initial, 2, 2 / 0.01, 3) # append the trajectory of segment 4 T_se_before = np.append(T_se_before, T_se_segment_4, axis=0) # --------------------------------- # segment 5: A trajectory to move the gripper to a "standoff" configuration above the final configuration # generate trajectory when moving to the final standoff position in segment 5 T_se_standoff_final = T_sc_final.dot(T_ce_standoff) T_se_segment_5 = mr.CartesianTrajectory(T_se_standoff_initial, T_se_standoff_final, 8, 8 / 0.01, 3) # append the trajectory of segment 5 T_se_before = np.append(T_se_before, T_se_segment_5, axis=0) # --------------------------------- # segment 6: A trajectory to move the gripper to the final configuration of the object # generate the end-effector configuration when losing T_se_lose = T_sc_final.dot(T_ce_grasp) # generate trajectory when moving to the final cube position in segment 6 T_se_segment_6 = mr.CartesianTrajectory(T_se_standoff_final, T_se_lose, 2, 2 / 0.01, 3) # append the trajectory of segment 6 T_se_before = np.append(T_se_before, T_se_segment_6, axis=0) # --------------------------------- # segment 7: Opening of the gripper # append the trajectory of segment 7 by 63 times for i in np.arange(64): T_se_before = np.append(T_se_before, np.array([T_se_before[-1]]), axis=0) # --------------------------------- # segment 8: A trajectory to move the gripper back to the "standoff" configuration # generate trajectory when moving to the final standoff position in segment 8 T_se_segment_8 = mr.CartesianTrajectory(T_se_before[-1], T_se_standoff_final, 2, 2 / 0.01, 3) # append the trajectory of segment 8 T_se_before = np.append(T_se_before, T_se_segment_8, axis=0) # --------------------------------- # generate a matrix which is n by 13 T_se_post = np.zeros([int(k * 21 / 0.01 + 64 * 2), 13]) # put the configuration, position and gripper state in matrix which is n by 13 for i in np.arange(int(k * 21 / 0.01 + 64 * 2)): T_se_post[i, 0] = T_se_before[i, 0, 0] T_se_post[i, 1] = T_se_before[i, 0, 1] T_se_post[i, 2] = T_se_before[i, 0, 2] T_se_post[i, 3] = T_se_before[i, 1, 0] T_se_post[i, 4] = T_se_before[i, 1, 1] T_se_post[i, 5] = T_se_before[i, 1, 2] T_se_post[i, 6] = T_se_before[i, 2, 0] T_se_post[i, 7] = T_se_before[i, 2, 1] T_se_post[i, 8] = T_se_before[i, 2, 2] T_se_post[i, 9] = T_se_before[i, 0, 3] T_se_post[i, 10] = T_se_before[i, 1, 3] T_se_post[i, 11] = T_se_before[i, 2, 3] T_se_post[i, 12] = 0 # amend the gripper state in segment 3, 4, 5, 6 for i in np.arange(int(k * 7 / 0.01), int(k * 19 / 0.01 + 64)): T_se_post[i, 12] = 1 return T_se_post
import numpy as np import math import modern_robotics as mr T = 5 t = 3 s = mr.QuinticTimeScaling(T, t) print(s) X_start = np.identity(4) X_end = np.array([[0, 0, 1, 1], [1, 0, 0, 2], [0, 1, 0, 3], [0, 0, 0, 1]]) T_f = 10 N = 10 method = 3 trajectory = mr.ScrewTrajectory(X_start, X_end, T_f, N, method) print(trajectory[-2]) method = 5 trajectory = mr.CartesianTrajectory(X_start, X_end, T_f, N, method) print(trajectory[-2])
import math import numpy as np import modern_robotics as mr pi = math.pi Tf = 5 t = 3 N = 10 s = mr.QuinticTimeScaling(Tf, t) print("\nQuestion 5:\n", np.around(s, decimals=2)) X_start = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) X_end = np.array([[0, 0, 1, 1], [1, 0, 0, 2], [0, 1, 0, 3], [0, 0, 0, 1]]) Tf = 10 traj = mr.ScrewTrajectory(X_start, X_end, Tf, N, 3) print("\nQuestion 6:\n", np.array2string(np.around(traj[8], decimals=3), separator=','), sep='') traj = mr.CartesianTrajectory(X_start, X_end, Tf, N, 5) print("\nQuestion 7:\n", np.array2string(np.around(traj[8], decimals=3), separator=','), sep='')