dynamic model of a 7 link biped

walking_model.m

clear all
close all
clc
syms q1(t) q2(t) q3(t) q4(t) q5(t) q6(t) px py g
l = sym('l_%d', [1 7]);
m = sym('m_%d', [1 7]);
qd = sym('qd', [1 6]);
qdd = sym('qdd', [1 6]);
P = [px;py];

q = [q1(t) q2(t) q3(t) q4(t) q5(t) q6(t)];


for i=2:6
  
    R = [ cos(q(i-1)), -sin(q(i-1)); sin(q(i-1)), cos(q(i-1))];
    if i==2 || i==3
        temp = P(:,i-1) + R*[0;2*l(i-1)];
    elseif i==5 || i==4
        temp = P(:,i-1) + R*[0;-2*l(i-1)];
    elseif i==6
        temp = P(:,3) + R*[0;2*l(i-1)];
    end
    P = [P,temp];
    
end



for i=1:6
    
   
    if i < 5 
        temp = 0.5*(P(:,i) + P(:,i+1));
        Q(:,i)=temp
    elseif i==5
        temp = P(:,i);
        Q(:,i)=temp;
    elseif i==6
        R = [ cos(q(i-1)), -sin(q(i-1)); sin(q(i-1)), cos(q(i-1))];
        temp = P(:,3) + R*[0;l(i-1)];
        Q(:,i)=temp;
    end 
end
Q = [P(:,1),Q];


K = sym(zeros(1,6));
sP = sym(zeros(1,6));
for i=1:6
    Qdot = diff(Q(:,i+1),t);
    j = (1/12)*m(i+1)*(2*l(i+1))^2;
    K(i) = 0.5*m(i+1)*transpose(Qdot)*Qdot + 0.5*j*diff(q(i),t)^2;
    sP(i) = m(i+1)*[0,-g]*Q(:,i+1);
end

L = sum(sum(K)-sum(P));
tau = functionalDerivative(L, q);


CG =subs(tau, [ diff(q,t,2)], [0,0,0,0,0,0]); 
G = subs(CG, [ diff(q,t) ], [0,0,0,0,0,0]);
C = subs(tau-G, [ diff(q,t,2)], [0,0,0,0,0,0]);
M = tau - C - G;

M = subs(M,diff(q,t),qd);
M = subs(M,diff(q,t,2),qdd)
C = subs(C,diff(q,t),qd)
G


Q=subs(Q,l,[1,1,1,1,1,1,1]);
Q=subs(Q,[px,py],[0,0]);
Q=double(subs(Q,[q1(t) q2(t) q3(t) q4(t) q5(t) q6(t)],[deg2rad(-20) 0 0.25*pi 0 -0.25*pi 0]));
P=subs(P,l,[1,1,1,1,1,1,1]);
P=subs(P,[px,py],[0,0]);
P=double(subs(P,[q1(t) q2(t) q3(t) q4(t) q5(t) q6(t)],[deg2rad(-20) 0 0.25*pi 0 -0.25*pi 0]));




hold on
plot(P(1,1:5),P(2,1:5));
plot([P(1,3), P(1,6)] ,[P(2,3),P(2,6)] );

plot(Q(1,:),Q(2,:),'*');







% R1 = [ cos(q1(t)), -sin(q1(t)); sin(q1(t)), cos(q1(t))]
% R2 = [ cos(q2(t)), -sin(q2(t)); sin(q2(t)), cos(q2(t))]
% 
% P0 = [0;0];
% 
% l = 1;
% n = [0;2*l]
% 
% P1 = P0 + R1*n
% P2 = P1 + R2*n
% m = 1 ;
% j = 1;
% 
% K1 = 0.5*m* transpose(diff(P1,t))*diff(P1,t) + diff(q1(t),t)
% 
% K2 = 0.5*m* transpose(diff(P1,t))*diff(P2,t) + diff(q2(t),t)
% 
% L = K1+K2
% 
% functionalDerivative(L,[q1(t),q2(t)])
% 
%