# Part of Spatial Math Toolbox for Python

# Copyright (c) 2000 Peter Corke

# MIT Licence, see details in top-level file: LICENCE

"""

These functions create and manipulate 2D rotation matrices and rigid-body

transformations as 2x2 SO(2) matrices and 3x3 SE(2) matrices respectively.

These matrices are represented as 2D NumPy arrays.

Vector arguments are what numpy refers to as ``array_like`` and can be a list,

tuple, numpy array, numpy row vector or numpy column vector.

"""

# pylint: disable=invalid-name

import sys

import math

import numpy as np

try:

import matplotlib.pyplot as plt

_matplotlib_exists = True

except ImportError:

_matplotlib_exists = False

import spatialmath.base as smb

from spatialmath.base.types import *

from spatialmath.base.transformsNd import rt2tr

from spatialmath.base.vectors import unitvec

_eps = np.finfo(np.float64).eps

try: # pragma: no cover

# print('Using SymPy')

import sympy

_symbolics = True

except ImportError: # pragma: no cover

_symbolics = False

# ---------------------------------------------------------------------------------------#

def rot2(theta: float, unit: str = "rad") -> SO2Array:

"""

Create SO(2) rotation

:param theta: rotation angle

:type theta: float

:param unit: angular units: 'rad' [default], or 'deg'

:type unit: str

:return: SO(2) rotation matrix

:rtype: ndarray(2,2)

- ``rot2(θ)`` is an SO(2) rotation matrix (2x2) representing a rotation of θ radians.

- ``rot2(θ, 'deg')`` as above but θ is in degrees.

.. runblock:: pycon

>>> from spatialmath.base import *

>>> rot2(0.3)

>>> rot2(45, 'deg')

"""

theta = smb.getunit(theta, unit, vector=False)

ct = smb.sym.cos(theta)

st = smb.sym.sin(theta)

# fmt: off

R = np.array([

[ct, -st],

[st, ct]])

# fmt: on

return R

# ---------------------------------------------------------------------------------------#

def trot2(theta: float, unit: str = "rad", t: Optional[ArrayLike2] = None) -> SE2Array:

"""

Create SE(2) pure rotation

:param theta: rotation angle about X-axis

:type θ: float

:param unit: angular units: 'rad' [default], or 'deg'

:type unit: str

:param t: 2D translation vector, defaults to [0,0]

:type t: array_like(2)

:return: 3x3 homogeneous transformation matrix

:rtype: ndarray(3,3)

- ``trot2(θ)`` is a homogeneous transformation (3x3) representing a rotation of

θ radians.

- ``trot2(θ, 'deg')`` as above but θ is in degrees.

.. runblock:: pycon

>>> from spatialmath.base import *

>>> trot2(0.3)

>>> trot2(45, 'deg', t=[1,2])

.. note:: By default, the translational component is zero but it can be

set to a non-zero value.

:seealso: xyt2tr

"""

T = np.pad(rot2(theta, unit), (0, 1), mode="constant")

if t is not None:

T[:2, 2] = smb.getvector(t, 2, "array")

T[2, 2] = 1 # integer to be symbolic friendly

return T

def xyt2tr(xyt: ArrayLike3, unit: str = "rad") -> SE2Array:

"""

Create SE(2) pure rotation

:param xyt: 2d translation and rotation

:type xyt: array_like(3)

:param unit: angular units: 'rad' [default], or 'deg'

:type unit: str

:return: SE(2) matrix

:rtype: ndarray(3,3)

- ``xyt2tr([x,y,θ])`` is a homogeneous transformation (3x3) representing a rotation of

θ radians and a translation of (x,y).

.. runblock:: pycon

>>> from spatialmath.base import *

>>> xyt2tr([1,2,0.3])

>>> xyt2tr([1,2,45], 'deg')

:seealso: tr2xyt

"""

xyt = smb.getvector(xyt, 3)

