clc:clear
syms alfa0 omega0 Jmax teta0 
syms M1 M2 Mp Mw Rw Mtp g I1 I2 l C
syms t1 t2 t3 S 
syms t1dot t2dot t3dot Sdot 
syms t1dotdot t2dotdot t3dotdot Sdotdot
q=[S t2 t3].';
qdot=[Sdot t2dot t3dot].';
qdotdot=[Sdotdot t2dotdot t3dotdot].';
L = ((1/2*(Mp+M1+3/2*Mw*Rw^2+M2+Mtp)*Sdot^2)+(1/2*I1*(t1dot^2+t2dot^2)))+...
    (1/2*M2*(l^2*t3dot^2+l^2*(cos(t3*(t1dot^2+t2dot^2)))^2))+...
    (1/2*M2*(2*l^2*(cos(t3*t1dot*t2dot))^2-2*l*Sdot*t1dot*sin(t2)*cos(t3)-...
    2*l*Sdot*t2dot*sin(t2)*cos(t3)-2*l*Sdot*t3dot*sin(t3)*cos(t2)))+...
    (1/2*I2*((cos(t3*(t2dot^2+2*t1dot*t2dot+t2dot^2)))^2+t3dot^2))+...
    (1/2*Mtp*((C^2)*(t3dot^2)+(C^2)*(cos(t3*(t1dot^2+t2dot^2)))^2))+...
    (1/2*Mtp*((C^2)*((cos(t3*t1dot*t2dot))^2)-(2*C*Sdot*t1dot*sin(t2)*cos(t3))-...
    (2*C*Sdot*t2dot*sin(t2)*cos(t3))-(2*C*Sdot*t3dot*sin(t3)*cos(t2))))-...
    ((M2*l+Mtp*C)*g*sin(t3));
n=max(size(q));
%......% pLpqdot(i)=diff(L,dq(i));
pLpqdot1=diff(L,Sdot);
pLpqdot2=diff(L,t2dot);
pLpqdot3=diff(L,t3dot);
F = diff(pLpqdot1,S)*Sdot + diff(pLpqdot1,Sdot)*Sdotdot + ...
    diff(pLpqdot1,t2)*t2dot + diff(pLpqdot1,t2dot)*t2dotdot+...
    diff(pLpqdot1,t3)*t3dot + diff(pLpqdot1,t3dot)*t3dotdot+...
    diff(pLpqdot1,t1)*t1dot + diff(pLpqdot1,t1dot)*t1dotdot-diff(L,S);
Tu2= diff(pLpqdot2,S)*Sdot + diff(pLpqdot2,Sdot)*Sdotdot + ...
     diff(pLpqdot2,t2)*t2dot + diff(pLpqdot2,t2dot)*t2dotdot+...
     diff(pLpqdot2,t3)*t3dot + diff(pLpqdot2,t3dot)*t3dotdot+...
     diff(pLpqdot2,t1)*t1dot + diff(pLpqdot2,t1dot)*t1dotdot-diff(L,t2);
Tu3= diff(pLpqdot3,S)*Sdot + diff(pLpqdot3,Sdot)*Sdotdot + ...
     diff(pLpqdot3,t2)*t2dot + diff(pLpqdot3,t2dot)*t2dotdot+...
     diff(pLpqdot3,t3)*t3dot + diff(pLpqdot3,t3dot)*t3dotdot+...
     diff(pLpqdot3,t1)*t1dot + diff(pLpqdot3,t1dot)*t1dotdot-diff(L,t3);
F=simplify(F);
Tu2=simple(Tu2);
Tu3=simple(Tu3);
%....First segment
figure(1)
alfa0=0;alfa_max=1;omega0=0;omega_max=0.75;teta0=0;Jmax=2;
i=0;
for t=0:0.001:0.5
    i=i+1;
    alfa(i,1)=i;
    omega(i,1)=i;
    tetasoft(i,1)=i;
    alfa(i,2)=alfa0 + Jmax*t;
    omega(i,2)=omega0+alfa0*t+1/2*Jmax*t^2;
    tetasoft(i,2)=(384*t^5)/16807 - (480*t^4)/2401 + (160*t^3)/343;
    plot(t,tetasoft(i,2),t,omega(i,2),'-',t,alfa(i,2),'--');
    hold on
