I have some problems with NDSolve, my ODE is about Dynamics of Robot mechanism, and my BVP cannot be solved by NDSolve

Question

Here are the parameters which are given in the task

g = 9.81;
h = 0.009;
m1 = 4.5;
m2 = 4.25;
m3 = 3.3;
L2 = 1.2;
L3 = 0.8;
\[Theta]CoM1 = 0.2;

Matrixes of the transforms

T01 = ({
{1, 0, 0, q1[t]},
{0, 1, 0, 0},
{0, 0, 1, 0},
{0, 0, 0, 1}
});

T12 = ({
{Cos[q2[t]], -Sin[q2[t]], 0, 0},
{Sin[q2[t]], Cos[q2[t]], 0, 0},
{0, 0, 1, 0},
{0, 0, 0, 1}
});

T23 = ({
{Cos[q3[t]], -Sin[q3[t]], 0, L2},
{Sin[q3[t]], Cos[q3[t]], 0, 0},
{0, 0, 1, 0},
{0, 0, 0, 1}
});

T02 = T01.T12;

T03 = T02.T23;

Total cebtre point

r3TCP = ({
{L3},
{0},
{0},
{1}
});

centrals of masses

r1CoM1 = ({
{0},
{0},
{0},
{1}
});
r2CoM2 = ({
{L2/2},
{0},
{0},
{1}
});
r3CoM3 = ({
{L3/2},
{0},
{0},
{1}
});
r0TCP = T03.r3TCP ;
r0CoM1 = T01.r1CoM1 ;
r0CoM2 = T02.r2CoM2 ;
r0CoM3 = T03.r3CoM3 ;

velocities

vCoM1 = D[r0CoM1, t];  // Simplify
vCoM2 = D[r0CoM2, t];  // Simplify
vCoM3 = D[r0CoM3, t];  // Simplify

\[Phi]1 = 0;
\[Phi]2 = \[Phi]1 + q2[t];
\[Phi]3 = \[Phi]2 + q3[t];
\[Omega]1 = D[\[Phi]1, t];
\[Omega]2 = D[\[Phi]2, t];
\[Omega]3 = D[\[Phi]3, t];

given initial parameters

qD1 = 0.5;
qD2 = 35 \[Pi]/180;
qD3 = 50 \[Pi]/180;
qDsubs = {q1[t] -> qD1, q2[t] -> qD2, q3[t] -> qD3};
r0TCP /. qDsubs 
r0CoM1 /. qDsubs;
r0CoM2 /. qDsubs
r0CoM3 /. qDsubs

\[Theta]CoM3 = 1/12*m3*L3^2
\[Theta]CoM2 = 1/12*m2*L2^2

Function U and T

U = g (m1*r0CoM1[[1, 1]] + m2*r0CoM2[[1, 1]] + m3*r0CoM3[[1, 1]]) // 
Simplify

T = 0.5 (m1*vCoM1\[Transpose].vCoM1 + m2*vCoM2\[Transpose].vCoM2 + 
 m3*vCoM3\[Transpose].vCoM3 + \[Theta]CoM1*\[Omega]1^2 + \
\[Theta]CoM2*\[Omega]2^2 + \[Theta]CoM3*\[Omega]3^2) // Simplify

ODEs