T = np.pad(rot2(xyt[2], unit), (0, 1), mode="constant")

T[:2, 2] = xyt[0:2]

T[2, 2] = 1.0

return T

def tr2xyt(T: SE2Array, unit: str = "rad") -> R3:

"""

Convert SE(2) to x, y, theta

:param T: SE(2) matrix

:type T: ndarray(3,3)

:param unit: angular units: 'rad' [default], or 'deg'

:type unit: str

:return: [x, y, θ]

:rtype: ndarray(3)

- ``tr2xyt(T)`` is a vector giving the equivalent 2D translation and

rotation for this SO(2) matrix.

.. runblock:: pycon

>>> from spatialmath.base import *

>>> T = xyt2tr([1, 2, 0.3])

>>> T

>>> tr2xyt(T)

:seealso: trot2

"""

if T.dtype == "O" and _symbolics:

angle = sympy.atan2(T[1, 0], T[0, 0])

else:

angle = math.atan2(T[1, 0], T[0, 0])

return np.r_[T[0, 2], T[1, 2], angle]

# ---------------------------------------------------------------------------------------#

@overload # pragma: no cover

def transl2(x: float, y: float) -> SE2Array:

...

@overload # pragma: no cover

def transl2(x: ArrayLike2) -> SE2Array:

...

@overload # pragma: no cover

def transl2(x: SE2Array) -> R2:

...

def transl2(x, y=None):

"""

Create SE(2) pure translation, or extract translation from SE(2) matrix

**Create a translational SE(2) matrix**

:param x: translation along X-axis

:type x: float

:param y: translation along Y-axis

:type y: float

:return: SE(2) matrix

:rtype: ndarray(3,3)

- ``T = transl2([X, Y])`` is an SE(2) homogeneous transform (3x3)

representing a pure translation.

- ``T = transl2( V )`` as above but the translation is given by a 2-element

list, dict, or a numpy array, row or column vector.

.. runblock:: pycon

>>> from spatialmath.base import *

>>> import numpy as np

>>> transl2(3, 4)

>>> transl2([3, 4])

>>> transl2(np.array([3, 4]))

**Extract the translational part of an SE(2) matrix**

:param x: SE(2) transform matrix

:type x: ndarray(3,3)

:return: translation elements of SE(2) matrix

:rtype: ndarray(2)

- ``t = transl2(T)`` is the translational part of the SE(3) matrix ``T`` as a

2-element NumPy array.

.. runblock:: pycon

>>> from spatialmath.base import *

>>> import numpy as np

>>> T = np.array([[1, 0, 3], [0, 1, 4], [0, 0, 1]])

>>> transl2(T)

.. note:: This function is compatible with the MATLAB version of the Toolbox. It

is unusual/weird in doing two completely different things inside the one

function.

:seealso: :func:`tr2pos2` :func:`pos2tr2`

"""

if smb.isscalar(x) and smb.isscalar(y):

# (x, y) -> SE(2)

t = np.array([x, y])

elif smb.isvector(x, 2):

# R2 -> SE(2)

t = cast(NDArray, smb.getvector(x, 2))

elif smb.ismatrix(x, (3, 3)):

# SE(2) -> R2

return x[:2, 2]

else:

raise ValueError("bad argument")

if t.dtype != "O":

t = t.astype("float64")

T = np.identity(3, dtype=t.dtype)

T[:2, 2] = t

return T

def tr2pos2(T):

"""

Extract translation from SE(2) matrix

:param x: SE(2) transform matrix

:type x: ndarray(3,3)

:return: translation elements of SE(2) matrix

:rtype: ndarray(2)

- ``t = tr2pos2(T)`` is the translational part of the SE(3) matrix ``T`` as a

2-element NumPy array.

.. runblock:: pycon

>>> from spatialmath.base import *

>>> import numpy as np

>>> T = np.array([[1, 0, 3], [0, 1, 4], [0, 0, 1]])

>>> tr2pos2(T)

:seealso: :func:`pos2tr2` :func:`transl2`

"""