end
%...Second Segment
alfa0=0;alfa_max=1;omega0=-0.25;omega_max=0.75;teta0=0.083;Jmax=2;
for t=0.5:0.001:1
    i=i+1;
    alfa(i,1)=i;
    omega(i,1)=i;
    tetasoft(i,1)=i;
    alfa(i,2)=alfa_max;
    omega(i,2)=omega0+alfa_max*t;
    tetasoft(i,2)=(384*t^5)/16807 - (480*t^4)/2401 + (160*t^3)/343;
    plot(t,tetasoft(i,2),t,omega(i,2),'-',t,alfa(i,2),'--');
    hold on
end
%....3rd segment
alfa0=3;omega0=-1.25;teta0=-0.601;omega_max=0.934;Jmax=-2;
for t=1:0.001:1.5
    i=i+1;
    alfa(i,1)=i;
    omega(i,1)=i;
    tetasoft(i,1)=i;
    alfa(i,2)=alfa0 + Jmax*t;
    omega(i,2)=omega0+alfa0*t+1/2*Jmax*t^2;
    tetasoft(i,2)=(384*t^5)/16807 - (480*t^4)/2401 + (160*t^3)/343;
    plot(t,tetasoft(i,2),t,omega(i,2),'-',t,alfa(i,2),'--');
    hold on
end
%...4th segment
teta0=-0.7;omega_max=1;
for t=1.5:0.001:2
    i=i+1;
    alfa(i,1)=i;
    omega(i,1)=i;
    tetasoft(i,1)=i;
    alfa(i,2)=0;
    omega(i,2)=omega_max;
    tetasoft(i,2)=(384*t^5)/16807 - (480*t^4)/2401 + (160*t^3)/343;
    plot(t,tetasoft(i,2),t,omega(i,2),'-',t,alfa(i,2),'--');
    hold on
end
%...5th Segment
alfa0=4;omega0=-3;teta0=-2.36;Jmax=-2;
for t=2:0.001:2.5
    i=i+1;
    alfa(i,1)=i;
    omega(i,1)=i;
    tetasoft(i,1)=i;
    alfa(i,2)=alfa0 + Jmax*t;
    omega(i,2)=omega0+alfa0*t+1/2*Jmax*t^2;
    tetasoft(i,2)=(384*t^5)/16807 - (480*t^4)/2401 + (160*t^3)/343;
    plot(t,tetasoft(i,2),t,omega(i,2),'-',t,alfa(i,2),'--');
    hold on
end
%...6th segment
alfa_max=-1;omega0=3.25;
for t=2.5:0.001:3
    i=i+1;
    alfa(i,1)=i;
    omega(i,1)=i;
    tetasoft(i,1)=i;
    alfa(i,2)=alfa_max;
    omega(i,2)=omega0+alfa_max*t;
    tetasoft(i,2)=(384*t^5)/16807 - (480*t^4)/2401 + (160*t^3)/343;
    plot(t,tetasoft(i,2),t,omega(i,2),'-',t,alfa(i,2),'--');
    hold on
end
%...7th segment
alfa0=-7;alfa_max=1;omega0=12.25;omega_max=0.75;teta0=0;Jmax=2;
for t=3:0.001:3.5
    i=i+1;
    alfa(i,1)=i;
    omega(i,1)=i;
    tetasoft(i,1)=i;
    alfa(i,2)=alfa0 + Jmax*t;
    omega(i,2)=omega0+alfa0*t+1/2*Jmax*t^2;
    tetasoft(i,2)=(384*t^5)/16807 - (480*t^4)/2401 + (160*t^3)/343;
    plot(t,tetasoft(i,2),t,omega(i,2),'-',t,alfa(i,2),'--');
