Add files via upload

main1.m

@@ -0,0 +1,58 @@
+clc; clear;
+close all;
+addpath('functions');
+%% Parameters
+% UR5e dh parameters
+a = [0, -0.425, -0.3922, 0, 0, 0];
+d = [0.1625, 0, 0, 0.1333, 0.0997, 0.0996];
+alpha = [pi/2, 0, 0, pi/2, -pi/2, 0];
+% zero position
+theta0 = [0 0 0 0 0 0]*pi/180; % [rad]
+% trajectory parameters
+goal_t = 60; % each letter's time [s], 60
+% plane 1
+R = eulerR(pi/2, 0, pi/2);
+p = [-500, 0, 500]';
+% plane 2
+% R = eulerR(pi/2, 0, pi/4);
+% p = [-300, 300, 500]';
+interL = 10; % interpolation point distance [mm], 10
+acct = 0.2; % accelerate time percentage [%], 0.2
+dacct = 0.2; % decelerate time percentage [%], 0.2
+accp = 0.2; % interpolation segments are used to accelerate [%], 0.2
+daccp = 0.2; % interpolation segments are used to decelerate [%], 0.2
+eta = 0.5; % sampling points [s], 0.5
+v = 20; % expected velocity of motion between two points [mm/s], 20
+pause_t = 2; % pause time [s], 2
+%% 'FLOW' interpolation points
+% F
+F = [-175 100 0; -275 100 0; -275 -100 0; -275 20 0; -175 20 0];
+% L
+L = [-125 100 0; -125 -100 0; -25 -100 0];
+% O
+tt = -3*pi/2: 2*pi/10: pi/2;
+ox = 75 + 50*cos(tt);
+oy = 100*sin(tt);
+oz = zeros(1, size(ox, 2));
+O = [ox' oy' oz'];
+% W
+W = [150 100 0; 190 -100 0; 230 100 0; 270 -100 0; 310 100 0];
+words{1} = F; words{2} = L; words{3} = O; words{4} = W; % put them in a cell array
+%% Generate FLOW trajectroy
+[total_t, total_q_traj, total_v_traj, total_a_traj, eva_t] = writeWords(words, R, p, theta0, v, interL, goal_t, acct, dacct, accp, daccp, eta, pause_t);
+%% Video
+Endeffector = [];
+q = total_q_traj;
+for t = 1: length(total_t)
+    % end effector trajectory
+    [~, JointPos0] = fkineUR5e(q(t, :), d, a, alpha);
+    Endeffector = [Endeffector; JointPos0(end, :)];
+    % after optimization
+    ur5eDemo3D(q(t, :), 40, Endeffector*1000, 1, 1, roundn(total_t(t), 0));
+end
+%% Observe joint status
+for joint_num = 1: 6
+    jointPVA(joint_num, total_t, total_q_traj*180/pi, total_v_traj*180/pi, total_a_traj*180/pi, eva_t, goal_t, pause_t);
+end
+%% End effector status
+EndeffectorPVA(Endeffector, total_t, eva_t, goal_t, pause_t);