return T[:2, 2]

def pos2tr2(x, y=None):

"""

Create a translational SE(2) matrix

:param x: translation along X-axis

:type x: float

:param y: translation along Y-axis

:type y: float

:return: SE(2) matrix

:rtype: ndarray(3,3)

- ``T = pos2tr2([X, Y])`` is an SE(2) homogeneous transform (3x3)

representing a pure translation.

- ``T = pos2tr2( V )`` as above but the translation is given by a 2-element

list, dict, or a numpy array, row or column vector.

.. runblock:: pycon

>>> from spatialmath.base import *

>>> import numpy as np

>>> pos2tr2(3, 4)

>>> pos2tr2([3, 4])

>>> pos2tr2(np.array([3, 4]))

:seealso: :func:`tr2pos2` :func:`transl2`

"""

if smb.isscalar(x) and smb.isscalar(y):

# (x, y) -> SE(2)

t = np.r_[x, y]

elif smb.isvector(x, 2):

# R2 -> SE(2)

t = cast(NDArray, smb.getvector(x, 2))

else:

raise ValueError("bad argument")

if t.dtype != "O":

t = t.astype("float64")

T = np.identity(3, dtype=t.dtype)

T[:2, 2] = t

return T

def ishom2(T: Any, check: bool = False, tol: float = 20) -> bool: # TypeGuard(SE2):

"""

Test if matrix belongs to SE(2)

:param T: SE(2) matrix to test

:type T: ndarray(3,3)

:param check: check validity of rotation submatrix

:type check: bool

:param tol: Tolerance in units of eps for zero-rotation case, defaults to 20

:type: float

:return: whether matrix is an SE(2) homogeneous transformation matrix

:rtype: bool

- ``ishom2(T)`` is True if the argument ``T`` is of dimension 3x3

- ``ishom2(T, check=True)`` as above, but also checks orthogonality of the

rotation sub-matrix and validitity of the bottom row.

.. runblock:: pycon

>>> from spatialmath.base import *

>>> import numpy as np

>>> T = np.array([[1, 0, 3], [0, 1, 4], [0, 0, 1]])

>>> ishom2(T)

>>> T = np.array([[1, 1, 3], [0, 1, 4], [0, 0, 1]]) # invalid SE(2)

>>> ishom2(T) # a quick check says it is an SE(2)

>>> ishom2(T, check=True) # but if we check more carefully...

>>> R = np.array([[1, 0], [0, 1]])

>>> ishom2(R)

:seealso: isR, isrot2, ishom, isvec

"""

return (

isinstance(T, np.ndarray)

and T.shape == (3, 3)

and (

not check

or (smb.isR(T[:2, :2], tol=tol) and all(T[2, :] == np.array([0, 0, 1])))

)

)

def isrot2(R: Any, check: bool = False, tol: float = 20) -> bool: # TypeGuard(SO2):

"""

Test if matrix belongs to SO(2)

:param R: SO(2) matrix to test

:type R: ndarray(3,3)

:param check: check validity of rotation submatrix

:type check: bool

:param tol: Tolerance in units of eps for zero-rotation case, defaults to 20

:type: float

:return: whether matrix is an SO(2) rotation matrix

:rtype: bool

- ``isrot2(R)`` is True if the argument ``R`` is of dimension 2x2

- ``isrot2(R, check=True)`` as above, but also checks orthogonality of the rotation matrix.

.. runblock:: pycon

>>> from spatialmath.base import *

>>> import numpy as np

>>> T = np.array([[1, 0, 3], [0, 1, 4], [0, 0, 1]])

>>> isrot2(T)

>>> R = np.array([[1, 0], [0, 1]])

>>> isrot2(R)

>>> R = np.array([[1, 1], [0, 1]]) # invalid SO(2)

>>> isrot2(R) # a quick check says it is an SO(2)

>>> isrot2(R, check=True) # but if we check more carefully...

:seealso: isR, ishom2, isrot

"""

