Contents

clear
close all
clc
options=optimset('Display','off','LargeScale','off'); % Necessary for 'fsolve'

Define geometric constant parameters and time vector

Oao=(45*pi)/180;
L2=1;
L3=2.3;
L4=1.8;
L1=2;
L5=3;
B=(30*pi)/180;
W=(30*pi)/180;

Define an initial guess for vector q

xo= [1,1,3,6,5,1,9,2,6,8,3]';
time=0:0.1:12;

Use a 'for' structure for computing kinematics

for i=1:length(time)
    if i>1
    xo=x;
    end

% % Pick the time value
    t=time(i);
% % Use 'fsolve' for obtaining positions q.
    [x] = fsolve('pos',xo,options,L1,L2,L3,L4,L5,B,W,Oao,t);
    alm_x(i,:)=x;
xa=x(1);
ya=x(2);
Oa=x(3);
xb=x(4);
yb=x(5);
Ob=x(6);
xc=x(7);
yc=x(8);
Oc=x(9);
xd=x(10);
yd=x(11);
% % Solve and store velocities qp
ve=[-(L2*W*sin(Oa))/2;
    (L2*W*cos(Oa))/2;
    W;
    (L2*W*(cos(Oa)*sin(Ob)*sin(Oc) - 2*cos(Ob)*sin(Oa)*sin(Oc) + cos(Oc)*sin(Oa)*sin(Ob)))/(2*(cos(Ob)*sin(Oc) - cos(Oc)*sin(Ob)));
    (L2*W*(cos(Oa)*cos(Ob)*sin(Oc) - 2*cos(Oa)*cos(Oc)*sin(Ob) + cos(Ob)*cos(Oc)*sin(Oa)))/(2*(cos(Ob)*sin(Oc) - cos(Oc)*sin(Ob)));
    -(L2*W*(cos(Oa)*sin(Oc) - cos(Oc)*sin(Oa)))/(L3*(cos(Ob)*sin(Oc) - cos(Oc)*sin(Ob)));
    (L2*W*(cos(Oa)*sin(Ob)*sin(Oc) - cos(Ob)*sin(Oa)*sin(Oc)))/(2*(cos(Ob)*sin(Oc) - cos(Oc)*sin(Ob)));
    -(L2*W*(cos(Oa)*cos(Oc)*sin(Ob) - cos(Ob)*cos(Oc)*sin(Oa)))/(2*(cos(Ob)*sin(Oc) - cos(Oc)*sin(Ob)));
    (L2*W*(cos(Oa)*sin(Ob) - cos(Ob)*sin(Oa)))/(L4*(cos(Ob)*sin(Oc) - cos(Oc)*sin(Ob)));
    (L2*W*(L5*sin(B + Ob)*cos(Oa)*sin(Oc) - L5*sin(B + Ob)*cos(Oc)*sin(Oa) - 2*L3*cos(Ob)*sin(Oa)*sin(Oc) + 2*L3*cos(Oc)*sin(Oa)*sin(Ob)))/(2*L3*(cos(Ob)*sin(Oc) - cos(Oc)*sin(Ob)));
    -(L2*W*(L5*cos(B + Ob)*cos(Oa)*sin(Oc) - L5*cos(B + Ob)*cos(Oc)*sin(Oa) - 2*L3*cos(Oa)*cos(Ob)*sin(Oc) + 2*L3*cos(Oa)*cos(Oc)*sin(Ob)))/(2*L3*(cos(Ob)*sin(Oc) - cos(Oc)*sin(Ob)))];
