# Matrixform ndsolve for mobile robot dynamic equations

Error is:

NDSolve::ndsdtc: The time constraint of 1. seconds was exceeded trying to solve for derivatives, so the system will be treated as a system of differential-algebraic equations. You can use Method->{"EquationSimplification"->"Solve"} to have the system solved as ordinary differential equations

and

NDSolve::nlnum: "The function value

ClearAll[xb, yb, θb, θ1, d2, d3];
m1 = 1;
m2 = 1;
m3 = 1;
lc2 = 1;
lc3 = 1;
l1 = 2;
l2 = 2;

Ix1 = 1;
Iy1 = 1;
Iz1 = 1;
Ix2 = 1;
Iy2 = 1;
Iz2 = 1;
Ix3 = 1;
Iy3 = 1;
Iz3 = 1;

g = 9.81;

b1 = 1;
b2 = 2;
b3 = 1;
b4 = 2;
b5 = 1;
b6 = 2;

c1 = 0;
c2 = 0;
c3 = 0;
c4 = 0;
c5 = 0;
c6 = 0;
M11[t_] := m1 + m2 + m3;
M12[t_] := 0;
M13[t_] := (m2*lc2  *
Cos[θ1[t]] - (m2 + m3) d2[t] Cos[θ1[t]] +
m3*lc3  Sin[θ1[t]] -
m3 d3[t] Sin[θ1[t]]) Sin[θb[t]];
M14[t_] := (-lc3 m3 Cos[θ1[t]] + m3* d3[t]* Cos[θ1[t]] +
m2 lc2  Sin[θ1[t]] - (m2 + m3) d2[
t] Sin[θ1[t]]) Cos[θb[t]];
M15[t_] := (m2 + m3) Cos[θ1[t]] Cos[θb[t]];
M16[t_] := m3 Cos[θb[t]] Sin[θ1[t]];
M21[t_] := 0;
M22[t_] := m1 + m2 + m3;
M23[t_] := (-m2*
lc2  Cos[θ1[t]] + (m2 + m3) d2[t] Cos[θ1[t]] -
m3*lc3  Sin[θ1[t]] +
m3 d3[t] Sin[θ1[t]]) Cos[θb[t]];
M24[t_] := (-lc3 m3 Cos[θ1[t]] + m3 Cos[θ1[t]] d3[t] +
lc2 m2 Sin[θ1[t]] - (m2 + m3) d2[
t] Sin[θ1[t]]) Sin[θb[t]];
M25[t_] := (m2 + m3) Cos[θ1[t]] Sin[θb[t]];
M26[t_] := m3 Sin[θ1[t]] Sin[θb[t]];
M31[t_] := (m2 lc2 Cos[θ1[t]] - (m2 + m3) d2[
t] Cos[θ1[t]] + m3 lc3 Sin[θ1[t]] -
m3 d3[t] Sin[θ1[t]]) Sin[θb[t]];
M32[t_] :=
Cos[θb[
t]] (-lc2 m2 Cos[θ1[t]] + (m2 + m3) Cos[θ1[t]] d2[
t] - lc3 m3 Sin[θ1[t]] + m3 d3[t] Sin[θ1[t]]);
M33[t_] :=
Iz1 + Iy2 Cos[θ1[t]]^2 + Iz3 Cos[θ1[t]]^2 +
m2 Cos[θ1[t]]^2 (lc2 - d2[t])^2 +
m3 (Cos[θ1[t]] d2[t] + (-lc3 + d3[t]) Sin[θ1[t]])^2;
M34[t_] := 0;
M35[t_] := 0;
M36[t_] :=
m3 (-Cos[θb[t]] + Cos[θb[t]]) Sin[θ1[
t]] (Cos[θ1[t]] d2[
t] + (-lc3 + d3[t]) Sin[θ1[t]]) Sin[θb[t]];
M41[t_] :=
Cos[θb[t]] (-lc3 m3 Cos[θ1[t]] +
m3 Cos[θ1[t]] d3[t] +
lc2 m2 Sin[θ1[t]] - (m2 + m3) d2[t] Sin[θ1[t]]);
M42[t_] := (-lc3 m3 Cos[θ1[t]] + m3 Cos[θ1[t]] d3[t] +
lc2 m2 Sin[θ1[t]] - (m2 + m3) d2[
t] Sin[θ1[t]]) Sin[θb[t]];
M43[t_] := 0;
M44[t_] :=
Iy3 + Iz2 + lc2^2 m2 + lc3^2 m3 -
2 lc2 m2 d2[t] + (m2 + m3) d2[t]^2 - 2 lc3 m3 d3[t] + m3 d3[t]^2;
M45[t_] := m3 (-lc3 + d3[t]);
M46[t_] :=
m3 (Cos[θ1[t]] Cos[θb[t]] (-Cos[θb[t]] +
Cos[θb[t]]) (lc3 - d3[t]) Sin[θ1[t]] -
d2[t] (Cos[θ1[t]]^2 +
Sin[θ1[t]]^2 (Cos[θb[t]] Cos[θb[t]] +
Sin[θb[t]]^2)));
M51[t_] := (m2 + m3) Cos[θ1[t]] Cos[θb[t]];
M52[t_] := (m2 + m3) Cos[θ1[t]] Sin[θb[t]];
M53[t_] := 0;
M54[t_] := m3 (-lc3 + d3[t]);
M55[t_] := m2 + m3;
M56[t_] :=
m3 Cos[θ1[t]] (Cos[θb[t]] -
Cos[θb[t]]) Cos[θb[t]] Sin[θ1[t]];
M61[t_] := m3 Cos[θb[t]] Sin[θ1[t]];
M62[t_] := m3 Sin[θ1[t]] Sin[θb[t]];
M63[t_] :=
m3 (-Cos[θb[t]] + Cos[θb[t]]) Sin[θ1[
t]] (Cos[θ1[t]] d2[
t] + (-lc3 + d3[t]) Sin[θ1[t]]) Sin[θb[t]];
M64[t_] :=
m3 (Cos[θ1[t]] Cos[θb[t]] (-Cos[θb[t]] +
Cos[θb[t]]) (lc3 - d3[t]) Sin[θ1[t]] -
d2[t] (Cos[θ1[t]]^2 +
Sin[θ1[t]]^2 (Cos[θb[t]] Cos[θb[t]] +
Sin[θb[t]]^2)));
M65[t_] :=
m3 Cos[θ1[t]] (Cos[θb[t]] -
Cos[θb[t]]) Cos[θb[t]] Sin[θ1[t]];
M66[t_] :=
m3 (Cos[θ1[t]]^2 +
Sin[θ1[t]]^2 (Cos[θb[t]]^2 + Sin[θb[t]]^2));