return (

isinstance(R, np.ndarray)

and R.shape == (2, 2)

and (not check or smb.isR(R, tol=tol))

)

# ---------------------------------------------------------------------------------------#

def trinv2(T: SE2Array) -> SE2Array:

r"""

Invert an SE(2) matrix

:param T: SE(2) matrix

:type T: ndarray(3,3)

:return: inverse of SE(2) matrix

:rtype: ndarray(3,3)

:raises ValueError: bad arguments

Computes an efficient inverse of an SE(2) matrix:

:math:`\begin{pmatrix} {\bf R} & t \\ 0\,0 & 1 \end{pmatrix}^{-1} = \begin{pmatrix} {\bf R}^T & -{\bf R}^T t \\ 0\, 0 & 1 \end{pmatrix}`

.. runblock:: pycon

>>> from spatialmath.base import *

>>> T = trot2(0.3, t=[4,5])

>>> trinv2(T)

>>> T @ trinv2(T)

:SymPy: supported

"""

if not ishom2(T):

raise ValueError("expecting SE(2) matrix")

# inline this code for speed, don't use tr2rt and rt2tr

R = T[:2, :2]

t = T[:2, 2]

Ti = np.zeros((3, 3), dtype=T.dtype)

Ti[:2, :2] = R.T

Ti[:2, 2] = -R.T @ t

Ti[2, 2] = 1

return Ti

@overload # pragma: no cover

def trlog2(

T: SO2Array,

twist: bool = False,

check: bool = True,

tol: float = 20,

) -> so2Array:

...

@overload # pragma: no cover

def trlog2(

T: SE2Array,

twist: bool = False,

check: bool = True,

tol: float = 20,

) -> se2Array:

...

@overload # pragma: no cover

def trlog2(

T: SO2Array,

twist: bool = True,

check: bool = True,

tol: float = 20,

) -> float:

...

@overload # pragma: no cover

def trlog2(

T: SE2Array,

twist: bool = True,

check: bool = True,

tol: float = 20,

) -> R3:

...

def trlog2(

T: Union[SO2Array, SE2Array],

twist: bool = False,

check: bool = True,

tol: float = 20,

) -> Union[float, R3, so2Array, se2Array]:

"""

Logarithm of SO(2) or SE(2) matrix

:param T: SE(2) or SO(2) matrix

:type T: ndarray(3,3) or ndarray(2,2)

:param check: check that matrix is valid

:type check: bool

:param twist: return a twist vector instead of matrix [default]

:type twist: bool

:param tol: Tolerance in units of eps for zero-rotation case, defaults to 20

:type: float

:return: logarithm

:rtype: ndarray(3,3) or ndarray(3); or ndarray(2,2) or ndarray(1)

:raises ValueError: bad argument

An efficient closed-form solution of the matrix logarithm for arguments that

are SO(2) or SE(2).

- ``trlog2(R)`` is the logarithm of the passed rotation matrix ``R`` which

will be 2x2 skew-symmetric matrix. The equivalent vector from ``vex()``

is parallel to rotation axis and its norm is the amount of rotation about

that axis.

- ``trlog(T)`` is the logarithm of the passed homogeneous transformation

matrix ``T`` which will be 3x3 augumented skew-symmetric matrix. The

equivalent vector from ``vexa()`` is the twist vector (6x1) comprising [v

w].

.. runblock:: pycon

>>> from spatialmath.base import *

>>> trlog2(trot2(0.3))

>>> trlog2(trot2(0.3), twist=True)

>>> trlog2(rot2(0.3))

>>> trlog2(rot2(0.3), twist=True)

:seealso: :func:`~trexp`, :func:`~spatialmath.base.transformsNd.vex`,

:func:`~spatialmath.base.transformsNd.vexa`

"""

