How to efficiently solve a system of 14 differential equations?

Question

Seonghoon Noh 2018-11-16

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/430347-how-to-efficiently-solve-a-system-of-14-differential-equations

clear all;
clc;
syms q1 q2 q3 q4 q5 q6 q7;
%system parameters
m_link = 1;         %mass of a link in kg
l_link = 0.7;       %legth of a link in m
rho = 3000;         %density of the link in kg/m^3
I = rho*0.1*0.1*l_link*(l_link.^2 + 0.1.^2)/12;     % MoI about the axis @ the centroid and perp. to the link
g = 9.81;            %gravity in m/s^2
%generalized coordinates
%x_body, y_body, theta_body, theta_fs, theta_fk, theta_rs, theta_rk
q = [q1; q2; q3; q4; q5; q6; q7];
n= size(q,1);
assume(q, 'real')
%rotation matrix from body link frame to inertial world frame
R = [cos(q3) -sin(q3) 0;
     sin(q3) cos(q3) 0;
     0 0 1];
%3x1 position vectors of the CoM of the 5 links w.r.t. inertial world frame
p_1 = [q(1); q(2); 0];
p_2 = p_1 + 0.5*l_link*[cos(q3); sin(q3); 0] ...
      + R*((l_link/2)*[cos(q4); sin(q4); 0]);
p_3 = p_1 + 0.5*l_link*[cos(q3); sin(q3); 0] ...
      + R*((l_link)*[cos(q4); sin(q4); 0])...
      + R*((l_link/2)*[cos(q5); sin(q5); 0]);
p_4 = p_1 - 0.5*l_link*[cos(q3); sin(q3); 0] ...
      + R*((l_link/2)*[cos(q6); sin(q6); 0]);
p_5 = p_1 - 0.5*l_link*[cos(q3); sin(q3); 0] ...
      + R*((l_link)*[cos(q6); sin(q6); 0])...
      + R*((l_link/2)*[cos(q7); sin(q7); 0]);
ff = p_1 + 0.5*l_link*[cos(q3); sin(q3); 0] ...
      + R*((l_link)*[cos(q4); sin(q4); 0])...
      + R*((l_link)*[cos(q5); sin(q5); 0]);
rf = p_1 - 0.5*l_link*[cos(q3); sin(q3); 0] ...
      + R*((l_link)*[cos(q6); sin(q6); 0])...
      + R*((l_link)*[cos(q7); sin(q7); 0]);
J_ff_pitch = simplify(diff(ff,q3));
J_ff_pitch = J_ff_pitch(1:2);
J_rf_pitch = simplify(diff(rf,q3));
J_rf_pitch = J_rf_pitch(1:2);
J_planar_elbow = [-l_link*sin(q4) - l_link*sin(q4+q5), -l_link*sin(q4+q5);
                  l_link*cos(q4) + l_link*cos(q4+q5), l_link*cos(q4+q5)];
J_robot = [eye(2), J_ff_pitch, J_planar_elbow, zeros(2,2);
           eye(2), J_rf_pitch, zeros(2,2), J_planar_elbow];
  
%linear velocity Jacobian
J_v1 = simplify(jacobian(p_1, q));
J_v2 = simplify(jacobian(p_2, q));
J_v3 = simplify(jacobian(p_3, q));
J_v4 = simplify(jacobian(p_4, q));
J_v5 = simplify(jacobian(p_5, q));
diagonal = [0 0 I I I I I];
I_r = diag(diagonal);
%D(q) Inertial matrix (symmetric positive definite)
D = simplify(m_link*(J_v1'*J_v1 + J_v2'*J_v2 + J_v3'*J_v3 + J_v4'*J_v4) + I_r);
%placeholder variables for derivatives
syms q1_dot q2_dot q3_dot q4_dot q5_dot q6_dot q_7_dot
q_dot = [q1_dot; q2_dot; q3_dot; q4_dot; q5_dot; q6_dot; q_7_dot];
% Christoffel symbols
c = sym(zeros(7, 7, 7));
% C(q, q_dot) Coriolis matrix
C = sym(zeros(7, 7));
for k = 1:n
    for j = 1:n
        for i = 1:n
            c(i, j, k) = 0.5*(diff(D(k,j), q(i)) + diff(D(k,i), q(j))- ...
                         diff(D(i,j), q(k)));
        end
    end
end
for k = 1:n
    for j = 1:n
        for i = 1:n
            C(k, j) = C(k, j) + c(i,j,k)*q_dot(i);
        end
    end
end
% gravity vector
G = sym(zeros(n, 1));
% scalar potential energy
P = m_link*(p_1(2)+p_2(2)+p_3(2)+p_4(2)+p_5(2))*g;
for i = 1:n
    G(i) = diff(P, q(i));
end
%realize the time-dependency of q and substitute them in
syms q1(t) q2(t) q3(t) q4(t) q5(t) q6(t) q7(t)
dqdt = [diff(q1); diff(q2); diff(q3); diff(q4); diff(q5); diff(q6); diff(q7)];
d2qdt2 = [diff(q1,2); diff(q2,2); diff(q3,2); diff(q4,2); diff(q5,2); diff(q6,2); diff(q7,2)];
D = subs(D, q, [q1; q2; q3; q4; q5; q6; q7]);
C = subs(C, q, [q1; q2; q3; q4; q5; q6; q7]);
C = subs(C, q_dot, [diff(q1); diff(q2); diff(q3); diff(q4); diff(q5); diff(q6); diff(q7)]);
G = subs(G, q, [q1; q2; q3; q4; q5; q6; q7]);
J_robot = subs(J_robot, q, [q1; q2; q3; q4; q5; q6; q7]);
%joint input torque (u; control)
tau = zeros(7,1);
syms F_fx F_fy F_rx F_ry
lambda = [F_fx; F_fy; F_rx; F_ry];
EoM = D*d2qdt2 + C*dqdt + G == tau + J_robot.'*lambda;
odes = EoM;
%convert it to a system of 1st order differential equations
[V,s] = odeToVectorField(odes);
%rearrange the equations and variables
V = [V(3); V(4); V(11); V(12); V(1); V(2); V(5); V(6); V(7); V(8); V(9); V(10); V(13); V(14)];
s = [s(3); s(4); s(11); s(12); s(1); s(2); s(5); s(6); s(7); s(8); s(9); s(10); s(13); s(14)]
M = matlabFunction(V,'vars', {'t','Y', 'lambda'});
sol = ode45(M,[0 10],[2 0 3 0 0.1 0 -3*pi/4 0 -pi/4 0 -3*pi/4 0 -pi/4 0]);

I am trying to model a planar walking robot, and I have a system of 2nd order, nonlinear differential equations. I read on a MATLAB Answers post that "odeToVectorField" can convert my 2nd order system to a 1st order system, so I can solve it using differential equation solvers like ode45. However, I'm running into the error: "unable to meet integration tolerances". Even using a solver like ode15s runs into the same error. Is there a problem in my formatting of the equations that is causing this to happen? Is it not possible to solve certain differential equations in MATLAB?