.. _coordTrans2d:

Coordinate Transformations in 2-D
=========================================

.. seealso::
    
    This `video by Peter Corke
    <https://www.youtube.com/watch?v=UpiRBNfK1f0>`_ discusses
    transformation matrices to apply both translation and rotation.

The calculation is different when we apply a transformation matrix to a
coordinate frame to create a new, translated, and rotated coordinate frame. We
read the order of transformations from left to right. To build a compound
coordinate frame, we start with the previous coordinate frame
and then multiply each sequential transform matrix until we reach the desired
coordinate frame position.


.. note::

    The base or world coordinate frame matrix is an identity matrix, so it
    is not necessary to specify it as the starting coordinate frame.

.. _coordTransform:

.. figure:: coordTransform.png
    :align: center
    :width: 40%

    A transformation that traverses a straight rigid body to a new coordinate
    frame at the other end of the rigid body rotates the frame and then
    translates the frame along the :math:`x`–axis.

Consider the pose at the end of a single robotic arm as shown in
:numref:`fig-oneArm`.  Since the rotation changes the orientation of the
coordinate frame, the translation is only along the :math:`\bm{x}`--axis.  If a
rigid body also has a displacement in the direction of the
:math:`\bm{y}`--axis, we can either use a second translation matrix, or specify
the translation matrix as having both :math:`x` and :math:`y` components.

.. math:: {}^WT_E = \spalignmat[r]{\cos\theta, -\sin\theta, 0;
                \sin\theta, \cos\theta, 0; 0, 0, 1} \,
        \spalignmat{1, 0, a; 0, 1, 0; 0, 0, 1} =
        \spalignmat[r]{\cos\theta, -\sin\theta, a\,\cos\theta;
                \sin\theta, \cos\theta, a\,\sin\theta; 0, 0, 1} \,

.. _fig-oneArm:

.. figure:: oneArm.png
   :align: center
   :width: 70%

   The pose of the end effector is represented by the coordinate frame
   :math:`\{E\}`, which is defined relative to the world frame by the
   homogeneous coordinate transformation 
   :math:`{}^W\mathbf{T}_E = R(\theta) Tx(a)`.

We read the nomenclature of the coordinate frame transformation
:math:`{}^W\mathbf{T}_E` as *the transformation with respect to the world
frame,* :math:`\{W\}`, *yielding frame* :math:`\{E\}`. The reference frame is a
prescript to the transformation symbol :math:`\mathbf{T}`, while the label of
the resulting frame is a subscript.

For serial-link robot arms, we define a coordinate frame for each joint of the
robot. As shown in :numref:`fig-twoArm`, the composite coordinate frames are
products of the transformation matrices from the world coordinate frame to each
joint.

.. _fig-twoArm:

.. figure:: twoArm.png
    :align: center
    :width: 95%

    A coordinate frame for each joint is created by multiplicaiton of
    transform  matrices.

If we know the position of a point relative to a transformed coordinate frame,
then the point's location in the world coordinate frame is the product of the
transformation matrices and the point location in the transformed coordinate
frame. However, if we know a point location in the world coordinate frame 
and want its location in another coordinate frame, we multiply the point
location by the inverse of the transformation.

::

    >> q1 = 30; % degrees
    >> q2 = -15
    >> a = 8
    >> b = 3.5000
    >> RA
    RA =
        0.8660   -0.5000         0
        0.5000    0.8660         0
             0         0    1.0000
    >> TA
    TA =
        1     0     8
        0     1     0
        0     0     1
    >> WTA = RA*TA
    WTA =
        0.8660   -0.5000    6.9282
        0.5000    0.8660    4.0000
             0         0    1.0000
    >> RB
    RB =
        0.9659    0.2588         0
       -0.2588    0.9659         0
             0         0    1.0000
    >> TB
    TB =
        1.0000         0    3.5000
             0    1.0000         0
             0         0    1.0000
    >> ATB = RB*TB
    ATB =
        0.9659    0.2588    3.3807
       -0.2588    0.9659   -0.9059
             0         0    1.0000
    >> B_frame = WTA*ATB
    B_frame =
        0.9659   -0.2588   10.3089
        0.2588    0.9659    4.9059
             0         0    1.0000

    % point in B to World
    >> p1_B = [1;1;1]; % homogeneous
    >> p1_W = B_frame*p1_B
    p1_W =
       11.0161
        6.1306
        1.0000

    % point in World to B
    >> p2_W = [12;7;1];
    >> p2_B = inv(B_frame)*p2_W
    p2_B =
        2.1754
        1.5851
        1.0000