if ishom2(T, check=check, tol=tol):

# SE(2) matrix

if smb.iseye(T, tol=tol):

# is identity matrix

if twist:

return np.zeros((3,))

else:

return np.zeros((3, 3))

else:

st = T[1, 0]

ct = T[0, 0]

theta = math.atan(st / ct)

if abs(theta) < tol * _eps:

tr = T[:2, 2].flatten()

else:

V = np.array([[st, -(1 - ct)], [1 - ct, st]])

tr = (np.linalg.inv(V) @ T[:2, 2]) * theta

if twist:

return np.hstack([tr, theta])

else:

return np.block(

[[smb.skew(theta), tr[:, np.newaxis]], [np.zeros((1, 3))]]

)

elif isrot2(T, check=check, tol=tol):

# SO(2) rotation matrix

theta = math.atan(T[1, 0] / T[0, 0])

if twist:

return theta

else:

return smb.skew(theta)

else:

raise ValueError("Expect SO(2) or SE(2) matrix")

# ---------------------------------------------------------------------------------------#

@overload # pragma: no cover

def trexp2(S: so2Array, theta: Optional[float] = None, check: bool = True) -> SO2Array:

...

@overload # pragma: no cover

def trexp2(S: se2Array, theta: Optional[float] = None, check: bool = True) -> SE2Array:

...

def trexp2(

S: Union[so2Array, se2Array],

theta: Optional[float] = None,

check: bool = True,

) -> Union[SO2Array, SE2Array]:

"""

Exponential of so(2) or se(2) matrix

:param S: se(2), so(2) matrix or equivalent vector

:type T: ndarray(3,3) or ndarray(2,2)

:param theta: motion

:type theta: float

:return: matrix exponential in SE(2) or SO(2)

:rtype: ndarray(3,3) or ndarray(2,2)

:raises ValueError: bad argument

An efficient closed-form solution of the matrix exponential for arguments

that are se(2) or so(2).

For se(2) the results is an SE(2) homogeneous transformation matrix:

- ``trexp2(Σ)`` is the matrix exponential of the se(2) element ``Σ`` which is

a 3x3 augmented skew-symmetric matrix.

- ``trexp2(Σ, θ)`` as above but for an se(3) motion of Σθ, where ``Σ``

must represent a unit-twist, ie. the rotational component is a unit-norm skew-symmetric

matrix.

- ``trexp2(S)`` is the matrix exponential of the se(2) element ``S`` represented as

a 3-vector which can be considered a screw motion.

- ``trexp2(S, θ)`` as above but for an se(2) motion of Sθ, where ``S``

must represent a unit-twist, ie. the rotational component is a unit-norm skew-symmetric

matrix.

.. runblock:: pycon

>>> from spatialmath.base import *

>>> trexp2(skew(1))

>>> trexp2(skew(1), 2) # revolute unit twist

>>> trexp2(1)

>>> trexp2(1, 2) # revolute unit twist

For so(2) the results is an SO(2) rotation matrix:

- ``trexp2(Ω)`` is the matrix exponential of the so(3) element ``Ω`` which is a 2x2

skew-symmetric matrix.

- ``trexp2(Ω, θ)`` as above but for an so(3) motion of Ωθ, where ``Ω`` is

unit-norm skew-symmetric matrix representing a rotation axis and a rotation magnitude

given by ``θ``.

- ``trexp2(ω)`` is the matrix exponential of the so(2) element ``ω`` expressed as

a 1-vector.

- ``trexp2(ω, θ)`` as above but for an so(3) motion of ωθ where ``ω`` is a

unit-norm vector representing a rotation axis and a rotation magnitude

given by ``θ``. ``ω`` is expressed as a 1-vector.

.. runblock:: pycon

>>> from spatialmath.base import *

>>> trexp2(skewa([1, 2, 3]))

>>> trexp2(skewa([1, 0, 0]), 2) # prismatic unit twist

>>> trexp2([1, 2, 3])

>>> trexp2([1, 0, 0], 2)

:seealso: trlog, trexp2

"""

