How would you I write a matlab function for theta that represents the 2nd order differential of the torque equation?

The 2nd order differential equation for the torque is given by:

Matrices for D, C,G and F:

My code:

function torque = fcn(dq,q,T)

% length of the 2 robotic arms

l1 = 1;

l2=1;

r1=l1/2;

r2=l2/2;

% mass of the 2 robotic arms

m1= 5;

m2=5;

% moment of inertia of the 2 robotic arms

I1= (m1*l1^2)/3;

I2 = (m2*l2^2)/3;

p1= m1*r1^2+m2*l1^2+I1;

p2= m2*r2^2+ I2;

p3= m2*l1*r2;

p4= m1*r1+m2*l1;

p5= m2*r2;

% Gravitational strength constant

g = 9.81

D= [p1+p2+2p3*cos(q2) p2+p3*cos(q2); p2+p3*cos(q2) p2];

C= [-p3*dq2sin(q2) -p3(dq1+dq2)sin(q2); p3*dq1*sin(q2) 0];

G= [p4*g*

I am terribly stuck on writing the code for matrix D, C, and G. I don't understand how to express theta in differential terms. Can someone help me?

Answer 1

[Alan Stevens]

Does this help?

% Theta seems to have two components, so:
D(theta) = [p1+p2+2*p3*cos(theta(2))  p2+p3*cos(theta(2));
            p2+p3*cos(theta(2))  p2];

% For thetadot use omega, so, for example:
C(theta, omega) = [-p3*omega(2)*sin(theta(2))  -p3*(omega(1)+omega(2))*sin(theta(2));
                    p3*omega(1)*sin(theta(2))   0];
                
% Replace the 2nd order ODEs by two 1st order ODEs,e.g.something like:
% dthetadt = omega
% domegadt = D(theta)\(tau-F(omega)-G(theta)-C(theta,omega)*omega);