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
T, U
, also it is better to show definition as it is. $\endgroup$T, U
can be derived by very simple way with using kinematic model. $\endgroup$