Pages

Showing posts with label Matlab. Show all posts
Showing posts with label Matlab. Show all posts

Tuesday, May 17, 2011

Theo Jansen Mechanism Matlab Code


I shared my m-code for the Theo Jansen mechanism awhile ago (LINK) but one of our readers recently created his own version of the program and offered to share it with us. Truong Duc Binh says:


"I programed for two legs and now I am making a computer interface with it, user can input parameters for Jansen walking machine. I divide this window form to two modes, you can simulate with constant alpha, or variable alpha."

Here is the complete m-code:

clear all

%Init link's length
L1=286;
L2=100;
L3=400;
L4=275;
L5=400;
L6=295;
L7=285;
L8=275;
L9=400;
L10=290;
L11=400;
L12=280;
L13=80;

alphaMax = pi/36; % 5 degree
alphaStep = pi/36/2; % 5/2 degree
alpha = -alphaStep;
k=3;

%To get the video:
mov=avifile('CoCau4KhauBanLe.avi','COMPRESSION','Cinepak');% None
for (a =1:1:(k+1))
    if a <= (k+1-rem(k+1,2))/2
        alpha = alpha + alphaStep;
    else
        alpha = alpha - alphaStep;
    end
    if alpha > alphaMax
        alpha = alphaMax;
    end
    if alpha<(-alphaMax)
        alpha = (-alphaMax);
    end
    for theta=0:pi/20:2*pi
    %for theta=0:(-pi/20):(-2*pi) % Quay cung chieu kim dong ho

    % Calculate angles:
    theta2 = pi - theta;
    theta3 = pi - theta + alpha;
    ssquared=(L2)^2+(L1)^2-(2*L1*L2*cos(theta3));
    s=sqrt(ssquared);
    beta=asin((L2*sin(theta3))/s);
    psi=acos(((L3^2)+(s^2)-(L4^2))/(2*L3*s));
    BCO2 = acos(((L5)^2+(L6)^2 - (L4)^2)/(2*L5*L6));
    CBO2 = acos(((L5)^2+(L4)^2 - (L6)^2)/(2*L5*L4));
    lambda=acos(((L4)^2+s^2-(L3)^2)/(2*(L4)*s));
    gamma = lambda - (BCO2 + CBO2)+ alpha + beta;
    O2AD = acos(((L9)^2+ssquared-(L8)^2)/(2*(L9)*s));
    omega = O2AD + alpha + beta;
    AO2D = acos(((L8)^2+ssquared-(L9)^2)/(2*(L8)*s));
    CO2D = pi - gamma - AO2D;
    CDsquared = (L6)^2+(L8)^2-2*L6*L8*cos(CO2D);
    CD = sqrt(CDsquared);
    O2CD = asin(L8*sin(CO2D)/CD);
    tempE = gamma - O2CD;
    CDE = acos(((L10)^2+CDsquared-(L7)^2)/(2*(L10)*CD));
    angleE = tempE + CDE;
    DEF = acos(((L10)^2+(L11)^2 - (L12)^2)/(2*L10*L11));
    tempF = DEF - angleE;
    DFE = acos(((L12)^2+(L11)^2 - (L10)^2)/(2*L12*L11));
    angleF = pi - tempF - DFE;

    %Find the points:
    O1=[0,0];
    O2=[-L1*cos(alpha),-L1*sin(alpha)];
    A=[L2*cos(theta) L2*sin(theta)];
    B=[A(1)-L3*cos(psi-alpha-beta) A(2)+L3*sin(psi-alpha-beta)];
    C=[O2(1)-L6*cos(gamma) O2(2)-L6*sin(gamma)];
    D=[A(1)-L9*cos(omega) A(2)-L9*sin(omega)];
    E=[D(1)-L10*cos(angleE) D(2)-L10*sin(angleE)];
    F=[D(1)-L12*cos(angleF) D(2)-L12*sin(angleF)];
    G=[F(1)-L13*cos(angleF) F(2)-L13*sin(angleF)];

    %************************LEFT********************************
    plot ([O1(1),A(1)],[O1(2),A(2)],'g','linewidth',3)
    hold on
    plot ([O2(1),O1(1)],[O2(2),O1(2)],'black','linewidth',3)
    plot ([A(1),B(1)],[A(2),B(2)],'b','linewidth',3)
    plot ([O2(1),B(1)],[O2(2),B(2)],'r','linewidth',3)
    plot ([O2(1),C(1)],[O2(2),C(2)],'r','linewidth',3)
    plot ([B(1),C(1)],[B(2),C(2)],'r','linewidth',3)
    plot ([O2(1),D(1)],[O2(2),D(2)],'b','linewidth',3)
    plot ([A(1),D(1)],[A(2),D(2)],'b','linewidth',3)
    plot ([C(1),E(1)],[C(2),E(2)],'b','linewidth',3)
    plot ([D(1),E(1)],[D(2),E(2)],'r','linewidth',3)
    plot ([D(1),F(1)],[D(2),F(2)],'r','linewidth',3)
    plot ([E(1),F(1)],[E(2),F(2)],'r','linewidth',3)
    plot ([G(1),F(1)],[G(2),F(2)],'b','linewidth',3)
    Gx(a)=G(1);
    Gy(a)=G(2);

    %*************************RIGHT******************************
    theta4 = theta + alpha;
    s2squared=(L2)^2+(L1)^2-(2*L1*L2*cos(theta4));
    s2=sqrt(s2squared);
    beta2=asin((L2*sin(theta4))/s2);
    angle2=acos(((L3^2)+((s2)^2)-(L4^2))/(2*L3*(s2)));
    B1C1O3 = acos(((L5)^2+(L6)^2 - (L4)^2)/(2*L5*L6));
    C1B1O3 = acos(((L5)^2+(L4)^2 - (L6)^2)/(2*L5*L4));
    lambda2=acos(((L4)^2+(s2)^2-(L3)^2)/(2*(L4)*(s2)));
    gamma2 = lambda2 - (B1C1O3 + C1B1O3)+ alpha + beta2;
    O3A1D1 = acos(((L9)^2+s2squared-(L8)^2)/(2*(L9)*(s2)));
    omega2 = O3A1D1 + alpha + beta2;
    A1O3D1 = acos(((L8)^2+s2squared-(L9)^2)/(2*(L8)*(s2)));
    C1O3D1 = 2*pi - A1O3D1 - lambda2 - (pi - B1C1O3 - C1B1O3);
    C1D1squared = (L6)^2+(L8)^2-2*L6*L8*cos(C1O3D1);
    C1D1 = sqrt(C1D1squared);
    O3C1D1 = asin(L8*sin(C1O3D1)/(C1D1));
    tempE2 = gamma2 - O3C1D1;
    C1D1E1 = acos(((L10)^2+C1D1squared-(L7)^2)/(2*(L10)*(C1D1)));
    angleE2 = tempE2 + C1D1E1;
    D1E1F1 = acos(((L10)^2+(L11)^2 - (L12)^2)/(2*L10*L11));
    tempF2 = D1E1F1 - angleE2;
    D1F1E1 = acos(((L12)^2+(L11)^2 - (L10)^2)/(2*L12*L11));
    angleF2 = tempF2 + D1F1E1;

    %Find the points:
    O3=[L1*cos(alpha),-L1*sin(alpha)];%
    B1=[A(1)+L3*cos(angle2-alpha-beta2) A(2)+L3*sin(angle2-alpha-beta2)];%
    C1=[O3(1)+L6*cos(gamma2) O3(2)-L6*sin(gamma2)];
    D1=[A(1)+L9*cos(omega2) A(2)-L9*sin(omega2)];
    E1=[D1(1)+L10*cos(angleE2) D1(2)-L10*sin(angleE2)];
    F1=[D1(1)-L12*cos(angleF2) D1(2)-L12*sin(angleF2)];
    G1=[F1(1)-L13*cos(angleF2) F1(2)-L13*sin(angleF2)];

    plot ([O3(1),O1(1)],[O3(2),O1(2)],'black','linewidth',3)
    plot ([A(1),B1(1)],[A(2),B1(2)],'b','linewidth',3)
    plot ([O3(1),B1(1)],[O3(2),B1(2)],'r','linewidth',3)
    plot ([O3(1),C1(1)],[O3(2),C1(2)],'r','linewidth',3)
    plot ([B1(1),C1(1)],[B1(2),C1(2)],'r','linewidth',3)
    plot ([O3(1),D1(1)],[O3(2),D1(2)],'b','linewidth',3)
    plot ([A(1),D1(1)],[A(2),D1(2)],'b','linewidth',3)
    plot ([C1(1),E1(1)],[C1(2),E1(2)],'b','linewidth',3)
    plot ([D1(1),E1(1)],[D1(2),E1(2)],'r','linewidth',3)
    plot ([D1(1),F1(1)],[D1(2),F1(2)],'r','linewidth',3)
    plot ([E1(1),F1(1)],[E1(2),F1(2)],'r','linewidth',3)
    plot ([G1(1),F1(1)],[G1(2),F1(2)],'b','linewidth',3)
    G1x(a)=G1(1);
    G1y(a)=G1(2);
   
    plot(Gx,Gy)
    plot(G1x,G1y)
  
    %************************************************************
    axis([-800 800 -800 400])
    hold off
    a=a+1;
    pause(.01)
    M=getframe;
    mov=addframe(mov,M);
    end