alm_ve(:,i)=ve;
% % Solve and store accelerations qpp
xap=ve(1);
yap=ve(2);
Oap=ve(3);
xbp=ve(4);
ybp=ve(5);
Obp=ve(6);
xcp=ve(7);
ycp=ve(8);
Ocp=ve(9);
xdp=ve(10);
ydp=ve(11);
ac= [(L2*Oap^2*cos(Oa))/2;
(L2*Oap^2*sin(Oa))/2;
0;
(L3*Obp^2*cos(Ob)^2*sin(Oc) + L4*Ocp^2*cos(Oc)^2*sin(Ob) + L3*Obp^2*sin(Ob)^2*sin(Oc) + L4*Ocp^2*sin(Ob)*sin(Oc)^2 + 2*L2*Oap^2*cos(Oa)*cos(Ob)*sin(Oc) - L2*Oap^2*cos(Oa)*cos(Oc)*sin(Ob) + L2*Oap^2*sin(Oa)*sin(Ob)*sin(Oc))/(2*(cos(Ob)*sin(Oc) - cos(Oc)*sin(Ob)));
-(L3*Obp^2*cos(Ob)^2*cos(Oc) + L4*Ocp^2*cos(Ob)*cos(Oc)^2 + L3*Obp^2*cos(Oc)*sin(Ob)^2 + L4*Ocp^2*cos(Ob)*sin(Oc)^2 + L2*Oap^2*cos(Oa)*cos(Ob)*cos(Oc) - L2*Oap^2*cos(Ob)*sin(Oa)*sin(Oc) + 2*L2*Oap^2*cos(Oc)*sin(Oa)*sin(Ob))/(2*(cos(Ob)*sin(Oc) - cos(Oc)*sin(Ob)));
-(L2*cos(Oa)*Oap^2*cos(Oc) + L2*sin(Oa)*Oap^2*sin(Oc) + L3*cos(Ob)*Obp^2*cos(Oc) + L3*sin(Ob)*Obp^2*sin(Oc) + L4*Ocp^2*cos(Oc)^2 + L4*Ocp^2*sin(Oc)^2)/(L3*(cos(Ob)*sin(Oc) - cos(Oc)*sin(Ob)));
(L2*cos(Oa)*Oap^2*cos(Ob)*sin(Oc) + L2*sin(Oa)*Oap^2*sin(Ob)*sin(Oc) + L3*Obp^2*cos(Ob)^2*sin(Oc) + L3*Obp^2*sin(Ob)^2*sin(Oc) + L4*Ocp^2*cos(Oc)^2*sin(Ob) + L4*Ocp^2*sin(Ob)*sin(Oc)^2)/(2*(cos(Ob)*sin(Oc) - cos(Oc)*sin(Ob)));
-(L2*cos(Oa)*Oap^2*cos(Ob)*cos(Oc) + L2*sin(Oa)*Oap^2*cos(Oc)*sin(Ob) + L3*Obp^2*cos(Ob)^2*cos(Oc) + L3*Obp^2*cos(Oc)*sin(Ob)^2 + L4*Ocp^2*cos(Ob)*cos(Oc)^2 + L4*Ocp^2*cos(Ob)*sin(Oc)^2)/(2*(cos(Ob)*sin(Oc) - cos(Oc)*sin(Ob)));
(L2*cos(Oa)*Oap^2*cos(Ob) + L2*sin(Oa)*Oap^2*sin(Ob) + L3*Obp^2*cos(Ob)^2 + L3*Obp^2*sin(Ob)^2 + L4*cos(Oc)*Ocp^2*cos(Ob) + L4*sin(Oc)*Ocp^2*sin(Ob))/(L4*(cos(Ob)*sin(Oc) - cos(Oc)*sin(Ob)));
(L4*L5*Ocp^2*sin(B + Ob)*cos(Oc)^2 + L4*L5*Ocp^2*sin(B + Ob)*sin(Oc)^2 + L2*L5*Oap^2*sin(B + Ob)*cos(Oa)*cos(Oc) + L3*L5*Obp^2*cos(B + Ob)*cos(Ob)*sin(Oc) - L3*L5*Obp^2*cos(B + Ob)*cos(Oc)*sin(Ob) + L3*L5*Obp^2*sin(B + Ob)*cos(Ob)*cos(Oc) + L2*L5*Oap^2*sin(B + Ob)*sin(Oa)*sin(Oc) + L3*L5*Obp^2*sin(B + Ob)*sin(Ob)*sin(Oc) + 2*L2*L3*Oap^2*cos(Oa)*cos(Ob)*sin(Oc) - 2*L2*L3*Oap^2*cos(Oa)*cos(Oc)*sin(Ob))/(2*L3*(cos(Ob)*sin(Oc) - cos(Oc)*sin(Ob)));
-(L4*L5*Ocp^2*cos(B + Ob)*cos(Oc)^2 + L4*L5*Ocp^2*cos(B + Ob)*sin(Oc)^2 + L2*L5*Oap^2*cos(B + Ob)*cos(Oa)*cos(Oc) + L3*L5*Obp^2*cos(B + Ob)*cos(Ob)*cos(Oc) + L2*L5*Oap^2*cos(B + Ob)*sin(Oa)*sin(Oc) + L3*L5*Obp^2*cos(B + Ob)*sin(Ob)*sin(Oc) - L3*L5*Obp^2*sin(B + Ob)*cos(Ob)*sin(Oc) + L3*L5*Obp^2*sin(B + Ob)*cos(Oc)*sin(Ob) - 2*L2*L3*Oap^2*cos(Ob)*sin(Oa)*sin(Oc) + 2*L2*L3*Oap^2*cos(Oc)*sin(Oa)*sin(Ob))/(2*L3*(cos(Ob)*sin(Oc) - cos(Oc)*sin(Ob)))];
alm_ac(:,i)=ac;
end