Eq1 = (D[D[T, q1'[t]], t] - D[T, q1[t]] + D[U, q1[t]])[[1, 1]];
Eq2 = (D[D[T, q2'[t]], t] - D[T, q2[t]] + D[U, q2[t]])[[1, 1]];
Eq3 = (D[D[T, q3'[t]], t] - D[T, q3[t]] + D[U, q3[t]])[[1, 1]];

MassMatrix

M = ({
{Coefficient[Eq1, q1''[t]], Coefficient[Eq1, q2''[t]], 
Coefficient[Eq1, q3''[t]]},
{Coefficient[Eq2, q1''[t]], Coefficient[Eq2, q2''[t]], 
Coefficient[Eq2, q3''[t]]},
{Coefficient[Eq3, q1''[t]], Coefficient[Eq3, q2''[t]], 
Coefficient[Eq3, q3''[t]]}
})

MqD = M /. qDsubs; MqD // MatrixForm

Cq = ({
 {Eq1},
 {Eq2},
 {Eq3}
}) - M.({
  {q1''[t]},
  {q2''[t]},
  {q3''[t]}
 });

CqD = Cq /. qDsubs ; CqD // MatrixForm 

solutionFreemotion = NDSolve[{({{Eq1},{Eq2},{Eq3}}) == {0, 0, 0}, q1[0] == 
0.4, q2[0] == 0, q3[0] ==0, 
q1'[0] == 0, q2'[0] == 0, q3'[0] == 0}, {q1[t], q2[t], q3[t], 
q1'[t], q2'[t], q3'[t]}, {t, 0, 100}, StartingStepSize -> h, 
Method -> {"FixedStep", 
Method -> "EquationSimplification\[Rule]Residual"}]

Fig2 = Plot[(T + U) /. solutionFreemotion, {t, 0, 100}];
Fig3 = Plot[q1[t] /. solutionFreemotion, {t, 0, 100}];
Fig4 = Plot[q2[t] /. solutionFreemotion, {t, 0, 100}];
Fig5= Plot[q3[t] /. solutionFreemotion, {t, 0, 100}];
Show[Fig2, Frame -> True, FrameLabel -> {"t[s]", "T+U [J]"}, 
GridLines -> Automatic, ImageSize -> Large, 
PlotRange -> {{0, 100}, {0, 150}}]
Show[Fig3, Frame -> True, FrameLabel -> {"t[s]", "q_1 [rad]"}, 
GridLines -> Automatic, ImageSize -> Large]
Show[Fig4, Frame -> True, FrameLabel -> {"t[s]", "q_2 [rad]"}, 
GridLines -> Automatic, ImageSize -> Large]
Show[Fig5, Frame -> True, FrameLabel -> {"t[s]", "q_3 [m]"}, 
GridLines -> Automatic, ImageSize -> Large]

p = 1/27
d = 17/57
kp = p/h^2
kd = d/h
H = IdentityMatrix[3]
KP = kp*IdentityMatrix[3]
KD = kd*IdentityMatrix[3]
uPD = -KP.({
  {q1[t] - qD1},
  {q2[t] - qD2},
  {q3[t] - qD3}
 }) - KD.({
 {q1'[t]},
 {q2'[t]},
 {q3'[t]}
 })
EQ = ({
{Eq1},
{Eq2},
{Eq3}
 })

solutionPD = 
NDSolve[{EQ[[1, 1]] - H.uPD[[1, 1]] == 0, 
EQ[[2, 1]] - H.uPD[[2, 1]] == 0, EQ[[3, 1]] - H.uPD[[3, 1]] == 0, 
q1[0] == 0.4, q2[0] == 0, q3[0] == 0, q1'[0] == 0, q2'[0] == 0, 
q3'[0] == 0}, {q1[t], q2[t], q3[t], q1'[t], q2'[t], q3'[t]}, {t, 0,
10}, StartingStepSize -> h, 
Method -> {"FixedStep", Method -> "Automatic"}]
Fig6 = Plot[q1[t] /. solutionPD, {t, 0, 5}, PlotRange -> {0, \[Pi]/3}];
Fig7 = Plot[q2[t] /. solutionPD, {t, 0, 5}, PlotRange -> {0, \[Pi]/3}];
Fig8 = Plot[q3[t] /. solutionPD, {t, 0, 5}, PlotRange -> {0, \[Pi]/3}];
Fig9 = Plot[uPD[[1]] /. solutionPD, {t, 0, 5}, 
PlotRange -> {-700, 700}];
Fig10 = Plot[uPD[[2]] /. solutionPD, {t, 0, 5}, 
PlotRange -> {-200, 200}];
Fig11 = Plot[uPD[[3]] /. solutionPD, {t, 0, 5}, 
PlotRange -> {-300, 300}];
Show[Fig6, Frame -> True, FrameLabel -> {"t[s]", "q1[t]"}, 
GridLines -> Automatic, ImageSize -> Large]
Show[Fig7, Frame -> True, FrameLabel -> {"t[s]", "q2[t]"}, 
GridLines -> Automatic, ImageSize -> Large]
 Show[Fig8, Frame -> True, FrameLabel -> {"t[s]", "q3[t]"}, 
GridLines -> Automatic, ImageSize -> Large]
Show[Fig9, Frame -> True, FrameLabel -> {"t[s]", "u1[t]"}, 
GridLines -> Automatic, ImageSize -> Large]
Show[Fig10, Frame -> True, FrameLabel -> {"t[s]", "u2[t]"}, 
GridLines -> Automatic, ImageSize -> Large]
Show[Fig11, Frame -> True, FrameLabel -> {"t[s]", "u3[t]"}, 
GridLines -> Automatic, ImageSize -> Large]

I have this part of code, according to my homework about Dynamics of Robotmechanism, im facing to this problem. Power::infy: Infinite expression 1/0. encountered. Infinity::indet: Indeterminate expression 0. ComplexInfinity encountered. I have the same problem in PD controlled task. I hope you can understand my problem now. I added the plotting. These are the diagrams I need. Thanks Vitya

Sorry, they are defined, just my code is long, so I didnt wanted to copy the whole of it. — Vitya, May 1, 2021 at 14:59
You could give a minimal working example with T, U, also it is better to show definition as it is. — Alex Trounev, May 1, 2021 at 16:41
Remarks to the model used. First, mass M2 (and M3) rotate not around of center of mass, but around center of coordinate system highlighted on the picture. Therefore parameters $\theta CoM2 , \theta CoM3$ should be revised. Second, T, U can be derived by very simple way with using kinematic model. — Alex Trounev, May 3, 2021 at 10:41

Alex Trounev · Accepted Answer · 2021-05-07 09:12:16Z

Short version of code for this model based on picture uploaded

g = 9.81; 
m1 = 4.5; 
m2 = 4.25; 
m3 = 3.3; 
L2 = 1.2; 
L3 = 0.8; 

r1 = {q1[t], 0}; r2 = {q1[t] + L2/2 Cos[q2[t]], 
  L2/2 Sin [q2[t]]}; r3 = {q1[t] + L2 Cos[q2[t]] + 
   L3/2 Cos[q3[t] + q2[t]], L2 Sin[q2[t]] + L3/2 Sin[q2[t] + q3[t]]};

T = (m1/2 D[r1, t] . D[r1, t] + m2/2 D[r2, t] . D[r2, t] + 
     m3/2 D[r3, t] . D[r3, t] + I2 D[q2[t], t]^2/2 + 
     I3 D[q3[t] + q2[t], t]^2/2) /. {I2 -> m2 L2^2/12, 
    I3 -> m3 L3^2/12};

U = r2 . {0, g} + r3 . {0, g};

Eq1 = (D[D[T, Derivative[1][q1][t]], t] - D[T, q1[t]] + D[U, q1[t]]); 
Eq2 = (D[D[T, Derivative[1][q2][t]], t] - D[T, q2[t]] + D[U, q2[t]]); 
Eq3 = (D[D[T, Derivative[1][q3][t]], t] - D[T, q3[t]] + D[U, q3[t]]); 

sol = NDSolve[{Eq1 == 0, Eq2 == 0, Eq3 == 0, q1[0] == 0.4, q2[0] == 0,
    q3[0] == 0, q1'[0] == 0, q2'[0] == 0, q3'[0] == 0, 
   WhenEvent[q1[t] == 0, end = t; "StopIntegration"]}, {q1[t], q2[t], 
   q3[t], q1'[t], q2'[t], q3'[t]}, {t, 0, 20}]

Visualization

var = {q1, q2, q3, q1', q2', q3'};

Table[Plot[var[[i]][t] /. sol[[1]], {t, 0, 20}, PlotLabel -> var[[i]],
   Frame -> True], {i, 6}]

Plot[(T + U) /. sol[[1]], {t, 0, 20}]

Second part of this project

p = 1./27;
d = 17./57; h = 0.009;
kp = p/h^2;
kd = d/h; qD1 = 0.5;
qD2 = 35. Pi/180;
qD3 = 50. Pi/180;
H = IdentityMatrix[3];
KP = kp*IdentityMatrix[3];
KD = kd*IdentityMatrix[3];
uPD = -KP . ({{q1[t] - qD1}, {q2[t] - qD2}, {q3[t] - qD3}}) - 
  KD . ({{q1'[t]}, {q2'[t]}, {q3'[t]}}); HuPD = H . uPD;

{b, m} = CoefficientArrays[{Eq1 - HuPD[[1, 1]] == 0, 
   Eq2 - HuPD[[2, 1]] == 0, Eq3 - HuPD[[3, 1]] == 0}, {q1''[t], 
   q2''[t], q3''[t]}];
eq = -Inverse[m] . b;
solPD = NDSolve[{q1''[t] == eq[[1]], q2''[t] == eq[[2]], 
   q3''[t] == eq[[3]], q1[0] == 0.4, q2[0] == 0, q3[0] == 0, 
   q1'[0] == 0, q2'[0] == 0, q3'[0] == 0}, {q1[t], q2[t], q3[t], 
   q1'[t], q2'[t], q3'[t]}, {t, 0, 10}]

Visualization

{Table[Plot[var[[i]][t] /. solPD[[1]], {t, 0, 10}, 
   PlotLabel -> var[[i]], Frame -> True, PlotRange -> All], {i, 3}], 
 Plot[uPD[[1]] /. solPD[[1]], {t, 0, 10}, Frame -> True, 
  PlotLabel -> "uPD[[1]]", PlotRange -> All], 
 Plot[uPD[[2]] /. solPD[[1]], {t, 0, 10}, Frame -> True, 
  PlotLabel -> "uPD[[2]]", PlotRange -> All], 
 Plot[uPD[[3]] /. solPD[[1]], {t, 0, 10}, Frame -> True, 
  PlotLabel -> "uPD[[3]]", PlotRange -> All]}

I had have such a wrong solution, but the graphs should have a constant values, first graph is an Energy which should havew a constant value. — Vitya, May 2, 2021 at 14:15
Energy is T+U, first plot is q1, and q1,q2,q3 are coordinates in this model. To plot energy please use Plot[(T + U) /. sol[[1]], {t, 0, 100}]. In this plot energy slightly deviates from the initial value. — Alex Trounev, May 2, 2021 at 15:32
I did, but it is bullshit. The values aren't correct, the energy cant be 0J, a robotmechanism can't do this oscillating rotation and a slider cant do this as shown in q1[m]. — Vitya, May 2, 2021 at 16:19
Hm... There is nothing in numerical result more than you proposed in your model. Where did you take this model? — Alex Trounev, May 2, 2021 at 16:24
I made the model based on the lectures and seminars. My friend did the same and his solution works. He has another model, but the whole problem is the same, just another coordinates — Vitya, May 2, 2021 at 16:37

Cesareo · Accepted Answer · 2021-05-07 09:36:19Z

About the dynamic modeling

J2 = 1/12*m2*L2^2;
J3 = 1/12*m3*L3^2;
p1 = {q1[t], 0};
p2 = p1 + L2/2 {Cos[q2[t]], Sin[q2[t]]};
p3 = p1 + L2 {Cos[q2[t]], Sin[q2[t]]} + L3/2 {Cos[q2[t] + q3[t]], Sin[q2[t] + q3[t]]};
v1 = D[p1, t]
v2 = D[p2, t]
v3 = D[p3, t]
omega2 = D[q2[t], t]
omega3 = D[q2[t] + q3[t], t]
T = 1/2 (m1 v1.v1 + m2 v2.v2 + m3 v3.v3) + 1/2 (J2 omega2^2 + J3 omega3^2)
U = g (m2 p2 + m3 p3).{0, 1}
L = T - U
vars = {q1[t], q2[t], q3[t]};
equs = D[Grad[L, D[vars, t]], t] - Grad[L, vars]
cinits = {q1[0] == 0.4, q2[0] == q3[0] == q1'[0] == q2'[0] == q3'[0] == 0};
ODE = Join[Thread[equs == 0], cinits];
tmax = 10;
sol = NDSolve[ODE, vars, {t, 0, tmax}][[1]]
Table[Plot[vars[[k]] /. sol, {t, 0, tmax}], {k, 1, 3}]

And with control

p = 1/27;
d = 17/57;
h = 0.009;
kp = p/h^2;
kd = d/h;
qD1 = 0.5;
qD2 = 35. Pi/180;
qD3 = 50. Pi/180;
H = IdentityMatrix[3];
KP = kp*IdentityMatrix[3];
KD = kd*IdentityMatrix[3];
uPD = -KP.({{q1[t] - qD1}, {q2[t] - qD2}, {q3[t] - qD3}}) - KD.({{q1'[t]}, {q2'[t]}, {q3'[t]}});
HuPD = Flatten[H.uPD];
CLODE = Join[Thread[equs == HuPD], cinits];
sol = NDSolve[CLODE, vars, {t, 0, tmax}][[1]];
qDs = {qD1, qD2, qD3};
Table[Plot[{vars[[k]] /. sol, qDs[[k]]}, {t, 0, tmax}, PlotRange -> All], {k, 1, 3}]

Nice approach (+1). Why do you take initial data are differ from that in the Vitya post? — Alex Trounev, May 7, 2021 at 9:16
Good question. Well, I would swear that the initial conditions of the question, they were the ones I used. There was a recent edit to the text of the question ... Any way, I will prepare according to the new conditions. — Cesareo, May 7, 2021 at 9:27
But in the second part Vitya used the same initial condition q1[0] == 0.4, q2[0] == 0, q3[0] == 0, q1'[0] == 0, q2'[0] == 0, q3'[0] == 0. — Alex Trounev, May 7, 2021 at 9:31

Stack Exchange Network

I have some problems with NDSolve, my ODE is about Dynamics of Robot mechanism, and my BVP cannot be solved by NDSolve

2 Answers 2

Your Answer

Not the answer you're looking for? Browse other questions tagged
differential-equations
simplifying-expressions
warning-messages
boundary-condition-at-infinity
or ask your own question.

Hot Network Questions

I have some problems with NDSolve, my ODE is about Dynamics of Robot mechanism, and my BVP cannot be solved by NDSolve

2 Answers 2

Your Answer

Sign up or log in

Post as a guest

Not the answer you're looking for? Browse other questions tagged differential-equationssimplifying-expressionswarning-messagesboundary-condition-at-infinity or ask your own question.

Related

Hot Network Questions

Not the answer you're looking for? Browse other questions tagged
differential-equations
simplifying-expressions
warning-messages
boundary-condition-at-infinity
or ask your own question.