the mass matrix and Jacobian matrix in ode15s
10 views (last 30 days)
Show older comments
Hi there, I have a set of odes in the form
where M and K are n*n matrixes q is a n*1 vector containing the generalised coordinates, and F is the n*1 generalised force vector.
I am using ode15s to solve this problem. Here I want to ask, are M and K the mass matrix and Jacobian matrix, respectively that should be input in odeset?
Any help is geatly appreciated.
Cheers
0 Comments
Accepted Answer
Sam Chak
on 14 Aug 2023
Hi @Tony Cheng
is the state-dependent mass matrix. Therefore, you can specify the mass matrix using the 'Mass' option of odeset(). However, is not a Jacobian matrix. By the way, please check if the highest degree of the ODE is or .
13 Comments
More Answers (2)
Sam Chak
on 15 Aug 2023
Hi @Tony Cheng
Following @Torsten's advice, perhaps showing you an example of obtaining the Jacobian matrix for a 2-DOF rigid robotic manipulator would be the best way for you to learn.
Rearranging the equation to obtain:
syms x1 x2 x3 x4 tau1 tau2
r1 = 1.0; % length of link 1
r2 = 0.8; % length of link 2
m1 = 1.0; % mass of link 1
m2 = 1.5; % mass of link 2
% State vector
x = [x1; % q1
x2; % q2
x3; % q1dot
x4]; % q2dot
% Mass matrix
M11 = (m1 + m2)*r1^2 + m2*r2^2 + 2*m2*r1*r2*cos(x2);
M12 = m2*r2^2 + m2*r1*r2*cos(x2);
M21 = M12;
M22 = m2*r2^2;
M = [M11 M12;
M21 M22]
% Coriolis matrix
C12 = m2*r1*sin(x2);
C = [-C12*x4 -C12*(x3+x4);
C12*x1 0]
% Gravity
G1 = (m1 + m2)*r1*cos(x2) + m2*r2*cos(x1 + x2);
G2 = m2*r2*cos(x1 + x2);
G = [G1;
G2]
% Control torque vector
T = [tau1;
tau2];
% Dynamics in state-space form, xdot = F(x, τ)
ddq = inv(M)*(T - C*[x3; x4] - G);
f1 = x3;
f2 = x4;
f3 = ddq(1);
f4 = ddq(2);
F = [f1;
f2;
f3;
f4];
% Jacobian matrix
J = jacobian(F, x)
7 Comments
Sam Chak
on 16 Aug 2023
@Bruno Luong, Thanks for the update on the Jacobian matrix for the Euler–Lagrange equations of motion in matrix form.
Walter Roberson
on 17 Aug 2023
Hi Torsten, thx so much for your valuable help! I get the definition of Jacobian matrix now!
Bruno Luong
on 16 Aug 2023
Edited: Bruno Luong
on 16 Aug 2023
This script tests the formula with dummy test configuration by comparing with the Jacobian obtained from finite difference:
runtest
function runtest
n = 3;
q = rand(n,1);
qd = rand(n,1);
hq = 1e-6;
hqd = 1e-6;
[y, M, K, C, F, dMdq, dKdq, dCdq, dCdqd, dFdq] = odefun(q, qd);
% Compute the Jacobian by finite difference
n = length(q);
Jdiff = zeros(2*n);
for j=1:n
ej = accumarray(j,1,[n 1]);
yq = odefun(q+hq*ej, qd);
yqd = odefun(q, qd+hqd*ej);
Jdiff(:,j) = (yq-y)/hq;
Jdiff(:,n+j) = (yqd-y)/hqd;
end
Jdiff
% Compute the Jacobian by the formula in the pdf paper
I = eye(n);
z = zeros(n);
qdd = -(M \ (K*q + C*qd - F));
Dq = odot(dMdq,qdd) + odot(dKdq,q) + odot(dCdq,qd) - diag(dFdq);
J = -[z, -I;
M\(K+Dq), M\(C+odot(dCdqd,qd))];
J
end
function AB = odot(A,B)
% Implement
% odot = @(A,B) A*kron(eye(width(A)/height(B)), B);
A = reshape(A, size(A,1), size(B,1), []);
AB = pagemtimes(A, B);
AB = reshape(AB, size(A,1), []);
end
%% Function that simulate the ODE state y = -dQ/dt = -[qdot; qdotdot]
function [y, M, K, C, F, dMdq, dKdq, dCdq, dCdqd, dFdq] = odefun(q, qd)
[M, dMdq] = Mfun(q);
[K, dKdq] = Kfun(q);
[C, dCdq, dCdqd] = Cfun(q, qd);
[F, dFdq] = Ffun(q);
I = eye(size(M));
z = zeros(size(M));
Mtilde = [I, z;
z M];
Ktilde = [z -I;
K C];
Q = [q; qd];
Ftilde = [z(:,1); F];
y = Mtilde \ (Ftilde-Ktilde*Q);
end
function A = setdiag(v)
n = length(v);
A = zeros(n,n,n);
for k=1:n
A(k,k,k) = v(k);
end
A = reshape(A, n, n*n);
end
%% functions that generate Mass, Stiffness, Coriolis and forcing
function [M, dMdq] = Mfun(q)
M = diag(1 + q.^2);
dMdq = setdiag(2.*q);
a = 0.1;
s = a*sum(q.^2);
M = M + s;
n = length(q);
dsdq = 2*a*q;
dsdq = repelem(reshape(dsdq,1,n),n,n);
dMdq = dMdq + dsdq;
end
%%
function [K, dKdq] = Kfun(q)
K = diag(1 + sqrt(q));
dKdq = setdiag((1/2)./sqrt(q));
a = 0.1;
s = a*sqrt(sum(q.^2));
K = K + s;
n = length(q);
dsdq = a*q./sqrt(sum(q.^2));
dsdq = repelem(reshape(dsdq,1,n),n,n);
dKdq = dKdq + dsdq;
end
%%
function [C, dCdq, dCdqd] = Cfun(q, qd)
C = 0*diag(sin(q).*cos(qd));
dCdq = 0*setdiag(cos(q).*cos(qd));
dCdqd = 0*setdiag(-sin(q).*sin(qd));
a = 0.1;
b = 1/4;
c = b*sum(qd.^2);
s = a*sin(c);
C = C + s;
n = length(q);
dsdq = 2*a*b*cos(c)*qd;
dsdq = repelem(reshape(dsdq,1,n),n,n);
dCdqd = dCdqd + dsdq;
end
%%
function [F, dFdq] = Ffun(q)
F = exp(2*q);
dFdq = 2*exp(2*q);
end
3 Comments
Bruno Luong
on 17 Aug 2023
Edited: Bruno Luong
on 17 Aug 2023
Normally the analytic formula is more accurate. In practice I do not think it makes a big difference between analytic formula and finite difference, unless if your ode system has sate vector with large dimension.
In robotics it has about 6 joints no?
It is just fun to derive the Jacobian for such system. Such formula might help to analyze where and when the system becomes singular. The final formula that has the same pattern as the ode, even the intermediate calculation seems messy and technical not easy.
But I'm curious to know if it helps ode15s to set 'Jacobian' in odeset.
See Also
Categories
Find more on Ordinary Differential Equations in Help Center and File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!