:github_url: https://github.com/kevinzakka/mink/tree/main/docs/tutorial/lie_groups.rst

.. _lie-groups:

====================
Rotations and Poses
====================

mink represents rotations and rigid transforms using Lie groups:

- :class:`~mink.SO3` for 3D rotations
- :class:`~mink.SE3` for 3D rigid body transforms (rotation + translation)

These classes are used throughout mink to represent frame poses, specify targets,
and compute pose errors consistently.

This page focuses on *how to use* these objects.

Conventions
===========

Quaternions
-----------

mink uses MuJoCo's quaternion convention: **scalar-first** quaternions
``(qw, qx, qy, qz)`` (also written ``wxyz``).

Composition and action via ``@``
--------------------------------

mink overloads the ``@`` operator:

- ``T1 @ T2`` composes two transformations.
- ``T @ p`` applies a transformation to a point ``p``.
- Similarly for rotations: ``R1 @ R2`` and ``R @ v``.

This mirrors common robotics notation.

Composition follows standard robotics conventions: ``A @ B`` means "apply B,
then A".

SO3: Rotations
==============

Creating rotations
------------------

Use the exponential map for axis-angle rotations:

.. code-block:: python

   import numpy as np
   import mink

   # Rotation from a tangent vector (axis-angle, radians).
   R = mink.SO3.exp(np.array([0.0, 0.0, np.pi / 2]))

Convenience constructors exist for common axes and roll/pitch/yaw:

.. code-block:: python

   Rz = mink.SO3.from_z_radians(np.pi / 2)
   Rrpy = mink.SO3.from_rpy_radians(roll=0.1, pitch=0.2, yaw=-0.3)

Identity and normalization:

.. code-block:: python

   R = mink.SO3.identity()
   R = R.normalize()

Applying and composing
----------------------

Apply a rotation to a vector:

.. code-block:: python

   v = np.array([1.0, 0.0, 0.0])
   v_rot = Rz @ v

Compose rotations (right-multiplication):

.. code-block:: python

   R = mink.SO3.from_z_radians(0.3) @ mink.SO3.from_x_radians(-0.2)

Log/Exp (tangent space)
-----------------------

``log()`` maps a rotation to a 3D tangent vector (axis-angle),
and ``exp()`` maps a tangent vector back to a rotation:

.. code-block:: python

   omega = R.log()          # shape (3,)
   R_recovered = mink.SO3.exp(omega)

These are useful for expressing *small* rotation errors and for interpolation.

SE3: Rigid Body Transformations
===============================

Creating transforms
-------------------

An :class:`~mink.SE3` stores rotation + translation. Use:

- :meth:`~mink.SE3.from_rotation_and_translation`
- :meth:`~mink.SE3.from_translation`
- :meth:`~mink.SE3.from_rotation`

.. code-block:: python

   import numpy as np
   import mink

   R = mink.SO3.from_z_radians(np.pi / 4)
   t = np.array([0.2, 0.0, 0.1])

   T = mink.SE3.from_rotation_and_translation(R, t)


Example: building a pose target
-------------------------------

The following example shows how to construct a nearby pose target in the
world frame using SE(3) composition.

.. code-block:: python

   T_current = configuration.get_transform_frame_to_world("ee", "site")
   T_target = mink.SE3.from_translation(np.array([0.0, 0.0, 0.1])) @ T_current
   frame_task.set_target(T_target)

Composition: world vs local updates
-----------------------------------

The most common source of confusion is where an increment is applied.

Let ``T`` be a pose. Consider a small translation transform ``Δ``:

.. code-block:: python

   Δ = mink.SE3.from_translation(np.array([0.0, 0.0, 0.1]))

- **Left-multiplying** applies the increment in the **world frame**:

  .. code-block:: python

     T_world = Δ @ T

- **Right-multiplying** applies the increment in the **local frame**:

  .. code-block:: python

     T_local = T @ Δ

This is the same pattern used in the quickstart when composing translations in
world coordinates and rotations in the end-effector's local frame.

Applying a transform
--------------------

.. code-block:: python

   p = np.array([0.0, 0.0, 0.0])
   p_world = T @ p

Access rotation and translation:

.. code-block:: python

   R = T.rotation()
   t = T.translation()

Pose differences and task errors (right-minus)
==============================================

For :class:`~mink.SE3`, the tangent vector is ordered as
:math:`(v_x, v_y, v_z, \omega_x, \omega_y, \omega_z)`,
i.e., translation first, then rotation.

For IK, mink represents pose errors using the **right-minus**
operator (also called a body-frame error):

.. math::

   T_1 \ominus T_2 = \log(T_2^{-1} T_1).

In code, this corresponds to :meth:`~mink.MatrixLieGroup.rminus`:

.. code-block:: python

   e = T.rminus(T_target)  # shape (6,) for SE3

Intuitively, ``T.rminus(T_target)`` answers the question:

   From my current pose ``T``, what small motion (twist) should I apply to
   myself to reach ``T_target``?

The resulting error is expressed in the **current (body) frame**: translations
are along the frame's local axes, and rotations are about its local axes. This
matches mink's body-frame Jacobians, which map joint velocities to body-frame
twists.

By contrast, ``T_target.rminus(T)`` would express the error in the **target
frame**, answering “where is the robot relative to the target?”. While this is a
valid quantity, it is not the convention used by mink's inverse kinematics.

In particular, :class:`~mink.FrameTask` uses:

.. math::

   e = T_{0t} \ominus T_{0b} = \log(T_{0b}^{-1} T_{0t}),

which is a 6D twist error consistent with mink's body-frame Jacobians.

Internally, :class:`~mink.FrameTask` uses :meth:`~mink.MatrixLieGroup.jlog` to
linearize the log-map error; most users do not need to call it directly.


Interpolation
=============

You can interpolate on the manifold using:

.. code-block:: python

   T_mid = T0.interpolate(T1, alpha=0.5)

This is useful for smooth target trajectories without linearizing rotations.

Further Reading
===============

For Lie group fundamentals and Jacobians (left/right Jacobians, plus/minus),
see:

- `A micro Lie theory <https://arxiv.org/abs/1812.01537>`_