Start another 'for' for plotting

for i=1:length(time)
    xa=alm_x(i,1);
    ya=alm_x(i,2);
    Oa=alm_x(i,3);
    xb=alm_x(i,4);
    yb=alm_x(i,5);
    Ob=alm_x(i,6);
    xc=alm_x(i,7);
    yc=alm_x(i,8);
    Oc=alm_x(i,9);
    xd=alm_x(i,10);
    yd=alm_x(i,11);
    subplot(121) % MBS plot
    cla % Clear axis
    title('MBS')
    hold on
        axis equal
   axis([-2 3 -1 4])
    % plot here
    P_Or=[0,0];
    P_A=[cos(Oa)*L2,sin(Oa)*L2];
    P_B=[cos(Ob)*L3+cos(Oa)*L2,sin(Ob)*L3+sin(Oa)*L2];
    P_C=[P_B(1)-cos(Oc-pi)*L4,P_B(2)-sin(Oc-pi)*L4];
    P_D=[xd,yd];
    puntos=[P_Or',P_A',P_B',P_C'];
    plot(puntos(1,:),puntos(2,:));

    al_xd(i)=xd;
    al_yd(i)=yd;
    plot(al_xd,al_yd);

    puntos_fill=[P_A',P_D',P_B'];
    fill(puntos_fill(1,:),puntos_fill(2,:),'b');
    hold off
    pause(0.0001)

  subplot(122)
  cla
  axis equal
  axis([-2 3 -1 4])
  title('Velocity and acceleration')
  hold on
  grid on
  xdv=alm_ve(10,i);
  ydv=alm_ve(11,i);
  quiver(xd,yd,xdv,ydv);
  plot(al_xd,al_yd);

  xda=alm_ac(10,i);
  yda=alm_ac(11,i);
  quiver(xd,yd,xda,yda);

%     xlabel('X')
%     ylabel('Y')

end
function F = pos(X,L1,L2,L3,L4,L5,B,W,Oao,t)
xa=X(1);
ya=X(2);
Oa=X(3);
xb=X(4);
yb=X(5);
Ob=X(6);
xc=X(7);
yc=X(8);
Oc=X(9);
xd=X(10);
yd=X(11);
F = [xa-(L2*cos(Oa))/2;
    ya-(L2*sin(Oa))/2;
    xb-xa-(L2*cos(Oa))/2-(L3*cos(Ob))/2;
    yb-ya-(L2*sin(Oa))/2-(L3*sin(Ob))/2;
    xc-xb-(L3*cos(Ob))/2-(L4*cos(Oc))/2;
    yc-yb-(L3*sin(Ob))/2-(L4*sin(Oc))/2;
    L1-xc-(L4*cos(Oc))/2;
   -yc-(L4*sin(Oc))/2;
   xd-xa-(L5*cos(B+Ob))/2-(L2*cos(Oa))/2;
   yd-ya-(L5*sin(B+Ob))/2-(L2*sin(Oa))/2;
   Oa-Oao-t*W];
end

Published with MATLAB® R2017b