create an animated two link manipulator using lagrangian mechanics for link and motor side dynamics in matlab in matlab

main.m
% Define the symbolic variables
syms m1 m2 l1 l2 q1 q2 q1_dot q2_dot q1_ddot q2_ddot 
syms g tau1 tau2

% Link lengths
l1 = 1;
l2 = 1;

% Masses of the links
m1 = 1;
m2 = 1;

% Lagrangian Equations for the two-link manipulator
T = 0.5*m1*(l1^2)*(q1_dot^2) + 0.5*m2*((l1^2)*(q1_dot^2) + (l2^2)*(q2_dot^2) + 2*l1*l2*q1_dot*q2_dot*cos(q1 - q2)) + 0.5*m2*(l1^2)*(q1_dot^2);
U = m1 * g * l1 * cos(q1) + m2 * g * (l1 * cos(q1) + l2 * cos(q2));

L = T - U;

% Motor Side Dynamics
tau1 = simplify(diff(diff(L, q1_dot), 't') - diff(L, q1));
tau2 = simplify(diff(diff(L, q2_dot), 't') - diff(L, q2));

% Define the state space representation
A = [0 0 1 0
     0 0 0 1
     0 g/l1 0 0
    -2*l1*l2*m2*q2_dot*sin(q1-q2)/(l1^2*l2*m2*(m1+2*m2)) -g/l2 0 0];
B = [0; 0; 1/(l1^2*m1+m2*(l1^2+l2^2+2*l1*l2*cos(q1-q2))); -2*cos(q1-q2)/(l2*l1^2*m2*(m1+2*m2))];

% State Space representation
C = eye(4);
D = zeros(4,1);

% Now you can use the State-Space represenation for your control implementation
979 chars
35 lines