end
grid on
legend('Angle rad','Angular Velocity rad/s','Angular Acceleration rad/s^2',2)
xlabel('Time (S)');
title('Soft motion trajectory for joint 2');
hold off
figure(2)
teta0=0;tetaf=3.2;tf=3.5;
a0=teta0;
a1=0;
a2=0;
a3=(20*(tetaf-teta0))/(2*tf*tf*tf);
a4=(30*(teta0-tetaf))/(2*tf*tf*tf*tf);
a5=(12*(tetaf-teta0))/(2*tf*tf*tf*tf*tf);
teta2= a0+a1*t+a2*t^2+a3*t^3+a4*t^4+a5*t^5;
dotteta2=a1+2*a2*t+3*a3*t^2+4*a4*t^3+5*a5*t^4;
dotdotteta2=2*a2+6*a3*t+12*a4*t^2+20*a5*t^3;
k=0;
for t=0:0.001:3.5
    k=k+1;
    teta(k,1)=k;
    dotteta(k,1)=k;
    dotdotteta(k,1)=k;
    teta2(k,2)= a0+a1*t+a2*t^2+a3*t^3+a4*t^4+a5*t^5;
    dotteta2(k,2)=a1+2*a2*t+3*a3*t^2+4*a4*t^3+5*a5*t^4;
    dotdotteta2(k,2)=2*a2+6*a3*t+12*a4*t^2+20*a5*t^3;
    plot(t,teta2(k,2),t,dotteta2(k,2),'-',t,dotdotteta2(k,2),'--')
    hold on
end
grid on
legend('Angle rad','Angular Velocity rad/s','Angular Acceleration rad/s^2',2)
xlabel('Time (S)');
title('Quintic trajectory for joint 2');
%........................Quintic trajectory for other joint
figure(3)
teta0=-0.26;tetaf=0.26;tf=3.5;
a0=teta0;
a1=0;
a2=0;
a3=(20*(tetaf-teta0))/(2*tf*tf*tf);
a4=(30*(teta0-tetaf))/(2*tf*tf*tf*tf);
a5=(12*(tetaf-teta0))/(2*tf*tf*tf*tf*tf);
teta1= a0+a1*t+a2*t^2+a3*t^3+a4*t^4+a5*t^5;
dotteta1=a1+2*a2*t+3*a3*t^2+4*a4*t^3+5*a5*t^4;
dotdotteta1=2*a2+6*a3*t+12*a4*t^2+20*a5*t^3;
k=0;
for t=0:0.001:3.5
    k=k+1;
    teta(k,1)=k;
    dotteta(k,1)=k;
    dotdotteta(k,1)=k;
    teta1(k,2)= a0+a1*t+a2*t^2+a3*t^3+a4*t^4+a5*t^5;
    dotteta1(k,2)=a1+2*a2*t+3*a3*t^2+4*a4*t^3+5*a5*t^4;
    dotdotteta1(k,2)=2*a2+6*a3*t+12*a4*t^2+20*a5*t^3;
    plot(t,teta1(k,2),t,dotteta1(k,2),'-',t,dotdotteta1(k,2),'--')
    hold on
end
grid on
legend('Angle rad','Angular Velocity rad/s','Angular Acceleration rad/s^2',2)
xlabel('Time (S)');
title('Quintic trajectory for joint 1');
figure(4)
teta0=pi/3;tetaf=pi/18;tf=3.5;
a0=teta0;
a1=0;
a2=0;
a3=(20*(tetaf-teta0))/(2*tf*tf*tf);
a4=(30*(teta0-tetaf))/(2*tf*tf*tf*tf);
a5=(12*(tetaf-teta0))/(2*tf*tf*tf*tf*tf);
teta3= a0+a1*t+a2*t^2+a3*t^3+a4*t^4+a5*t^5;
dotteta3=a1+2*a2*t+3*a3*t^2+4*a4*t^3+5*a5*t^4;
dotdotteta3=2*a2+6*a3*t+12*a4*t^2+20*a5*t^3;
k=0;
for t=0:0.001:3.5
    k=k+1;
    teta(k,1)=k;
    dotteta(k,1)=k;
    dotdotteta(k,1)=k;
    teta3(k,2)= a0+a1*t+a2*t^2+a3*t^3+a4*t^4+a5*t^5;
    dotteta3(k,2)=a1+2*a2*t+3*a3*t^2+4*a4*t^3+5*a5*t^4;
    dotdotteta3(k,2)=2*a2+6*a3*t+12*a4*t^2+20*a5*t^3;
    plot(t,teta3(k,2),t,dotteta3(k,2),'-',t,dotdotteta3(k,2),'--')
    hold on
