% Characteristics of the Robot
m1 = 10;  % kg
m2 = 5;   % kg
d1 = 200; % mm
l2 = 400; % mm
w0 = [-100;-200;200;0;0;-1];
w1 = [200;150;200;0;0;-1];
T  = 15;   % s
dt = 0.01; % s
t = 0:dt:T;

Function sddot(t, T) is the speed profile to be used for that motion in tool-configuration space is a linear acceleration profile and indirectly given by

a0 = 6/T^2;
sddot = a0 - 2*a0*t/T;

Function sdot(t, T) delivering the value of the integral of speed profile

sz = size(t);
sdot(1) = 0;
for i =1:sz(2)-1;
    sdot(i+1) = sdot(i) + dt*sddot(i);
end

Function s(t, T)

s(1) = 0;
for i =1:sz(2)-1;
    s(i+1) = s(i) + dt*sdot(i);
end

Function w(t, T, w0, w1) delivering the tool-configuration position at time t of the straight line between w0 and w1 Function wdot(t, T, w0, w1) delivering the tool-configuration velocity at time t of the straight line between w0 and w1,computed using function sdot(t, T) Function wddot(t, T, w0, w1) delivering the tool-configuration acceleration at time t of the straight line between w0 and w1, computed using function sddot(t, T)

for i = 1:sz(2)
    w_des(:,i)      = (1 - s(i))*w0 + s(i)*w1;
    wdot_des(:,i)   = sdot(i)*(w1-w0);
    wddot_des(:,i)  = sddot(i)*(w1-w0);
end

Function qw(w), yielding the vector of joint variables given a tool-configuration position (i.e. solution of inverse kinematics)

for i = 1:sz(2)
q_des(1,i) = atan2(-w_des(1,i),w_des(2,i));
%q_des(2,i) = w_des(2,i)/cos(q_des(1,i)); % vers1
q_des(2,i) = sqrt(w_des(1,i)^2+w_des(2,i)^2);
end

Function qdot(w, wdot), computing the vector of joint velocities given a tool-configuration position and velocity Function qddot(w, wdot, wddot), computing the vector of joint accelerations given a tool-configuration position, velocity and acceleration

for i = 1:sz(2)
qdot(1,i) = (w_des(1,i)*wdot_des(2,i)-w_des(2,i)*wdot_des(1,i))/...
    (w_des(1,i)^2+w_des(2,i)^2);
qdot(2,i) = (w_des(1,i)*wdot_des(1,i)+w_des(2,i)*wdot_des(2,i))/...
    sqrt(w_des(1,i)^2+w_des(2,i)^2);
w1 = w_des(1,i);
w2 = w_des(2,i);
wdt1 = wdot_des(1,i); wdt2 = wdot_des(2,i);
wddt1 = wddot_des(1,i);wddt2 = wddot_des(2,i);
qddot(1,i) = ((w1^2+w2^2)*(w1*wddt2-w2*wddt1)-2*(w1*wdt2-w2*wdt1)*...
    (w1*wdt1+w2*wdt2))/((w1^2+w2^2)^2);
qddot(2,i) = ((w1^2+w2^2)*(w1*wddt1+w2*wddt2)+(w1*wdt2-w2*wdt1)^2)/...
    ((w1^2+w2^2)^1.5);
end

Function torques(q, qdot, qddot), computing the vector of torques given the vectors of joint-space positions q, velocities qdot and accelerations qddot, equations of motion, direct dynamics

for i = 1:sz(2)
 % torques(q, qdot, qddot)
 trq1(i) = m2*(l2^2/3 - l2*q_des(2,i)+q_des(2,i)^2)*qddot(1,i)  +...
     m2*(2*q_des(2,i)-l2)*qdot(1,i)* qdot(2,i);
 trq2(i) = m2*qddot(2,i)+m2*(l2/2 - q_des(2,i))*qdot(1,i)^2;
%
end
q_cur    = [q_des(1,1); q_des(2,1)];
qdot_cur = [0;0];
for i = 1:sz(2)
  tau   = [trq1(i) trq2(i)];
  ct = t(i); % Current time
  [t0, y0] = ode45(@(ct,y) dyns(ct,y,tau), [ct,ct+dt],...
      [q_cur(1); qdot_cur(1); q_cur(2); qdot_cur(2)]);
rs = size(y0);
q_cur = [y0(rs(1),1);y0(rs(1),3)];
q_curv(i,:) = q_cur;
qdot_cur = [y0(rs(1),2);y0(rs(1),4)];
% Function wq(q), yielding the tool-configuration position for  vector
% of  joint  variables q,   direct kinematics
% Given  q = [q1 ,q2], return w
w_cur = [-sin(q_cur(1))*q_cur(2); cos(q_cur(1))*q_cur(2); d1];
% fprintf('[ %6.4f %6.4f  %6.4f ] \n',w_cur);
w_curv(i,:) = w_cur;
end
% Plots
figure('Position',[200 0 600 600])
subplot(2,1,1);
hold on;
plot(t,q_curv(:,1)*180/pi ,'r');
xlabel('Time[s]');
ylabel('q1(t)[deg]');
grid on;
subplot(2,1,2);
plot(t,q_curv(:,2),'b');
xlabel('Time[s]');
ylabel('q2(t)[mm]');
grid on;
figure('Position',[200 0 600 600])
subplot(2,1,1);
plot(t,w_curv(:,1),'r');
xlabel('Time[s]');
ylabel('w1(t)[mm]');
grid on;
subplot(2,1,2);
plot(t,w_curv(:,2),'b');
xlabel('Time[s]');
ylabel('w2(t)[mm]');
grid on;