if smb.ismatrix(S, (3, 3)) or smb.isvector(S, 3):

# se(2) case

if smb.ismatrix(S, (3, 3)):

# augmentented skew matrix

if check and not smb.isskewa(S):

raise ValueError("argument must be a valid se(2) element")

tw = smb.vexa(cast(se2Array, S))

else:

# 3 vector

tw = smb.getvector(S)

if smb.iszerovec(tw):

return np.eye(3)

if theta is None:

(tw, theta) = smb.unittwist2_norm(tw)

elif not smb.isunittwist2(tw):

raise ValueError("If theta is specified S must be a unit twist")

t = tw[0:2]

w = tw[2]

R = smb.rot2(w * theta)

skw = smb.skew(w)

V = (

np.eye(2) * theta

+ (1.0 - math.cos(theta)) * skw

+ (theta - math.sin(theta)) * skw @ skw

)

return smb.rt2tr(R, V @ t)

elif smb.ismatrix(S, (2, 2)) or smb.isvector(S, 1):

# so(2) case

if smb.ismatrix(S, (2, 2)):

# skew symmetric matrix

if check and not smb.isskew(S):

raise ValueError("argument must be a valid so(2) element")

w = smb.vex(S)

else:

# 1 vector

w = smb.getvector(S)

if theta is not None:

if not smb.isunitvec(w):

raise ValueError("If theta is specified S must be a unit twist")

w *= theta

# compute rotation matrix, simpler than Rodrigues for 2D case

return smb.rot2(w[0])

else:

raise ValueError(" First argument must be SO(2), 1-vector, SE(2) or 3-vector")

@overload # pragma: no cover

def trnorm2(R: SO2Array) -> SO2Array:

...

def trnorm2(T: SE2Array) -> SE2Array:

r"""

Normalize an SO(2) or SE(2) matrix

:param T: SE(2) or SO(2) matrix

:type T: ndarray(3,3) or ndarray(2,2)

:return: normalized SE(2) or SO(2) matrix

:rtype: ndarray(3,3) or ndarray(2,2)

:raises ValueError: bad arguments

- ``trnorm(R)`` is guaranteed to be a proper orthogonal matrix rotation

matrix (2,2) which is *close* to the input matrix R (2,2).

- ``trnorm(T)`` as above but the rotational submatrix of the homogeneous

transformation T (3,3) is normalised while the translational part is

unchanged.

The steps in normalization are:

#. If :math:`\mathbf{R} = [a, b]`

#. Form unit vectors :math:`\hat{b}

#. Form the orthogonal planar vector :math:`\hat{a} = [\hat{b}_y -\hat{b}_x]`

#. Form the normalized SO(2) matrix :math:`\mathbf{R} = [\hat{a}, \hat{b}]`

.. runblock:: pycon

>>> from spatialmath.base import trnorm, troty

>>> from numpy import linalg

>>> T = trot2(45, 'deg', t=[3, 4])

>>> linalg.det(T[:2,:2]) - 1 # is a valid SO(3)

>>> T = T @ T @ T @ T @ T @ T @ T @ T @ T @ T @ T @ T @ T

>>> linalg.det(T[:2,:2]) - 1 # not quite a valid SE(2) anymore

>>> T = trnorm2(T)

>>> linalg.det(T[:2,:2]) - 1 # once more a valid SE(2)

.. note::

- Only the direction of a-vector (the z-axis) is unchanged.

- Used to prevent finite word length arithmetic causing transforms to

become 'unnormalized', ie. determinant :math:`\ne 1`.

"""

if not ishom2(T) and not isrot2(T):

raise ValueError("expecting SO(2) or SE(2)")

a = T[:, 0]

b = T[:, 1]

b = unitvec(b)

# fmt: off