end
grid on
legend('Angle rad','Angular Velocity rad/s','Angular Acceleration rad/s^2',2)
xlabel('Time (S)');
title('Quintic trajectory for joint 3');
figure(5)
for i=1:1:3500
    l=0.15;M1=3.5;M2=4;I1=0.0175;I2=0.03;Mp=20;Mw=5;Rw=0.1;Mtp=0.5;C=0.25;sdotdot=0.001;sdot=3;g=9.81;
    F=1.9*(sdotdot*(Mp*3/2*Mw*Rw^2+M1+M2+Mtp)-(M2*l+Mtp*C)*sin(tetasoft(i,2))*cos(teta3(k,2))*(dotdotteta1(k,2)+alfa(i,2))...
    -(M2*l+Mtp*C)*sin(teta3(k,2))*cos(tetasoft(i,2))*dotdotteta3(k,2)...
    +(M2*l+Mtp*C)*sin(teta3(k,2))*sin(tetasoft(i,2))*((dotteta1(k,2)*dotteta3(k,2))+2*(omega(i,2)*dotteta3(k,2)))...
    -(M2*l+Mtp*C)*cos(tetasoft(i,2))*cos(teta3(k,2))*((dotteta1(k,2)*omega(i,2))+(omega(i,2))^2 +(dotteta3(k,2))^2));
    Tu2=(I2*((cos(teta3(k,2)))^2)+(M2*l^2+Mtp*C^2)*(cos(teta3(k,2)))^2)*(dotdotteta1(k,2)) ...
    +(I1+I2*((cos(teta3(k,2)))^2)+(M2*l^2+Mtp*C^2)*(cos(teta3(k,2)))^2)*(alfa(i,2)) ...
    -(M2*l+Mtp*C)*sdotdot*sin(tetasoft(i,2))*cos(teta3(k,2))...
    +(M2*l+Mtp*C)*sdot*dotteta1(k,2)*cos(tetasoft(i,2))*cos(teta3(k,2))...
    -2*(M2*l^2+Mtp*C^2)*sin(teta3(k,2))*cos(teta3(k,2))*((dotteta1(k,2)*dotteta3(k,2))+(omega(i,2)*dotteta3(k,2)))...
    -2*I2*sin(teta3(k,2))*cos(teta3(k,2))*((dotteta1(k,2)*dotteta3(k,2))+(omega(i,2)*dotteta3(k,2)));
    plot(0.001*i,F,0.001*i,Tu2,'--')
    hold on
end
legend('Force N','Torque 2 Nm','Torque 3 Nm',2)
for k=1:1:3500
    l=0.15;M1=3.5;M2=4;I1=0.0175;I2=0.03;Mp=20;Mw=5;Rw=0.1;Mtp=0.5;C=0.25;sdotdot=0.001;sdot=0.1;g=9.81;
    Tu3=1.05*((I2+M2*l^2+Mtp*C^2)*(dotdotteta3(k,2))-(M2*l^2+Mtp*C^2)*sdotdot*sin(teta3(k,2))*cos(teta2(k,2))...
    +(I2+M2*l^2+Mtp*C^2)*cos(teta3(k,2))*sin(teta3(k,2))*((dotteta1(k,2)+dotteta2(k,2))^2)...
    -(M2*l+Mtp*C)*sdot*sin(teta2(k,2))*sin(teta3(k,2))+(M2*l+Mtp*C)*g*cos(teta3(k,2)));
    plot(0.001*k,Tu3,'r-')
    hold on