M = ({
{M11[t], M12[t], M13[t], M14[t], M15[t], M16[t]},
{M21[t], M22[t], M23[t], M24[t], M25[t], M26[t]},
{M31[t], M32[t], M33[t], M34[t], M35[t], M36[t]},
{M41[t], M42[t], M43[t], M44[t], M45[t], M46[t]},
{M51[t], M52[t], M53[t], M54[t], M55[t], M56[t]},
{M61[t], M62[t], M63[t], M64[t], M65[t], M66[t]}
});

V1[t_] := 0;
V2[t_] := 0;
V3[t_] := 0;
V4[t_] := 0;
V5[t_] := 0;
V6[t_] := 0;

F1[t_] := 0;
F2[t_] := 0;
τ3[t_] := 0;
τ4[t_] := 0;
F5[t_] := 0;
F6[t_] := 0;

u1[t_] := F1[t] - b1*xb'[t] - c1*Sign[xb'[t]];
u2[t_] := F2[t] - b2*yb'[t] - c2*Sign[yb'[t]];
u3[t_] := τ3[t] - b3*θb'[t] - c3*Sign[θb'[t]];
u4[t_] := τ4[t] - b4*θ1'[t] - c4*Sign[θ1'[t]];
u5[t_] := F5[t] - b5*d2'[t] - c5*Sign[d2'[t]];
u6[t_] := F6[t] - b6*d3'[t] - c6*Sign[d3'[t]];

