diff --git a/doc/chap_examples.rst b/doc/chap_examples.rst index 59017c6d..847efea8 100644 --- a/doc/chap_examples.rst +++ b/doc/chap_examples.rst @@ -529,4 +529,293 @@ to the green region. American Control Conference*, 3750–3755. Denver, Colorado, USA. +Two-link planar manipulator +------------- + +.. _hwfig: + +.. figure:: /pic/chapter06_section06_task.PNG + :alt: highway + :width: 30 % + + Two-link planar manipulator. + + +Consider a model of two-link planar pendulum whose hinge axes are parallel. +Movements occur in a vertical plane. + + + + +At first, write the kinetic and potential energy for this system. + +.. math:: + :label: energy_def + + T_1 =& \ \frac{m_1 (\ddot{x}_1^2 + \ddot{y}_1^2)}{2} + \frac{J_{c_1} \dot{\phi}_1^2}{2},\\ + T_2 =& \ \frac{m_2 (\ddot{x}_2^2 + \ddot{y}_1^2)}{2} + \frac{J_{c_2} \dot{\phi}_2^2}{2}, \\ + V_1 =& \ m_1 g s_1 \sin{\phi_1},\\ + V_2 =& \ m_2 g l_1 \sin{\phi_1} + m_2 g s_2 \sin{\phi_2} + +In these equations :math:`m_1, m_2` -- masses of objects, :math:`s_1 = |O_1 C_1|, +s_2 = |O_2 C_2|`, where :math:`C_1, C_2` -- centers of masses, +:math:`\phi_1, \phi_2` -- angles of elements' position, :math:`l_1, l_2` -- full +lengths of elements, :math:`J_{c_1}, J_{c_2}` -- moments of inertia, +:math:`\left | u_1\right | \leqslant \alpha_1, \left | u_2\right | \leqslant \alpha_2` + -- restriction of controls, which are attached to the hinges, +:math:`M_{tr1}, M_{tr2}` -- torques of friction force and torques are proportional + to the angular speed with coefficients :math:`k_1, k_2`. + +.. math:: + :label: coordinates + + x_1 =& \ s_1 \cos{\phi_1},\\ + y_1 =& \ s_1 \sin{\phi_1},\\ + x_2 =& \ l_1 \cos{\phi_1} + s_2 \cos{\phi_2},\\ + y_2 =& \ s_1 \cos{\phi_1}. + +.. math:: + :label: kinetic_def_1 + + T_1 =& \ \frac{1}{2} \dot{\phi}_1^2(m_1 s_1^2 + J_{c_1}),\\ + T_2 =& \ \frac{m_2}{2} (l_1^2 \dot{\phi}_1^2 + s_2^2 \dot{\phi}_2^2 + 2l_1 s_2 + \dot{\phi}_1 \dot{\phi}_2 \cos{(\phi_1 - \phi_2)}) + \frac{J_{c_2} \dot{\phi}_2^2}{2}, + +Next we come to Lagrange function: + +.. math:: + :label: lagran_1 + + L = T - V = \dot{\phi}_1^2 \frac{m_1 s_1^2 + J_{c_1} + m_2 l_1^2}{2} + + \dot{\phi}_2^2 \frac{m_2 s_2^2 + J_{c_2}}{2} + \\ + + \dot{\phi}_1\dot{\phi}_2 l_1 s_2 \cos{(\phi_1 - \phi_2)} - m_2 g s_2 \sin{\phi_2} + - (m_1 s_1 + m_2 l_1)g \sin{\phi_1} + +Take partial derivatives: + +.. math:: + :label: lagran_derivatives + + \dfrac{\partial{L}}{\partial \phi_1} = & \ - \dot{\phi}_1 \dot{\phi}_2l_1s_2 \sin(\phi_1 - \phi_2) + - (m_1s_1 + m_2l_1)g\cos(\phi_1), \\ + \dfrac{\partial{L}}{\partial \phi_2} = & \ \dot{\phi}_1 \dot{\phi}_2l_1s_2 \sin(\phi_1 - \phi_2) + - m_2gs_2\cos(\phi_2),\\ + \dfrac{d}{dt}\left ( \dfrac{\partial L}{\partial \dot{\phi}_1 }\right ) = & + \ \ddot{\phi}_1(m_1a_1^2 + J_{c_1} + m_2l_1^2) + \ddot{\phi}_2l_1s_2\cos(\phi_1 - \phi_2) - + (\dot{\phi}_1 - \dot{\phi}_2)\dot{\phi}_2l_1s_2\sin(\phi_1 - \phi_2),\\ + \dfrac{d}{dt}\left ( \dfrac{\partial L}{\partial \dot{\phi}_2 }\right ) = & + \ \ddot{\phi}_2(m_2s_2^2 + J_{c_2}) + \ddot{\phi}_1l_1s_2\cos(\phi_1 - \phi_2) - + (\dot{\phi}_1 - \dot{\phi}_2)\dot{\phi}_1l_1s_2\sin(\phi_1 - \phi_2) + +Now write Lagrange equation: + +.. math:: + :label: lagran_equ + + \dfrac{d}{dt}\left ( \dfrac{\partial L}{\partial \dot{\phi}_1 }\right ) - + \dfrac{\partial{L}}{\partial \phi_1} = u_1 - M_{tr1}\\ + \dfrac{d}{dt}\left ( \dfrac{\partial L}{\partial \dot{\phi}_2 }\right ) - + \dfrac{\partial{L}}{\partial \phi_2} = u_2 - M_{tr2} + +Make some changes of variables for more convenient view: + +.. math:: + :label: replacements + + & a_1 = m_1s_1^2 + J_{c_1} + m_2l_1^2,\\ + & b_1 = l_1s_2\cos(\phi_1 - \phi_2),\\ + & c_1 = l_1s_2\sin(\phi_1- \phi_2),\\ + & d_1 = (m_1s_1 + m_2l_1)g\cos(\phi_1),\\ + & f_1 = u_1 - M_{tr1},\\ + & a_2 = l_1 s_2 \cos(\phi_1 - \phi_2),\\ + & b_2 = m_2 s_2^2 + J_{c_2},\\ + & e_2 = l_1 s_2 \sin(\phi_1- \phi_2),\\ + & d_2 = m_2 g s_2 \cos(\phi_2),\\ + & f_2 = u_2 - M_{tr2}. + +As a result we get: + +.. math:: + :label: after_replace + + & \ddot{\phi}_1 a_1 +\ddot{\phi}_2 b_1 + \dot{\phi}_2^2 c_1 + d_1 = f_1\\ + & \ddot{\phi}_1 a_2 +\ddot{\phi}_2 b_2 - \dot{\phi}_1^2 e_2 + d_2 = f_2. + +Remember that torques of friction forces are proportional to the angular speed: + +.. math:: + :label: trenie + + + M_{tr1} = k_1 \dot{\phi}_1,\\ + M_{tr2} = k_2 \dot{\phi}_2. + + +Transform the equation: + +.. math:: + :label: phi_1phi_2 + + & \ddot{\phi}_1 = + \dfrac{f_1b_2-f_2b_1 + d_2b_1 - d_1b_2 - \dot{\phi}_1^2b_1e_2 - \dot{\phi}_2^2c_1b_2} + {s_2b_1 - s_1b_2}\\ + + & \ddot{\phi}_2 = + \dfrac{f_1s_2-f_2s_1 - d_1s_2 - d_2a_1 + \dot{\phi}_1^2s_1e_2 - \dot{\phi}_2^2s_2c_1} + {s_2b_1 - s_1b_2} + +Make new replacements: + +.. math:: + :label: replacement_before_taylor + + & x_1 = \frac{-k_1 a_2}{a_2 b_1 - a_1 b_2}, \\ + & x_2 = \frac{k_2 a_1}{a_2 b_1 - a_1 b_2}, \\ + & x_3 = \frac{a_2}{a_2 b_1 - a_1 b_2}, \\ + & x_4 = \frac{-a_1}{a_2 b_1 - a_1 b_2}, \\ + & x_5 = \frac{-d_1 a_2 - d_2 a_1}{a_2 b_1 - a_1 b_2}, \\ + & y_1 = \frac{-k_1 b_2}{a_2 b_1 - a_1 b_2}, \\ + & y_2 = \frac{k_2 b_1}{a_2 b_1 - a_1 b_2}, \\ + & y_3 = \frac{b_2}{a_2 b_1 - a_1 b_2}, \\ + & y_4 = \frac{-b_1}{a_2 b_1 - a_1 b_2}, \\ + & y_5 = \frac{-d_2 b_1 - d_1 b_2}{a_2 b_1 - a_1 b_2} + +With new variables we get: + +.. math:: + :label: taylor + + & \ddot{\phi}_1 = \dot{\phi}_1 y_1 + \dot{\phi}_2 y_2 + u_1 y_3 + u_2 y_4 + y_5,\\ + & \ddot{\phi}_2 = \dot{\phi}_1 x_1 + \dot{\phi}_2 x_2 + u_1 x_3 + u_2 x_4 + x_5. + +Here :math:`x_5, y_5` depends on :math:`cos{\phi_1}, cos{\phi_2}` +so write Taylor formula for :math:`\cos` function: + +.. math:: + :label: taylor + + & f(x + \Delta x) \approx f(x) + \dot{f}(x)\Delta x,\\ + & cos(x + \Delta x) \approx cos(x) - sin(x) \Delta x + + +Then :math:`x_5, y_5` we can imagine as: + +.. math:: + :label: x_5_y_5 + + x_5 = & \phi_1 \left( \frac{d_1^{'} a_2 \sin{\phi_1^0}}{a_2 b_1 - a_1 b_2} \right) + + \phi_2 \left( \frac{d_2^{'} a_1 \sin{\phi_2^0}}{a_2 b_1 - a_1 b_2} \right) + \\ + & + \frac{-d_1^{'} a_2 \cos{\phi_1^0} -d_1^{'} a_2 \phi_1 \sin{\phi_1^0}}{a_2 b_1 - a_1 b_2} + + \frac{-d_2^{'} a_1 \cos{\phi_2^0} -d_2^{'} a_1 \phi_1^0 \sin{\phi_1^0}}{a_2 b_1 - a_1 b_2}, \\ + + y_5 = & \phi_1 \left( \frac{d_1^{'} b_2 \sin{\phi_1^0}}{a_2 b_1 - a_1 b_2} \right)+ + \phi_2 \left( \frac{- d_2^{'} b_1 \sin{\phi_2^0}}{a_2 b_1 - a_1 b_2} \right) + \\ + & + \frac{-d_2^{'} b_1 \cos{\phi_2^0} - d_2^{'} b_1 \phi_2^0 \sin{\phi_2^0}}{a_2 b_1 - a_1 b_2}+ + \frac{-d_1^{'} b_2 \cos{\phi_1^0} - d_1^{'} b_2 \phi_1^0 \sin{\phi_1^0}}{a_2 b_1 - a_1 b_2}, + +where :math:`d_1^{'} \cos(\phi_1) = d_1 , d_2^{'} \cos(\phi_2) = d_2`. +Now define new variables: + +.. math:: + :label: x_11_x12 + + x_{11} = & \ \frac{d_1^{'} a_2 \sin{\phi_1^0}}{a_2 b_1 - a_1 b_2}, \\ + x_{12} = & \ \frac{d_2^{'} a_1 \sin{\phi_2^0}}{a_2 b_1 - a_1 b_2}, \\ + x_{13} = & \ \frac{-d_1^{'} a_2 \cos{\phi_1^0} -d_1^{'} a_2 \phi_1 \sin{\phi_1^0}}{a_2 b_1 - a_1 b_2} + \\ + & + \frac{-d_2^{'} a_1 \cos{\phi_2^0} -d_2^{'} a_1 \phi_1^0 \sin{\phi_1^0}}{a_2 b_1 - a_1 b_2}, \\ + y_{11} = & \ \frac{d_1^{'} b_2 \sin{\phi_1^0}}{a_2 b_1 - a_1 b_2}, \\ + y_{12} = & \ \frac{- d_2^{'} b_1 \sin{\phi_2^0}}{a_2 b_1 - a_1 b_2}, \\ + y_{13} = & \ \frac{-d_2^{'} b_1 \cos{\phi_2^0} - d_2^{'} b_1 \phi_2^0 \sin{\phi_2^0}}{a_2 b_1 - a_1 b_2} + \\ + & + \frac{-d_1^{'} b_2 \cos{\phi_1^0} - d_1^{'} b_2 \phi_1^0 \sin{\phi_1^0}}{a_2 b_1 - a_1 b_2}, \\ + + + + +where :math:`\phi_1^0, \phi_2^0` -- angles of construction in equilibrium position. + +We have system :math:`\dot{t} = At + Bu + C`, here :math:`t` - vector with 4 coordinates, +where :math:`t_1 = \phi_1,\ t_2 = \phi_2,\ t_3 = \dot{\phi_3},\ t_4 = \dot{\phi_4}`, +:math:`A, B \in \mathbb{R}^{4 \times 4}`. + +:math:`C` not depend on :math:`\phi_1, \phi_2`, so we can do replacement :math:`z = t + \frac{A}{C}`. + +As a result, we get: + +.. math:: + :label: equation_AB + + \dot{z} = Az + Bu + + + + + +.. math:: + + \left[\begin{array}{c} + \dot{z}_1\\ + \dot{z}_2\\ + \dot{z}_3\\ + \dot{z}_4\end{array}\right] + = + \left[\begin{array}{cccc} + 0 & 0 & 1 & 0 \\ + 0 & 0 & 0 & 1 \\ + y_{11} & y_{12} & y_1 & y_2 \\ + x_{11} & x_{12} & x_1 & x_2 + \end{array}\right] + \left[\begin{array}{c} + z_1\\ + z_2\\ + z_3\\ + z_4\end{array}\right] + + + \left[\begin{array}{cccc} + 0 & 0 & 0 & 0 \\ + 0 & 0 & 0 & 0 \\ + y_{3} & y_{4} & 0 & 0 \\ + x_{3} & x_{4} & 0 & 0 + \end{array}\right] + \left[\begin{array}{c}\ + u_1 \\ + u_2 \\ + u_3 \\ + u_4 \end{array}\right], + +Here :math:`u_3, u_4` -- fictitious variables for right size, +:math:`|u_1| \leqslant \alpha_1, |u_2| \leqslant \alpha_2`. + + +Now we can compute the reach set of the linear system +and taking its projection onto :math:`(z_1, z_2)` subspace. + + +.. _hwfig: + +.. figure:: /pic/chapter06_section06_pic1.png + :alt: highway + :width: 100 % + + Figure shows solvability set projection onto (x_3, x_4) of system + ( evolves in time from t=0 to t=5, end position is x1=0, x2=0,x3=0,x4=0. + + +.. _hwfig: + +.. figure:: /pic/chapter06_section06_pic2.png + :alt: highway + :width: 100 % + + Figure shows solvability set projection onto (x_1, x_2) of system + ( evolves in time from t=0 to t=5, end position is x1=0, x2=0,x3=0,x4=0. + +.. raw:: latex + + \newpage + +.. literalinclude:: ../products/+elltool/+doc/+snip/s_chapter06_section06_snippet01.m + :language: matlab + :linenos: + + \ No newline at end of file diff --git a/doc/pic/chapter06_section06_pic1.png b/doc/pic/chapter06_section06_pic1.png new file mode 100644 index 00000000..93fb8cca Binary files /dev/null and b/doc/pic/chapter06_section06_pic1.png differ diff --git a/doc/pic/chapter06_section06_pic2.png b/doc/pic/chapter06_section06_pic2.png new file mode 100644 index 00000000..62bfaba8 Binary files /dev/null and b/doc/pic/chapter06_section06_pic2.png differ diff --git a/doc/pic/chapter06_section06_task.png b/doc/pic/chapter06_section06_task.png new file mode 100644 index 00000000..86faa6d2 Binary files /dev/null and b/doc/pic/chapter06_section06_task.png differ diff --git a/products/+elltool/+doc/+snip/s_chapter06_section06_snippet01.m b/products/+elltool/+doc/+snip/s_chapter06_section06_snippet01.m new file mode 100644 index 00000000..b33bd139 --- /dev/null +++ b/products/+elltool/+doc/+snip/s_chapter06_section06_snippet01.m @@ -0,0 +1,108 @@ + +% in this block we initialize all variables for this model +m_1 = 1; +m_2 = 2; + +J_1 = 0; +J_2 = 0; + +phi_10 = 0; +phi_20 = 0; + +L_1 = 5; +L_2 = 5; + +s_1 = 3; +s_2 = 3; + +alpha_1 = 3; +alpha_2 = 10; + +k_1 = 0; +k_2 = 0; + +g = 9.8; + +a_1 = m_1 * s_1^2 + J_1 + m_2 * L_1^2; +a_2 = L_1 * s_2 * cos(phi_10 - phi_20); +b_1 = L_1 * s_2 * cos(phi_10 - phi_20); +b_2 = m_2 * s_2^2 + J_2; + +d_1 = (m_1 * a_1 + m_2 * L_2) * g; +d_2 = m_2 * a_2 * g; + +X_1 = -k_1 * a_2 / (a_2 * b_1 - a_1 * b_2); +X_2 = k_2 * a_1 / (a_2 * b_1 - a_1 * b_2); +X_3 = a_2 / (a_2 * b_1 - a_1 * b_2); +X_4 = -a_1 / (a_2 * b_1 - a_1 * b_2); +X_5 = -(d_1 * a_2 + d_2 * a_1) / (a_2 * b_1 - a_1 * b_2); + +Y_1 = -k_1 * b_2 / (a_2 * b_1 - a_1 * b_2); +Y_2 = k_2 * b_1 / (a_2 * b_1 - a_1 * b_2); +Y_3 = b_2 / (a_2 * b_1 - a_1 * b_2); +Y_4 = -b_1 / (a_2 * b_1 - a_1 * b_2); +Y_5 = (d_2 * b_1 - d_1 * b_2) / (a_2 * b_1 - a_1 * b_2); + +X_11 = d_1 * a_2 * sin(phi_10) / (a_2 * b_1 - a_1 * b_2); +X_12 = d_2 * a_1 * sin(phi_20) / (a_2 * b_1 - a_1 * b_2); +X_13 = (-d_1 * a_2 * cos(phi_10) - d_1 * a_2 * sin(phi_10) * phi_10) / ... + (a_2 * b_1 - a_1 * b_2) + (-d_2 * a_1 * cos(phi_20) - ... + d_2 * a_1 * sin(phi_20) * phi_20) / (a_2 * b_1 - a_1 * b_2); + +Y_11 = d_1 * b_2 * sin(phi_10) / (a_2 * b_1 - a_1 * b_2); +Y_12 = -d_2 * b_1 * sin(phi_20) / (a_2 * b_1 - a_1 * b_2); +Y_13 = (d_2 * b_1 * cos(phi_20) + d_2 * b_1 * sin(phi_20) * phi_20) / ... + (a_2 * b_1 - a_1 * b_2) + (-d_1 * b_2 * cos(phi_10) - ... + d_1 * b_2 * sin(phi_10) * phi_10) / (a_2 * b_1 - a_1 * b_2); + +%initialize A and B relying on theoretical materials +aMat = [0 0 1 0; 0 0 0 1; Y_11 Y_12 Y_1 Y_2; X_11 X_12 X_1 X_2]; +bMat = [0 0 0 0; 0 0 0 0; Y_3 Y_4 0 0 ; X_3 X_4 0 0]; + +x_01=0; +x_02=0; +x_03=0; +x_04=0; +myTime=5; + +% after initializing we create ellipsoid for control function +diagonal = diag([alpha_1, alpha_2, 1, 1]); +uEllObj = ellipsoid(diagonal); + +% define linear systems +linSys=elltool.linsys.LinSysContinuous(aMat, bMat, uEllObj); + +% initialize direction matrix +nShape = 4; +mSize = 10; +dirMat = 2*(rand(nShape, mSize) - 0.5); +for iElem = 1 : mSize + dirMat(:, iElem) = dirMat(:, iElem) ./ norm(dirMat(:, iElem)); +end; + +% end position +x_0v = [x_01; x_02; x_03; x_04]; + +% set of start coordinates +x0EllObj = 1E-2 * ell_unitball(4) + x_0v; + +% calculate solvability set +timeVec = [myTime, 0]; +rsTube = elltool.reach.ReachContinuous(linSys, x0EllObj, dirMat, ... + timeVec, 'isRegEnabled', true, 'isJustCheck', false, 'regTol', 1e-3); + +% create projection matrix and draw projection on current axis (z_3, z_4) +v1v2Mat = [0 0 1 0 ; 0 0 0 1]'; +prTube = rsTube.projection(v1v2Mat); +prTube.plotByIa(); +hold on; +ylabel('z_3'); zlabel('z_4'); +rotate3d on; + +% create projection matrix and draw projection on current axis (z_1, z_2) +v1v2Mat = [1 0 0 0 ; 0 1 0 0]'; +prTube = rsTube.projection(v1v2Mat); +prTube.plotByIa(); +hold on; +ylabel('z_1'); zlabel('z_2'); +rotate3d on; \ No newline at end of file