end
mov=close(mov);


Thanks again to Truong Duc Binh!

Monday, March 14, 2011

Theo Jansen Mechanism Matlab Code M-file

 A few readers have requested the full m file for the Theo Jansen mechanism. I've uploaded it here. If the download won't work for you lease me your email address and I will gladly send it to you. Please don't just copy the file. This is meant for reference only. In other words, figure it out on your own!



http://cadsystemshelp.blogspot.com/2010/04/theo-jansen-mechanism-matlab-code.html

Theo Jansen Mechanism Matlab Code.doc download 

*7/22/2011 download link should be fixed now!

Wednesday, April 21, 2010

Theo Jansen Mechanism Matlab Code


As promised, here is some of the Matlab code for the Theo Jansen Mechanism we posted a video of a little while back. We begin every Matlab program with the clear all command. Next are the lengths for all of the linkages.

clear all

%Link Lengths

L1=286;

L2=120;

L3=400;

L4=275;

L5=400;

L6=290;

L7=275;

L8=275;

L9=400;

L10=290;

L11=400;

L12=275;

L13=170;


 

%To get the video output:

a=1;

mov=avifile('TheoJansen.avi','COMPRESSION','None');

for theta=0:pi/20:8*pi


 

%Relating all values to theta2 of 4-bar mechanism O2ABO1