end
grid on
xlabel('Time (S)');
title('Joint Force and Torques (Soft motion trajectory, Mtp=0.5 Kg)');
figure(6)
for i=1:1:3500
    l=0.15;M1=3.5;M2=4;I1=0.0175;I2=0.03;Mp=20;Mw=5;Rw=0.1;Mtp=1;C=0.25;sdotdot=0.001;sdot=3;g=9.81;
    F=2.3*(sdotdot*(Mp*3/2*Mw*Rw^2+M1+M2+Mtp)-(M2*l+Mtp*C)*sin(tetasoft(i,2))*cos(teta3(k,2))*(dotdotteta1(k,2)+alfa(i,2))...
    -(M2*l+Mtp*C)*sin(teta3(k,2))*cos(tetasoft(i,2))*dotdotteta3(k,2)...
    +(M2*l+Mtp*C)*sin(teta3(k,2))*sin(tetasoft(i,2))*((dotteta1(k,2)*dotteta3(k,2))+2*(omega(i,2)*dotteta3(k,2)))...
    -(M2*l+Mtp*C)*cos(tetasoft(i,2))*cos(teta3(k,2))*((dotteta1(k,2)*omega(i,2))+(omega(i,2))^2 +(dotteta3(k,2))^2));
    Tu2=(I2*((cos(teta3(k,2)))^2)+(M2*l^2+Mtp*C^2)*(cos(teta3(k,2)))^2)*(dotdotteta1(k,2)) ...
    +(I1+I2*((cos(teta3(k,2)))^2)+(M2*l^2+Mtp*C^2)*(cos(teta3(k,2)))^2)*(alfa(i,2)) ...
    -(M2*l+Mtp*C)*sdotdot*sin(tetasoft(i,2))*cos(teta3(k,2))...
    +(M2*l+Mtp*C)*sdot*dotteta1(k,2)*cos(tetasoft(i,2))*cos(teta3(k,2))...
    -2*(M2*l^2+Mtp*C^2)*sin(teta3(k,2))*cos(teta3(k,2))*((dotteta1(k,2)*dotteta3(k,2))+(omega(i,2)*dotteta3(k,2)))...
    -2*I2*sin(teta3(k,2))*cos(teta3(k,2))*((dotteta1(k,2)*dotteta3(k,2))+(omega(i,2)*dotteta3(k,2)));
    plot(0.001*i,F,0.001*i,Tu2,'--')
    hold on
end
legend('Force N','Torque 2 Nm','Torque 3 Nm',2)
for k=1:1:3500
    l=0.15;M1=3.5;M2=4;I1=0.0175;I2=0.03;Mp=20;Mw=5;Rw=0.1;Mtp=1;C=0.25;sdotdot=0.001;sdot=0.1;g=9.81;
    Tu3=1.05*((I2+M2*l^2+Mtp*C^2)*(dotdotteta3(k,2))-(M2*l^2+Mtp*C^2)*sdotdot*sin(teta3(k,2))*cos(teta2(k,2))...
    +(I2+M2*l^2+Mtp*C^2)*cos(teta3(k,2))*sin(teta3(k,2))*((dotteta1(k,2)+dotteta2(k,2))^2)...
    -(M2*l+Mtp*C)*sdot*sin(teta2(k,2))*sin(teta3(k,2))+(M2*l+Mtp*C)*g*cos(teta3(k,2)));
    plot(0.001*k,Tu3,'r-')
    hold on
end
grid on
xlabel('Time (S)');
title('Joint Force and Torques (Soft motion trajectory, Mtp=1 Kg)');
figure(7)
for k=1:1:3500
    l=0.15;M1=3.5;M2=4;I1=0.0175;I2=0.03;Mp=20;Mw=5;Rw=0.1;Mtp=0.5;C=0.25;sdotdot=0.001;sdot=0.1;g=9.81;
    F=sdotdot*(Mp*3/2*Mw*Rw^2+M1+M2+Mtp)-(M2*l+Mtp*C)*sin(teta2(k,2))*cos(teta3(k,2))*(dotdotteta1(k,2)+dotdotteta2(k,2))...
    -(M2*l+Mtp*C)*sin(teta3(k,2))*cos(teta2(k,2))*dotdotteta3(k,2)...
    +(M2*l+Mtp*C)*sin(teta3(k,2))*sin(teta2(k,2))*((dotteta1(k,2)*dotteta3(k,2))+2*(dotteta2(k,2)*dotteta3(k,2)))...
    -(M2*l+Mtp*C)*cos(teta2(k,2))*cos(teta3(k,2))*((dotteta1(k,2)*dotteta2(k,2))+(dotteta2(k,2))^2 +(dotteta3(k,2))^2);
    Tu3=(I2+M2*l^2+Mtp*C^2)*(dotdotteta3(k,2))-(M2*l^2+Mtp*C^2)*sdotdot*sin(teta3(k,2))*cos(teta2(k,2))...
    +(I2+M2*l^2+Mtp*C^2)*cos(teta3(k,2))*sin(teta3(k,2))*((dotteta1(k,2)+dotteta2(k,2))^2)...
    -(M2*l+Mtp*C)*sdot*sin(teta2(k,2))*sin(teta3(k,2))+(M2*l+Mtp*C)*g*cos(teta3(k,2));
    Tu2=(I2*((cos(teta3(k,2)))^2)+(M2*l^2+Mtp*C^2)*(cos(teta3(k,2)))^2)*(dotdotteta1(k,2)) ...
    +(I1+I2*((cos(teta3(k,2)))^2)+(M2*l^2+Mtp*C^2)*(cos(teta3(k,2)))^2)*(dotdotteta2(k,2)) ...
    -(M2*l+Mtp*C)*sdotdot*sin(teta2(k,2))*cos(teta3(k,2))...
    +(M2*l+Mtp*C)*sdot*dotteta1(k,2)*cos(teta2(k,2))*cos(teta3(k,2))...
    -2*(M2*l^2+Mtp*C^2)*sin(teta3(k,2))*cos(teta3(k,2))*((dotteta1(k,2)*dotteta3(k,2))+(dotteta2(k,2)*dotteta3(k,2)))...
    -2*I2*sin(teta3(k,2))*cos(teta3(k,2))*((dotteta1(k,2)*dotteta3(k,2))+(dotteta2(k,2)*dotteta3(k,2)));
    plot(0.001*k,F,0.001*k,Tu2,'--',0.001*k,Tu3,'-')
    hold on
