---------------------------------------

x0 = 1.5; x1 = -1; x0dd=0; khai báo x0 x1

x0d = 0; x1d = 0;
y0 = 0; y1=1; khai báo y0 y1
z0 = 1; z1=1.5; khai báo z0 z1
t0=0; t1=10; khai báo t0 t1
---------------------------------------

%find time path for coordinates; để tìm x

A = [1 t0 t0^2 t0^3 t0^4 t0^5;
0 1 2*t0 3*t0^2 4*t0^3 5*t0^4;
0 0 2 6*t0 12*t0^2 20*t0^3;
1 t1 t1^2 t1^3 t1^4 t1^5;
0 1 2*t1 3*t1^2 4*t1^3 5*t1^4;
0 0 2 6*t1 12*t1^2 20*t1^3];

b = [x0; x0d; x0dd; x1; x0d; x0dd];

a = A\b ( inverse)
a0 = a(1); a1 = a(2); a2 = a(3); a3 = a(4); a4 = a(5);a5 = a(6);
---------------------------------------
%quintic path for X; Tìm phương trình X
t = t0:0.01:t1;
X = a0 + a1*t + a2*t.^2 + a3*t.^3+ a4*t.^4+ a5*t.^5;
Xtd = a1 + 2*a2*t + 3*a3*t.^2+ 4*a4*t.^3+ 5*a5*t.^4;
Xtdd = 2*a2 + 6*a3*t+ 12*a4*t.^2+ 20*a5*t.^3;
Xtddd = 6*a3+ 24*a4*t+ 60*a5*t.^2;
---------------------------------------
%quintic path for Y,Z base on X
Y = ((X-x0)*(y1-y0))/(x1-x0)+y0; Tìm phương trình Y theo X
Z =((X-x0)*(z1-z0))/(x1-x0)+z0; Tìm phương trình Z theo X

figure
subplot(2,2,1)
plot(t,X,'r', t,Xtd,'g', t,Xtdd,'b');
legend('X', 'Xd', 'Xdd');
xlabel('t')
ylabel('x(t)')
title('quintic path for X (time path)')

subplot(2,2,2)
plot(t,Y,'r')
grid on
legend('Y');
xlabel('t')
ylabel('y(t)')
title('quintic path for Y (time path)')

subplot(2,2,3)
plot(t,Z,'r')
grid on
legend('Z');
xlabel('t')
ylabel('z(t)')
title('quintic path for Z (time path)')

subplot(2,2,4)
plot3(X,Y,Z,'m')
grid on
xlabel('x(t)')
ylabel('y(t)')
zlabel('z(t)')
title('path planning from P1 to P2')
---------------------------------------
%inverse to find joint variables
l1 = 1;
l2 = 1;
l3 = 1;
q1=[];
q2=[];
q3=[];

for t = t0:t1
X = a0 + a1*t + a2*t^2 + a3*t^3+ a4*t^4+ a5*t^5;
Y = ((X-x0)*(y1-y0))/(x1-x0)+y0;
Z =(Y-y0)*((z1-z0)/(y1-y0))+z0;
if X>=0
theta1 = atan(Y/X);
elseif X<0
theta1 = atan(Y/X)+pi;
end

c1 = l1^2 - 2*l1*Z + l2^2 + (2*l2*X)/cos(theta1) - l3^2 + (X^2)/(cos(theta1)^2) +

Z^2;

c2 = 2*l1*l2 - 2*l2*Z;

c3 = l1^2 - 2*l1*Z + l2^2 - (2*l2*X)/cos(theta1) - l3^2 + (X^2)/(cos(theta1)^2) +

Z^2;

theta2 = 2*atan((-c2 + sqrt(c2^2 - c1*c3))/c1);

theta3 = acos((l1 - Z + l2*sin(theta2))./l2)- theta2;

q1 = [q1 theta1];
q2 = [q2 theta2];
q3 = [q3 theta3];
end

figure
subplot(1,2,1)
plot(q1,'r');
hold on
plot(q2,'b');
hold on
plot(q3,'g');
legend('theta1','theta2','theta3');
xlabel('t')
ylabel('theta(t)')
title('theta angle')