In Augmented Formulation, the following matrix will be derived. The thing is how to formulate the equation as a function of first order differential equations, while q double dot is acceleration of generalized coordinate and lambda are constraint forces. My code is as below:
syms L1 L2 L3 L4 t1(t) t2(t) t3(t) t4(t) lambda1 lambda2
m1 = 1; m2 = 1; m3 = 3; L1 = 0.5; L2 = 0.076; L3 = 0.102; L4 = 0.127; T = 10; g = 9.81;
%Generalized Coordinate
q = [ t2; t3; t4];
%Lagrange Multiplier
lambda = [ lambda1; lambda2];
%Constraint Jacobian Matrix
Cq = [ -L2*sin(t2), -L3*sin(t3), -L4*sin(t4); L2*cos(t2), L3*cos(t3), L4*cos(t4)]; CqT = transpose(Cq);
%Mass Matrix
J1 = (1/12)*m1*power(L2,2); J2 = (1/12)*m2*power(L3,2); J3 = (1/12)*m3*power(L4,2); M = [ J1, 0, 0; 0, J2, 0; 0, 0, J3];
%Generalized Force
Qe = [ T-m1*g*(L2/2)*sin(t2)-m2*g*L2*sin(t2)-m3*g*L2*sin(t2), 0, 0; 0, -m2*g*(L3/2)*sin(t3)-m2*g*L3*sin(t3), 0; 0, 0, -m3*g*(L4/2)*sin(t4)]; Qd = [L2*cos(t2), L3*cos(t3), L4*cos(t4); L2*sin(t2), L3*sin(t3), L4*sin(t4)];
%Equation of Motion
B = [M, CqT; Cq, zeros(2)]; Q = [ Qe; Qd]; A1 = [ diff(q,2); lambda]; A2 = inv(B)*Q;
Best Answer