Forward\_Kinematics module
==========================

.. automodule:: Forward_Kinematics
    :members:
    :undoc-members:
    :show-inheritance:

Python Program
--------------

.. literalinclude:: ../../src/Forward_Kinematics.py
  :linenos:
  :language: Python

Output
------

.. code-block:: bash

    Matrix t_0_1:

    ⎡1.0⋅cos(θ₁)  -1.0⋅sin(θ₁)   0     0    ⎤
    ⎢                                      ⎥
    ⎢1.0⋅sin(θ₁)  1.0⋅cos(θ₁)    0     0    ⎥
    ⎢                                      ⎥
    ⎢     0            0        1.0  1.0⋅l₁⎥
    ⎢                                      ⎥
    ⎣     0            0         0    1.0  ⎦


    Matrix t_1_2:
    ⎡1.0⋅cos(θ₂)  -1.0⋅sin(θ₂)   0     0  ⎤
    ⎢                                    ⎥
    ⎢     0            0        -1.0   0 ⎥
    ⎢                                    ⎥
    ⎢1.0⋅sin(θ₂)  1.0⋅cos(θ₂)   0.0    0  ⎥
    ⎢                                    ⎥
    ⎣     0            0         0    1.0⎦


    Matrix t_2_3:

    ⎡1.0⋅cos(θ₃)  -1.0⋅sin(θ₃)   0   1.0⋅l₂ ⎤
    ⎢                                      ⎥
    ⎢1.0⋅sin(θ₃)  1.0⋅cos(θ₃)    0     0    ⎥
    ⎢                                      ⎥
    ⎢     0            0        1.0    0   ⎥
    ⎢                                      ⎥
    ⎣     0            0         0    1.0  ⎦


    Matrix t_3_4:

    ⎡1.0⋅cos(θ₄)  -1.0⋅sin(θ₄)   0   1.0⋅l₃ ⎤
    ⎢                                      ⎥
    ⎢1.0⋅sin(θ₄)  1.0⋅cos(θ₄)    0     0    ⎥
    ⎢                                      ⎥
    ⎢     0            0        1.0    0   ⎥
    ⎢                                      ⎥
    ⎣     0            0         0    1.0  ⎦


    Matrix t_4_5:

    ⎡1.0   0    0   1.0⋅l₄⎤
    ⎢                     ⎥
    ⎢ 0   1.0   0     0   ⎥
    ⎢                     ⎥
    ⎢ 0    0   1.0    0   ⎥
    ⎢                     ⎥
    ⎣ 0    0    0    1.0  ⎦


    Matrix T_0_5: with substitutions Round

    ⎡1.0⋅cos(θ₁)⋅cos(θ₂ + θ₃ + θ₄)  -1.0⋅sin(θ₂ + θ₃ + θ₄)⋅cos(θ₁)  1.0⋅sin(θ₁)      1.0⋅(l₂⋅cos(θ₂) + l₃⋅cos(θ₂ + θ₃) + l₄⋅cos(θ₂ + θ₃ + θ₄))⋅cos(θ₁)        ⎤
    ⎢                                                                                                                                                     ⎥
    ⎢1.0⋅sin(θ₁)⋅cos(θ₂ + θ₃ + θ₄)  -1.0⋅sin(θ₁)⋅sin(θ₂ + θ₃ + θ₄)  -1.0⋅cos(θ₁)     1.0⋅(l₂⋅cos(θ₂) + l₃⋅cos(θ₂ + θ₃) + l₄⋅cos(θ₂ + θ₃ + θ₄))⋅sin(θ₁)        ⎥
    ⎢                                                                                                                                                     ⎥
    ⎢    1.0⋅sin(θ₂ + θ₃ + θ₄)          1.0⋅cos(θ₂ + θ₃ + θ₄)            0        1.0⋅l₁ + 1.0⋅l₂⋅sin(θ₂) + 1.0⋅l₃⋅sin(θ₂ + θ₃) + 1.0⋅l₄⋅sin(θ₂ + θ₃ + θ₄)    ⎥
    ⎢                                                                                                                                                     ⎥
    ⎣              0                              0                      0                                          1.0                                   ⎦


    Matrix T_0_5: with substitutions Round

    ⎡1.0⋅cos(θ₁)⋅cos(θ₂ + θ₃ + θ₄)  -1.0⋅sin(θ₂ + θ₃ + θ₄)⋅cos(θ₁)  1.0⋅sin(θ₁)   (500.0⋅cos(θ₂) + 500.0⋅cos(θ₂ + θ₃) + 180.0⋅cos(θ₂ + θ₃ + θ₄))⋅cos(θ₁)    ⎤
    ⎢                                                                                                                                                   ⎥
    ⎢1.0⋅sin(θ₁)⋅cos(θ₂ + θ₃ + θ₄)  -1.0⋅sin(θ₁)⋅sin(θ₂ + θ₃ + θ₄)  -1.0⋅cos(θ₁)  (500.0⋅cos(θ₂) + 500.0⋅cos(θ₂ + θ₃) + 180.0⋅cos(θ₂ + θ₃ + θ₄))⋅sin(θ₁)    ⎥
    ⎢                                                                                                                                                   ⎥
    ⎢    1.0⋅sin(θ₂ + θ₃ + θ₄)          1.0⋅cos(θ₂ + θ₃ + θ₄)            0         500.0⋅sin(θ₂) + 500.0⋅sin(θ₂ + θ₃) + 180.0⋅sin(θ₂ + θ₃ + θ₄) + 230.0   ⎥
    ⎢                                                                                                                                                   ⎥
    ⎣              0                              0                      0                                         1.0                                  ⎦


    Matrix T_1_5:

    ⎡1.0⋅cos(θ₂ + θ₃ + θ₄)  -1.0⋅sin(θ₂ + θ₃ + θ₄)   0    1.0⋅l₂⋅cos(θ₂) + 1.0⋅l₃⋅cos(θ₂ + θ₃) + 1.0⋅l₄⋅cos(θ₂ + θ₃ + θ₄)    ⎤
    ⎢                                                                                                                    ⎥
    ⎢          0                      0             -1.0                                 0                               ⎥
    ⎢                                                                                                                    ⎥
    ⎢1.0⋅sin(θ₂ + θ₃ + θ₄)  1.0⋅cos(θ₂ + θ₃ + θ₄)   0.0   1.0⋅l₂⋅sin(θ₂) + 1.0⋅l₃⋅sin(θ₂ + θ₃) + 1.0⋅l₄⋅sin(θ₂ + θ₃ + θ₄)    ⎥
    ⎢                                                                                                                    ⎥
    ⎣          0                      0              0                                  1.0                              ⎦


    Matrix T_1_5: for theta_1 = 0

    ⎡1.0⋅cos(θ₂ + θ₃ + θ₄)  -1.0⋅sin(θ₂ + θ₃ + θ₄)   0    1.0⋅l₂⋅cos(θ₂) + 1.0⋅l₃⋅cos(θ₂ + θ₃) + 1.0⋅l₄⋅cos(θ₂ + θ₃ + θ₄)    ⎤
    ⎢                                                                                                                    ⎥
    ⎢          0                      0             -1.0                                 0                               ⎥
    ⎢                                                                                                                    ⎥
    ⎢1.0⋅sin(θ₂ + θ₃ + θ₄)  1.0⋅cos(θ₂ + θ₃ + θ₄)   0.0   1.0⋅l₂⋅sin(θ₂) + 1.0⋅l₃⋅sin(θ₂ + θ₃) + 1.0⋅l₄⋅sin(θ₂ + θ₃ + θ₄)    ⎥
    ⎢                                                                                                                    ⎥
    ⎣          0                      0              0                                  1.0                              ⎦


    Matrix T_1_5: for theta_1 = 0

    ⎡1.0⋅cos(θ₂ + θ₃ + θ₄)  -1.0⋅sin(θ₂ + θ₃ + θ₄)   0    1.0⋅l₂⋅cos(θ₂) + 1.0⋅l₃⋅cos(θ₂ + θ₃) + 1.0⋅l₄⋅cos(θ₂ + θ₃ + θ₄)    ⎤
    ⎢                                                                                                                    ⎥
    ⎢          0                      0             -1.0                                 0                               ⎥
    ⎢                                                                                                                    ⎥
    ⎢1.0⋅sin(θ₂ + θ₃ + θ₄)  1.0⋅cos(θ₂ + θ₃ + θ₄)   0.0   1.0⋅l₂⋅sin(θ₂) + 1.0⋅l₃⋅sin(θ₂ + θ₃) + 1.0⋅l₄⋅sin(θ₂ + θ₃ + θ₄)    ⎥
    ⎢                                                                                                                    ⎥
    ⎣          0                      0              0                                  1.0                              ⎦


    Matrix T_1_4:

    ⎡1.0⋅cos(θ₂ + θ₃ + θ₄)  -1.0⋅sin(θ₂ + θ₃ + θ₄)   0    1.0⋅l₂⋅cos(θ₂) + 1.0⋅l₃⋅cos(θ₂ + θ₃)   ⎤
    ⎢                                                                                         ⎥
    ⎢          0                      0             -1.0                   0                  ⎥
    ⎢                                                                                         ⎥
    ⎢1.0⋅sin(θ₂ + θ₃ + θ₄)  1.0⋅cos(θ₂ + θ₃ + θ₄)   0.0   1.0⋅l₂⋅sin(θ₂) + 1.0⋅l₃⋅sin(θ₂ + θ₃)   ⎥
    ⎢                                                                                         ⎥
    ⎣          0                      0              0                    1.0                 ⎦


    Matrix t_3_5:

    ⎡1.0⋅cos(θ₃)⋅cos(θ₃ + θ₄)  1.0⋅sin(θ₃)⋅cos(θ₃ + θ₄)   -sin(θ₃ + θ₄)    1.0⋅l₂⋅cos(θ₂) - l₂⋅cos(θ₃)⋅cos(θ₃ + θ₄) + 1.0⋅l₃⋅cos(θ₂ + θ₃) + 1.0⋅l₄⋅cos(θ₂ + θ₃ + θ₄)     ⎤
    ⎢                                                                                                                                                               ⎥
    ⎢        -sin(θ₃)                1.0⋅cos(θ₃)               0.0                                              1.0⋅l₂⋅sin(θ₃)                                       ⎥
    ⎢                                                                                                                                                               ⎥
    ⎢1.0⋅sin(θ₃ + θ₄)⋅cos(θ₃)  1.0⋅sin(θ₃)⋅sin(θ₃ + θ₄)  1.0⋅cos(θ₃ + θ₄)  1.0⋅l₂⋅sin(θ₂) - l₂⋅sin(θ₃ + θ₄)⋅cos(θ₃) + 1.0⋅l₃⋅sin(θ₂ + θ₃) + 1.0⋅l₄⋅sin(θ₂ + θ₃ + θ₄)      ⎥
    ⎢                                                                                                                                                               ⎥
    ⎣           0                         0                     0                                                     1.0                                           ⎦