How to efficiently solve a system of 14 differential equations?

2 次查看(过去 30 天)
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?

回答(0 个)

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by