end
grid on
legend('Force N','Torque 2 Nm','Torque 3 Nm',2)
xlabel('Time (S)');
title('Joint Force and Torques (Quintic trajectory, Mtp=0.5 Kg)');
figure(8)
for k=1:1:3500
    l=0.15;M1=3.5;M2=4;I1=0.0175;I2=0.03;Mp=20;Mw=5;Rw=0.1;Mtp=1;C=0.25;sdotdot=0.001;sdot=0.1;g=9.81;
    F=sdotdot*(Mp*3/2*Mw*Rw^2+M1+M2+Mtp)-(M2*l+Mtp*C)*sin(teta2(k,2))*cos(teta3(k,2))*(dotdotteta1(k,2)+dotdotteta2(k,2))...
    -(M2*l+Mtp*C)*sin(teta3(k,2))*cos(teta2(k,2))*dotdotteta3(k,2)...
    +(M2*l+Mtp*C)*sin(teta3(k,2))*sin(teta2(k,2))*((dotteta1(k,2)*dotteta3(k,2))+2*(dotteta2(k,2)*dotteta3(k,2)))...
    -(M2*l+Mtp*C)*cos(teta2(k,2))*cos(teta3(k,2))*((dotteta1(k,2)*dotteta2(k,2))+(dotteta2(k,2))^2 +(dotteta3(k,2))^2);
    Tu3=(I2+M2*l^2+Mtp*C^2)*(dotdotteta3(k,2))-(M2*l^2+Mtp*C^2)*sdotdot*sin(teta3(k,2))*cos(teta2(k,2))...
    +(I2+M2*l^2+Mtp*C^2)*cos(teta3(k,2))*sin(teta3(k,2))*((dotteta1(k,2)+dotteta2(k,2))^2)...
    -(M2*l+Mtp*C)*sdot*sin(teta2(k,2))*sin(teta3(k,2))+(M2*l+Mtp*C)*g*cos(teta3(k,2));
    Tu2=(I2*((cos(teta3(k,2)))^2)+(M2*l^2+Mtp*C^2)*(cos(teta3(k,2)))^2)*(dotdotteta1(k,2)) ...
    +(I1+I2*((cos(teta3(k,2)))^2)+(M2*l^2+Mtp*C^2)*(cos(teta3(k,2)))^2)*(dotdotteta2(k,2)) ...
    -(M2*l+Mtp*C)*sdotdot*sin(teta2(k,2))*cos(teta3(k,2))...
    +(M2*l+Mtp*C)*sdot*dotteta1(k,2)*cos(teta2(k,2))*cos(teta3(k,2))...
    -2*(M2*l^2+Mtp*C^2)*sin(teta3(k,2))*cos(teta3(k,2))*((dotteta1(k,2)*dotteta3(k,2))+(dotteta2(k,2)*dotteta3(k,2)))...
    -2*I2*sin(teta3(k,2))*cos(teta3(k,2))*((dotteta1(k,2)*dotteta3(k,2))+(dotteta2(k,2)*dotteta3(k,2)));
    plot(0.001*k,F,0.001*k,Tu2,'--',0.001*k,Tu3,'-')
    hold on
end
grid on
legend('Force N','Torque 2 Nm','Torque 3 Nm',2)
xlabel('Time (S)');
title('Joint Force and Torques (Quintic trajectory, Mtp=1 Kg)');