diff --git a/doc/chap_examples.rst b/doc/chap_examples.rst index 0a253da6..6ea1b35c 100644 --- a/doc/chap_examples.rst +++ b/doc/chap_examples.rst @@ -532,11 +532,10 @@ to the green region. Two-link planar manipulator ------------- -.. _hwfig: +.. _tlpmfig: -.. figure:: /pic/chapter06_section06_task.PNG - :alt: highway - :width: 30 % +.. figure:: /pic/chapter06_section06_task.png + :width: 30 % Two-link planar manipulator. @@ -557,13 +556,13 @@ At first, write the kinetic and potential energy for this system. 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, +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 + --- 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:: @@ -572,7 +571,7 @@ lengths of elements, :math:`J_{c_1}, J_{c_2}` -- moments of inertia, 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}. + y_2 =& \ l_1 \sin{\phi_1} + s_2 \sin{\phi_2}. .. math:: :label: kinetic_def_1 @@ -683,7 +682,7 @@ Make new replacements: With new variables we get: .. math:: - :label: taylor + :label: deriv_angles & \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. @@ -731,7 +730,7 @@ Now define new variables: -where :math:`\phi_1^0, \phi_2^0` -- angles of construction in equilibrium position. +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}`, @@ -770,112 +769,90 @@ As a result, we get: 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 + \left[\begin{array}{cc} + 0 & 0 \\ + 0 & 0 \\ + y_{3} & y_{4} \\ + x_{3} & x_{4} \end{array}\right] \left[\begin{array}{c}\ u_1 \\ - u_2 \\ - u_3 \\ - u_4 \end{array}\right], + u_2 + \end{array}\right], -Here :math:`u_3, u_4` -- fictitious variables for right size, +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 +Now we can compute the solvability 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 (:math:`z_3, z_4`) of system ( evolves in time from t=0 to t=5, end position is :math:`z_1=0, z_2=0,z_3=0,z_4=0`) - -.. _hwfig: .. figure:: /pic/chapter06_section06_pic2.png - :alt: highway :width: 100 % Figure shows solvability set projection onto (:math:`z_1, z_2`) of system ( evolves in time from t=0 to t=5, end position is :math:`z_1=0, z_2=0,z_3=0,z_4=0`) -.. _hwfig: .. figure:: /pic/chapter06_section06_pic3.png - :alt: highway :width: 100 % Figure shows solvability set projection onto (:math:`z_1, z_3`) of system ( evolves in time from t=0 to t=5, end position is :math:`z_1=0, z_2=0,z_3=0,z_4=0`) -.. _hwfig: .. figure:: /pic/chapter06_section06_pic4.png - :alt: highway :width: 100 % Figure shows solvability set projection onto (:math:`z_2, z_4`) of system ( evolves in time from t=0 to t=5, end position is :math:`z_1=0, z_2=0,z_3=0,z_4=0`) -.. _hwfig: .. figure:: /pic/chapter06_section06_pic5.png - :alt: highway :width: 100 % Figure shows solvability set projection onto (:math:`z_3, z_4`) of system ( evolves in time from t=0 to t=5, end position is :math:`z_1=\frac{\pi}{8}, z_2=\frac{\pi}{8}, z_3=0, z_4=0`) - -.. _hwfig: .. figure:: /pic/chapter06_section06_pic6.png - :alt: highway :width: 100 % Figure shows solvability set projection onto (:math:`z_1, z_2`) of system ( evolves in time from t=0 to t=5, end position is :math:`z_1=\frac{\pi}{8}, z_2=\frac{\pi}{8}, z_3=0, z_4=0`) - -.. _hwfig: + .. figure:: /pic/chapter06_section06_pic7.png - :alt: highway :width: 100 % Figure shows solvability set projection onto (:math:`z_1, z_3`) of system ( evolves in time from t=0 to t=5, end position is :math:`z_1=\frac{\pi}{8}, z_2=\frac{\pi}{8}, z_3=0, z_4=0`) - -.. _hwfig: + .. figure:: /pic/chapter06_section06_pic8.png - :alt: highway :width: 100 % Figure shows solvability set projection onto (:math:`z_2, z_4`) of system ( evolves in time from t=0 to t=5, end position is :math:`z_1=\frac{\pi}{8}, z_2=\frac{\pi}{8}, z_3=0, z_4=0`) -.. _hwfig: .. figure:: /pic/chapter06_section06_pic9.png - :alt: highway :width: 100 % Now set control restrictions on :math:`\alpha_1 = 10, \alpha_2 = 10`. @@ -883,10 +860,8 @@ and taking its projection onto :math:`(z_1, z_2)` subspace. ( evolves in time from t=0 to t=5, end position is :math:`z_1=\frac{\pi}{8}, z_2=\frac{\pi}{8}, z_3=0, z_4=0`) -.. _hwfig: .. figure:: /pic/chapter06_section06_pic10.png - :alt: highway :width: 100 % Figure shows solvability set projection onto (:math:`z_3, z_4`) of system @@ -894,10 +869,7 @@ and taking its projection onto :math:`(z_1, z_2)` subspace. :math:`z_1=\frac{\pi}{8}, z_2=\frac{\pi}{8}, z_3=0, z_4=0`, control limits: :math:`\alpha_1 = 10, \alpha_2 = 10`) - -.. raw:: latex - - \newpage + .. literalinclude:: ../products/+elltool/+doc/+snip/s_chapter06_section06_snippet01.m :language: matlab diff --git a/doc/pic/chapter06_section06_task.png b/doc/pic/chapter06_section06_task.png index 86faa6d2..8c2758f0 100644 Binary files a/doc/pic/chapter06_section06_task.png 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 index b33bd139..3d71bc72 100644 --- a/products/+elltool/+doc/+snip/s_chapter06_section06_snippet01.m +++ b/products/+elltool/+doc/+snip/s_chapter06_section06_snippet01.m @@ -1,77 +1,56 @@ - -% 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; - +% initialize all variables for this model +m = [1; 2]; +J = [0; 0]; +phi_0 = [0; 0]; +L = [5; 5]; +s = [3; 3]; +alpha = [3; 10]; +k = [0; 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; +a_1 = m(1) * s(1)^2 + J(1) + m(2) * L(1)^2; +a_2 = L(1) * s(2) * cos(phi_0(1) - phi_0(2)); +b_1 = L(1) * s(2) * cos(phi_0(1) - phi_0(2)); +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; +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_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_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); +X_11 = d_1 * a_2 * sin(phi_0(1)) / (a_2 * b_1 - a_1 * b_2); +X_12 = d_2 * a_1 * sin(phi_0(2)) / (a_2 * b_1 - a_1 * b_2); +X_13 = (-d_1 * a_2 * cos(phi_0(1)) - d_1 * a_2 * sin(phi_0(1)) * phi_0(1)) / ... + (a_2 * b_1 - a_1 * b_2) + (-d_2 * a_1 * cos(phi_0(2)) - ... + d_2 * a_1 * sin(phi_0(2)) * phi_0(2)) / (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); +Y_11 = d_1 * b_2 * sin(phi_0(1)) / (a_2 * b_1 - a_1 * b_2); +Y_12 = -d_2 * b_1 * sin(phi_0(2)) / (a_2 * b_1 - a_1 * b_2); +Y_13 = (d_2 * b_1 * cos(phi_0(2)) + d_2 * b_1 * sin(phi_0(2)) * phi_0(2)) / ... + (a_2 * b_1 - a_1 * b_2) + (-d_1 * b_2 * cos(phi_0(1)) - ... + d_1 * b_2 * sin(phi_0(1)) * phi_0(1)) / (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; +bMat = [0 0; 0 0; Y_3 Y_4; X_3 X_4]; +x_0 = [0; 0; 0; 0]; +endTime=5; % after initializing we create ellipsoid for control function -diagonal = diag([alpha_1, alpha_2, 1, 1]); -uEllObj = ellipsoid(diagonal); - +diagMat = diag([alpha(1), alpha(2)]); +uEllObj = ellipsoid(diagMat); % define linear systems linSys=elltool.linsys.LinSysContinuous(aMat, bMat, uEllObj); - % initialize direction matrix nShape = 4; mSize = 10; @@ -79,30 +58,18 @@ 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; - +x0EllObj = 1E-2 * ell_unitball(4) + x_0; % calculate solvability set -timeVec = [myTime, 0]; +timeVec = [endTime, 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