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!

3 comments:

  1. I am wondering if you have jansen mechanism static model build in matlab.

    ReplyDelete
  2. hola como estas , me gustaria saber si tienes la parte del guide, el codigo esta excelente. pero me faltaria esa parte del guide muchas gracias

    ReplyDelete
    Replies
    1. The complete Theo Jansen mechanism MatLAB code can be downloaded for free here: https://www.box.com/s/6ux2amtesurwrysbmdgj

      Delete

I'd love to hear from you!