R = np.array([

[ b[1], b[0]],

[-b[0], b[1]]

])

# fmt: on

if ishom2(T):

return rt2tr(cast(SO2Array, R), T[:2, 2])

else:

return R

@overload # pragma: no cover

def tradjoint2(T: SO2Array) -> R1x1:

...

@overload # pragma: no cover

def tradjoint2(T: SE2Array) -> R3x3:

...

def tradjoint2(T):

r"""

Adjoint matrix in 2D

:param T: SE(2) or SO(2) matrix

:type T: ndarray(3,3) or ndarray(2,2)

:return: adjoint matrix

:rtype: ndarray(3,3) or ndarray(1,1)

Computes an adjoint matrix that maps the Lie algebra between frames.

.. math:

Ad(\mat{T}) \vec{X} X = \vee \left( \mat{T} \skew{\vec{X} \mat{T}^{-1} \right)

where :math:`\mat{T} \in \SE2`.

``tr2jac2(T)`` is an adjoint matrix (6x6) that maps spatial velocity or

differential motion between frame {B} to frame {A} which are attached to the

same moving body. The pose of {B} relative to {A} is represented by the

homogeneous transform T = :math:`{}^A {\bf T}_B`.

.. runblock:: pycon

>>> from spatialmath.base import tr2adjoint2, trot2

>>> T = trot2(0.3, t=[1,2])

>>> tr2adjoint2(T)

:Reference:

- Robotics, Vision & Control for Python, Section 3.1, P. Corke, Springer 2023.

- `Lie groups for 2D and 3D Transformations <http://ethaneade.com/lie.pdf>_

:SymPy: supported

"""

# http://ethaneade.com/lie.pdf

if T.shape == (2, 2):

# SO(2) adjoint

return np.identity(1)

elif T.shape == (3, 3):

# SE(2) adjoint

(R, t) = smb.tr2rt(cast(SE3Array, T))

# fmt: off

return np.block([

[R, np.c_[t[1], -t[0]].T],

[0, 0, 1]

]) # type: ignore

# fmt: on

else:

raise ValueError("bad argument")

def tr2jac2(T: SE2Array) -> R3x3:

r"""

SE(2) Jacobian matrix

:param T: SE(2) matrix

:type T: ndarray(3,3)

:return: Jacobian matrix

:rtype: ndarray(3,3)

Computes an Jacobian matrix that maps spatial velocity between two frames defined by

an SE(2) matrix.

``tr2jac2(T)`` is a Jacobian matrix (3x3) that maps spatial velocity or

differential motion from frame {B} to frame {A} where the pose of {B}

elative to {A} is represented by the homogeneous transform T = :math:`{}^A {\bf T}_B`.

.. runblock:: pycon

>>> from spatialmath.base import *

>>> T = trot2(0.3, t=[4,5])

>>> tr2jac2(T)

:Reference: Robotics, Vision & Control for Python, Section 3.1, P. Corke, Springer 2023.

:SymPy: supported

"""

if not ishom2(T):

raise ValueError("expecting an SE(2) matrix")

J = np.eye(3, dtype=T.dtype)

J[:2, :2] = smb.t2r(T)

return J

@overload

def trinterp2(

start: Optional[SO2Array], end: SO2Array, s: float, shortest: bool = True

) -> SO2Array:

...

@overload

def trinterp2(

start: Optional[SE2Array], end: SE2Array, s: float, shortest: bool = True

) -> SE2Array:

...

def trinterp2(start, end, s, shortest: bool = True):