V = {{0}, {0}, {0}, {0}, {0}, {0}};
G = {{1}, {1}, {1}, {1}, {1}, {1}};

initu = q[0] == Flatten@{{0}, {0}, {0}, {0}, {1}, {1}};(* <---N.B.*)
initv = q'[0] == Flatten@{{0}, {0}, {0}, {0}, {0}, {0}};(* <---N.B.*)

q[t_] := {xb[t], yb[t], θb[t], θ1[t], d2[t], d3[t]};

sol = First@
M.q''[t] + V + G == {u1[t], u2[t], u3[t], u4[t], u5[t], u6[t]}],
initu, initv}, q[t], {t, 0, 100}];

Plot[{xb[t] /. sol,
yb[t] /. sol, θb[t] /. sol, θ1[t] /. sol,
d2[t] /. sol, d3[t] /. sol}, {t, 0, 100}]

• Your code will run faster and, perhaps, be easier to debug, if you do not use SetDelayed so much. Commented Jul 17, 2015 at 1:39

You shouldn't post such a code. Please isolate your problematic chunk first. Anyway.

q[t_] := {xb[t], yb[t], θb[t], θ1[t], d2[t], d3[t]};
initu = Thread[q[0] == {0, 0, 0, 0, 1, 1}];
initv = Thread[q'[0] == {0, 0, 0, 0, 0, 0}];

jj = Join @@ ({Thread[M.q''[t] + V + G == {u1[t], u2[t], u3[t], u4[t], u5[t], u6[t]}],
initu, initv}) /. {x_} == y_ :> x == y;
sol = First@NDSolve[jj, q[t], {t, 0, 100},
Method -> {"EquationSimplification" -> "Residual"}];

Plot[q[t] /. sol, {t, 0, 100}, Evaluated -> True]


Plot[q[t] /. sol, {t, 0, 10}, Evaluated -> True]


• Using your code plus the definitions in the question, I still get the error message. However, if I Solve jj for the second derivatives and insert the results into NDSolve, I obtain the results in your answer, whether or not I use Method -> {"EquationSimplification" -> "Residual"}. Is it possible that you have omitted something from your answer? Commented Jul 17, 2015 at 4:08
• @bbgodfrey I had two lines permuted. Please ClearAll and try again. Thanks! Commented Jul 17, 2015 at 4:25
• @bbgodfrey Please check that you use the OP's code only till the G definition (including it) Commented Jul 17, 2015 at 4:36
• If one of c1,c2,...c6 be nonzero an error occurs !!! NDSolve::nderr: Error test failure at t == 0.; unable to continue. why? how can i fix it? i think its for sign function and discontinuity of sign function!!! if c be zero threre is not any error Commented Jul 17, 2015 at 9:28
• @Belisarius The computation with your correction works for me now. Thanks. Commented Jul 17, 2015 at 13:39

The following works both for the original question and for the case described in the comment about setting one of c1,c2,...c6 to be nonzero. Note that the error reported by the OP, NDSolve::nderr, does not have very helpful documentation, and the suggestion to use "StiffnessSwitching" in this case leads to another error, NDSolve::nodae, which is unexpected, because NDSolve is not being asked to solve a system of differential-algebraic equations.

Because the root cause of all these errors appears to be that NDSolve has difficulty solving algebraically for the second derivatives of the dependent variables, we help it by replacing the NDSolve line in the question by

Thread[ M.q''[t] + V + G == {u1[t], u2[t], u3[t], u4[t], u5[t], u6[t]}];
Simplify[#] & /@ %;
eqs = (Solve[%, {xb''[t], yb''[t], θb''[t], θ1''[t], d2''[t], d3''[t]}]
// Flatten) /. Rule -> Equal;
sol = First@NDSolve[{eqs, initu, initv}, q[t], {t, 0, 100}];


For c1 = 1 it gives curves similar to those in the answer by Belisarius (which addressed c1 = 0). For c6 = 1 instead, it gives modestly different results.

Incidentally, the Simplify above is unnecessary but does reduce a bit the enormous size of eqs.