Appendix A: MATLAB Scripts
Note, all scripts are included as text below and as an .m file attachment
A1 - Main Script
%This script performs calculations for a Jansen walking mechanism
%If you run this script as-is, it will save 360 images and a video to your
%default MATLAB folder. If you want to run the script without saving those
%images remove lines 168, 239-256, 226-230.
close all
clear
clc
%%
%%Known Values
global A B C D E F G H I J K M %These global values get used in mechanismState.m
ao=38;
lo=7.8;
B=41.5;
C=39.3;
D=40.1;
E=55.8;
F=39.4;
G=36.7;
H=65.7;
I=59;
J=50;
K=61.9;
M=15;
%create a new diagonal link A from orthogonal links ao and lo
A=(ao^2+lo^2)^0.5;
ta=atan(lo/ao);
rotLength=360; %Max rotation of Link M for this sim
%%
%%Solve the two fourbars
L1=A;
L2=M;
for i=1:rotLength
thetaM(i)=i;
%solve first fourbar
theta2=180+thetaM(i)-rad2deg(ta); %rotate sub-mechanism so that it fits the fourbarpos.m parameters
L3=J;
L4=B;
t2=deg2rad(theta2);
[theta]=fourbarpos(L2,L3,L4,L1,0,t2,-1);
%rotate mechanism back and convert back to degrees
thetaJt(i)=rad2deg(theta(3)-pi+ta);
thetaBt(i)=rad2deg(theta(4)-pi+ta);
%solve second fourbar
theta2=thetaM(i)-rad2deg(ta)-180; %rotate sub-mechanism so that it fits the fourbarpos.m parameters
t2=deg2rad(theta2);
L3=K;
L4=C;
[theta]=fourbarpos(L2,L3,L4,L1,0,t2,1);
%rotate mechanism back and convert back to degrees
thetaKt(i)=rad2deg(pi+ta+theta(3));
thetaCt(i)=rad2deg(pi+ta+theta(4));
end
%set all angles to be between 0 and 360
[thetaB] = Range360Deg(thetaBt);
[thetaJ] = Range360Deg(thetaJt);
[thetaK] = Range360Deg(thetaKt);
[thetaC] = Range360Deg(thetaCt);
%%
%%Solve other links
%Define interior triangle angles for the two couplers
%BDE
ThetaTriD1=acosd((E^2+B^2-D^2)/(2*E*B));
ThetaTriB=asind(B/D*sind(ThetaTriD1));
ThetaTriE=asind(E/D*sind(ThetaTriD1));
%HIG
ThetaTriG=acosd((H^2+I^2-G^2)/(2*H*I));
ThetaTriH=asind(H/G*sind(ThetaTriG));
ThetaTriI=asind(I/G*sind(ThetaTriG));
for i=1:rotLength
%Solve Using Geometry of Triangle BDE
thetaD(i)=thetaB(i)+ThetaTriE;
thetaE(i)=thetaD(i)+ThetaTriB;
%Solve Using quadrilateral DCFG with long diagonal S
Dx=D*cosd(thetaD(i));
Dy=D*sind(thetaD(i));
Cx=C*cosd(thetaC(i));
Cy=C*sind(thetaC(i));
%Find values of "ghost link" S which connects point CIKG to DEF
Sx=Dx-Cx;
Sy=Dy-Cy;
temp=atand(Sy/Sx);
if temp<0 %theta S will be in Q1 or Q2, not Q4
thetaS=temp+180;
else
thetaS=temp;
end
S=(Sx^2+Sy^2)^0.5;
ThetaTriF=acosd((G^2+S^2-F^2)/(2*G*S));%Angle of triangle FGS that is opposite F
ThetaTriS2=asind(S/F*sind(ThetaTriF)); %Angle of triangle FGS that is opposite S
thetaG(i)=thetaS+ThetaTriF;
thetaF(i)=thetaG(i)-ThetaTriS2;
%Using Triangle HIG
thetaI(i)=thetaG(i)+ThetaTriH;
thetaH(i)=180+thetaG(i)-ThetaTriI;
end
%%
%%Create Plots
%Plot Angles vs ThetaM
figure
hold on
plot (thetaM,thetaB,'c-')
plot (thetaM,thetaC,'r-')
plot (thetaM,thetaD,'m--')
plot (thetaM,thetaE,'k--')
plot (thetaM,thetaF,'g--')
plot (thetaM,thetaG,'b--')
plot (thetaM,thetaH,'c-.')
plot (thetaM,thetaI,'m-.')
plot (thetaM,thetaJ,'b-')
plot (thetaM,thetaK,'y-')
xlabel('Link M Angle [deg]')
ylabel('Output Angles [deg]')
title('Mechanism Angles vs Input Angle (all measured from horizontal)')
legend('thetaB','thetaC','thetaD','thetaE','thetaF','thetaG','thetaH','thetaI','thetaJ','thetaK')
%Create Animation of Jansen Visualization
figure
figure
hold off
fig2=figure(2);
xlabel('x')
ylabel('y')
title('Jansen Mechanism Visualization')
axis([-80 80 -100 60])
hold on;
%acquire XY coordinates of all the points
[xMJK,yMJK,xBDC,yBDC,xBEJ,yBEJ,xDEF,yDEF,xCIKG,yCIKG,xM,yM,xFGH,yFGH,xHI,yHI] = ...
mechanismState(rad2deg(ta),thetaB(1),thetaC(1),thetaD(1),thetaE(1),thetaF(1),thetaG(1),thetaH(1),thetaI(1),thetaJ(1),thetaK(1),thetaM(1));
%Plot all the points
plotBDC=plot(xBDC,yBDC,'rx');
plotMm=plot(xM,yM,'bx');
plotMJK=plot(xMJK,yMJK,'o');
plotBEJ=plot(xBEJ,yBEJ,'o');
plotDEF=plot(xDEF,yDEF,'o');
plotCIKG=plot(xCIKG,yCIKG,'o');
plotFGH=plot(xFGH,yFGH,'o');
plotHI=plot(xHI,yHI,'o');
%Plot all the links
plotB=plot([xBDC xBEJ],[yBDC yBEJ],'-') ;%B
plotD=plot([xBDC xDEF],[yBDC yDEF],'-');%D
plotM=plot([xM xMJK],[yM yMJK],'-');%M
plotJ=plot([xMJK xBEJ],[yMJK yBEJ],'-');%J
plotE=plot([xDEF xBEJ],[yDEF yBEJ],'-') ;%E
plotC=plot([xBDC xCIKG],[yBDC yCIKG]);%C
plotK=plot([xMJK xCIKG],[yMJK yCIKG]);%K
plotF=plot([xDEF xFGH],[yDEF yFGH]);%F
plotG=plot([xCIKG xFGH],[yCIKG yFGH]);%G
plotH=plot([xFGH xHI],[yFGH yHI]);%H
plotI=plot([xHI xCIKG],[yHI yCIKG]);%I
hold off
images=cell(rotLength,1); %initialize matrix of images for video
for i=1:rotLength %repeat the previous plot for all values of ThetaM
[xMJK,yMJK,xBDC,yBDC,xBEJ,yBEJ,xDEF,yDEF,xCIKG,yCIKG,xM,yM,xFGH,yFGH,xHI,yHI] = ...
mechanismState(rad2deg(ta),thetaB(i),thetaC(i),thetaD(i),thetaE(i),thetaF(i),thetaG(i),thetaH(i),thetaI(i),thetaJ(i),thetaK(i),thetaM(i));
plotBDC=plot(xBDC,yBDC,'rx');
hold on
axis([-80 80 -100 60])
xlabel('X')
ylabel('Y')
title('Jansen Mechanism Visualization')
plotMm=plot(xM,yM,'bx');
plotMJK=plot(xMJK,yMJK,'o');
plotBEJ=plot(xBEJ,yBEJ,'o');
plotDEF=plot(xDEF,yDEF,'o');
plotCIKG=plot(xCIKG,yCIKG,'o');
plotFGH=plot(xFGH,yFGH,'o');
plotHI=plot(xHI,yHI,'o');
%Plot links and also calculate length of each link (for double checking
%work later)
plotB=plot([xBDC xBEJ],[yBDC yBEJ],'-'); %B
Bdist(i)=((xBDC-xBEJ)^2+(yBDC-yBEJ)^2)^0.5;
plotD=plot([xBDC xDEF],[yBDC yDEF],'-');%D
Ddist(i)=((xBDC-xDEF)^2+(yBDC-yDEF)^2)^0.5;
plotM=plot([xM xMJK],[yM yMJK],'-');%M
Mdist(i)=((xM-xMJK)^2+(yM-yMJK)^2)^0.5;
plotJ=plot([xMJK xBEJ],[yMJK yBEJ],'-');%J
Jdist(i)=((xMJK-xBEJ)^2+(yMJK-yBEJ)^2)^0.5;
plotE=plot([xDEF xBEJ],[yDEF yBEJ],'-'); %E
Edist(i)=((xDEF-xBEJ)^2+(yDEF-yBEJ)^2)^0.5;
plotC=plot([xBDC xCIKG],[yBDC yCIKG]);%C
Cdist(i)=((xBDC-xCIKG)^2+(yBDC-yCIKG)^2)^0.5;
plotK=plot([xMJK xCIKG],[yMJK yCIKG]);%K
Kdist(i)=((xMJK-xCIKG)^2+(yMJK-yCIKG)^2)^0.5;
plotF=plot([xDEF xFGH],[yDEF yFGH]);%F
Fdist(i)=((xDEF-xFGH)^2+(yDEF-yFGH)^2)^0.5;
plotG=plot([xCIKG xFGH],[yCIKG yFGH]);%G
Gdist(i)=((xCIKG-xFGH)^2+(yCIKG-yFGH)^2)^0.5;
plotH=plot([xFGH xHI],[yFGH yHI]);%H
Hdist(i)=((xFGH-xHI)^2+(yFGH-yHI)^2)^0.5;
plotI=plot([xHI xCIKG],[yHI yCIKG]);%I
Idist(i)=((xHI-xCIKG)^2+(yHI-yCIKG)^2)^0.5;
%create a vector of foot positions for the Trajectory plot
Xfoot(i)=xHI;
Yfoot(i)=yHI;
%save image for video
GraphFileName=join(['JansenPic',num2str(i)]);
GraphNamePNG=join([GraphFileName,'.png']);
saveas(gcf, GraphFileName, 'png') %save current plot
images{i}=imread(GraphNamePNG);%add current plot to image matrix
%If you want the animation to run smoothly/quickly in this code,
%remove the parts that update the video and set the pause value to be >0.07
%(see line 5)
pause(.08)
hold off
end
%Create video of all the saved images
% create the video writer with 1 fps
writerObj = VideoWriter('myVideo.avi');
writerObj.FrameRate = 1;
% set the seconds per image
secsPerImage(1:rotLength)=1;
% open the video writer
open(writerObj);
% write the frames to the video
for u=1:length(images)
% convert the image to a frame
frame = im2frame(images{u});
for v=1:secsPerImage(u)
writeVideo(writerObj, frame);
end
end
% close the writer object
close(writerObj);
%Plot Foot Trajectory
figure
plot(Xfoot,Yfoot)
xlabel('X Position')
ylabel('Y Position')
title('Trajectory of Foot Joint')
axis([-55 25 -110 -50])
axis square
%%
%%Double Check work by ensuring that links do not change lengths
errB=max(abs(Bdist-B));
errC=max(abs(Cdist-C));
errD=max(abs(Ddist-D));
errE=max(abs(Edist-E));
errF=max(abs(Fdist-F));
errG=max(abs(Gdist-G));
errH=max(abs(Hdist-H));
errI=max(abs(Idist-I));
errJ=max(abs(Jdist-J));
errK=max(abs(Kdist-K));
MaxError=max([errB,errC,errD,errE,errF,errG,errH,errI,errJ,errK]);
if MaxError>0.0001
disp('error, a link is changing length during animation')
disp(MaxError)
else
disp('Simulation successful')
end