"""

Interpolate SE(2) or SO(2) matrices

:param start: initial SE(2) or SO(2) matrix value when s=0, if None then identity is used

:type start: ndarray(3,3) or ndarray(2,2) or None

:param end: final SE(2) or SO(2) matrix, value when s=1

:type end: ndarray(3,3) or ndarray(2,2)

:param s: interpolation coefficient, range 0 to 1

:type s: float

:param shortest: take the shortest path along the great circle for the rotation

:type shortest: bool, default to True

:return: interpolated SE(2) or SO(2) matrix value

:rtype: ndarray(3,3) or ndarray(2,2)

:raises ValueError: bad arguments

- ``trinterp2(None, T, S)`` is an SE(2) matrix interpolated

between identity when `S`=0 and `T` when `S`=1.

- ``trinterp2(T0, T1, S)`` as above but interpolated

between `T0` when `S`=0 and `T1` when `S`=1.

- ``trinterp2(None, R, S)`` is an SO(2) matrix interpolated

between identity when `S`=0 and `R` when `S`=1.

- ``trinterp2(R0, R1, S)`` as above but interpolated

between `R0` when `S`=0 and `R1` when `S`=1.

.. note:: Rotation angle is linearly interpolated.

.. runblock:: pycon

>>> from spatialmath.base import *

>>> T1 = transl2(1, 2)

>>> T2 = transl2(3, 4)

>>> trinterp2(T1, T2, 0)

>>> trinterp2(T1, T2, 1)

>>> trinterp2(T1, T2, 0.5)

>>> trinterp2(None, T2, 0)

>>> trinterp2(None, T2, 1)

>>> trinterp2(None, T2, 0.5)

:seealso: :func:`~spatialmath.base.transforms3d.trinterp`

"""

if smb.ismatrix(end, (2, 2)):

# SO(2) case

if start is None:

# TRINTERP2(T, s)

th0 = math.atan2(end[1, 0], end[0, 0])

th = s * th0

else:

# TRINTERP2(T1, start= s)

if start.shape != end.shape:

raise ValueError("start and end matrices must be same shape")

th0 = math.atan2(start[1, 0], start[0, 0])

th1 = math.atan2(end[1, 0], end[0, 0])

if shortest:

th1 = th0 + smb.wrap_mpi_pi(th1 - th0)

th = th0 * (1 - s) + s * th1

return rot2(th)

elif smb.ismatrix(end, (3, 3)):

if start is None:

# TRINTERP2(T, s)

th0 = math.atan2(end[1, 0], end[0, 0])

p0 = transl2(end)

th = s * th0

pr = s * p0

else:

# TRINTERP2(T0, T1, s)

if start.shape != end.shape:

raise ValueError("both matrices must be same shape")

th0 = math.atan2(start[1, 0], start[0, 0])

th1 = math.atan2(end[1, 0], end[0, 0])

if shortest:

th1 = th0 + smb.wrap_mpi_pi(th1 - th0)

p0 = transl2(start)

p1 = transl2(end)

pr = p0 * (1 - s) + s * p1

th = th0 * (1 - s) + s * th1

return smb.rt2tr(rot2(th), pr)

else:

raise ValueError("Argument must be SO(2) or SE(2)")

def trprint2(

T: Union[SO2Array, SE2Array],

label: str = "",

file: TextIO = sys.stdout,

fmt: str = "{:.3g}",

unit: str = "deg",

) -> str:

"""

Compact display of SE(2) or SO(2) matrices

:param T: matrix to format

:type T: ndarray(3,3) or ndarray(2,2)

:param label: text label to put at start of line

:type label: str

:param file: file to write formatted string to

:type file: file object

:param fmt: conversion format for each number

:type fmt: str

:param unit: angular units: 'rad' [default], or 'deg'

:type unit: str

:return: formatted string

:rtype: str

The matrix is formatted and written to ``file`` and the

string is returned. To suppress writing to a file, set ``file=None``.

- ``trprint2(R)`` displays the SO(2) rotation matrix in a compact

single-line format and returns the string::

[LABEL:] θ UNIT

- ``trprint2(T)`` displays the SE(2) homogoneous transform in a compact

single-line format and returns the string::

[LABEL:] [t=X, Y;] θ UNIT

.. runblock:: pycon

>>> from spatialmath.base import *