1
$\begingroup$

Position control of a 3DoF planar manipulator 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

$\endgroup$
5
  • 1
    $\begingroup$ Functions $T, U$ are not defined. $\endgroup$ May 1, 2021 at 12:50
  • $\begingroup$ Sorry, they are defined, just my code is long, so I didnt wanted to copy the whole of it. $\endgroup$
    – Vitya
    May 1, 2021 at 14:59
  • $\begingroup$ You could give a minimal working example with T, U, also it is better to show definition as it is. $\endgroup$ May 1, 2021 at 16:41
  • $\begingroup$ Edited it thank you $\endgroup$
    – Vitya
    May 2, 2021 at 8:28
  • $\begingroup$ 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. $\endgroup$ May 3, 2021 at 10:41

2 Answers 2

4
$\begingroup$

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}]

Figure 1

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]}

Figure 2

$\endgroup$
16
  • $\begingroup$ 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. $\endgroup$
    – Vitya
    May 2, 2021 at 14:15
  • $\begingroup$ 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. $\endgroup$ May 2, 2021 at 15:32
  • $\begingroup$ 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]. $\endgroup$
    – Vitya
    May 2, 2021 at 16:19
  • $\begingroup$ Hm... There is nothing in numerical result more than you proposed in your model. Where did you take this model? $\endgroup$ May 2, 2021 at 16:24
  • $\begingroup$ 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 $\endgroup$
    – Vitya
    May 2, 2021 at 16:37
2
$\begingroup$

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}]

enter image description here enter image description here enter image description here

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}]

enter image description here enter image description here enter image description here

$\endgroup$
4
  • $\begingroup$ Nice approach (+1). Why do you take initial data are differ from that in the Vitya post? $\endgroup$ May 7, 2021 at 9:16
  • $\begingroup$ 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. $\endgroup$
    – Cesareo
    May 7, 2021 at 9:27
  • $\begingroup$ 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. $\endgroup$ May 7, 2021 at 9:31
  • $\begingroup$ I did the corrections. Now looks good. $\endgroup$
    – Cesareo
    May 7, 2021 at 9:39

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.