theta2=pi-theta;

ssquared=(L2)^2+(L1)^2-(2*L1*L2*cos(theta2));

s=sqrt(ssquared);

beta=asin((L2*sin(theta2))/s);

psi=acos(((L3^2)+(s^2)-(L4^2))/(2*L3*s));

lambda=acos((L4^2+s^2-L3^2)/(2*L4*s));

theta3=pi-psi+beta;

theta4=pi+beta+lambda;

theta5=lambda+beta;


 

%Finding the points relative to the motion of theta2

O1=[0,0];

O2=[-L1,0];

A=[L2*cos(theta) L2*sin(theta)];

B=[A(1)-L3*cos(psi-beta) A(2)+L3*sin(psi-beta)];


 


%Plotting the 4-bar mechanism


plot ([O1(1),A(1)],[O1(2),A(2)],'r','linewidth',3)

hold on

plot ([O2(1),O1(1)],[O2(2),O1(2)],'black','linewidth',3)

plot ([A(1),B(1)],[A(2),B(2)],'g','linewidth',3)

plot ([O2(1),B(1)],[O2(2),B(2)],'r','linewidth',3)

%Bottom triangle DEF

F=[E(1)-L12*cos(lambda+beta) E(2)-L12*sin(lambda+beta)];


 

plot([D(1),F(1)],[D(2),F(2)],'linewidth',3)

plot([E(1),F(1)],[E(2),F(2)],'linewidth',3)


 


%Link FG


G=[F(1)-L13*cos(lambda+beta) F(2)-L13*sin(lambda+beta)];


 


%Trajectory of point G


plot([F(1),G(1)],[F(2),G(2)],'linewidth',3)

Gx(a)=G(1);

Gy(a)=G(2);

plot(Gx,Gy)


 

axis([-800 400 -800 400])

hold off

a=a+1;

pause(.1)

M=getframe;

mov=addframe(mov,M);

end

mov=close(mov);


 

I left out a portion in the middle so you'll just have to figure that out by yourself!


 




Bookmark and Share

Wednesday, March 10, 2010

Theo Jansen Mechanism drawn in Matlab

For one on my classes this quarter we had to draw the Theo Jansen 12 bar mechanism in Matlab. To do this we had to calculate each angle based on the input angle which in the video is the red link connected to the black base link. This took over 100 lines of code in Matlab which took forever to figure out but in the end it was worth it. There will be more projects to come in the future. Feel free to comment if you have any questions or want to see my .m file for this!