1717import math
1818from math import sin , cos
1919import numpy as np
20+ import scipy as sp
2021from spatialmath import base
2122from collections .abc import Iterable
2223
@@ -1526,16 +1527,12 @@ def tr2jac(T):
15261527 R = base .t2r (T )
15271528 return np .block ([[R , Z ], [Z , R ]])
15281529
1529- def eul2jac (* angles ):
1530+ def eul2jac (angles ):
15301531 """
15311532 Euler angle rate Jacobian
15321533
1533- :param phi: Z-axis rotation
1534- :type phi: float
1535- :param theta: Y-axis rotation
1536- :type theta: float
1537- :param psi: Z-axis rotation
1538- :type psi: float
1534+ :param angles: Euler angles (φ, θ, ψ)
1535+ :type angles: array_like(3)
15391536 :return: Jacobian matrix
15401537 :rtype: ndarray(3,3)
15411538
@@ -1561,7 +1558,7 @@ def eul2jac(*angles):
15611558
15621559 :SymPy: supported
15631560
1564- :seealso: :func:`rpy2jac`, :func:`eul2r `
1561+ :seealso: :func:`rpy2jac`, :func:`exp2jac`, :func:`rot2jac `
15651562 """
15661563
15671564 if len (angles ) == 1 :
@@ -1582,25 +1579,19 @@ def eul2jac(*angles):
15821579 ])
15831580
15841581
1585- def rpy2jac (* angles , order = 'zyx' ):
1582+ def rpy2jac (angles , order = 'zyx' ):
15861583 """
15871584 Jacobian from RPY angle rates to angular velocity
15881585
1589- :param order: [description], defaults to 'zyx'
1586+ :param angles: roll-pitch-yaw angles (⍺, β, γ)
1587+ :param order: angle sequence, defaults to 'zyx'
15901588 :type order: str, optional
1591-
1592- :param roll: roll angle
1593- :type roll: float
1594- :param pitch: pitch angle
1595- :type pitch: float
1596- :param yaw: yaw angle
1597- :type yaw: float
15981589 :param order: rotation order: 'zyx' [default], 'xyz', or 'yxz'
15991590 :type order: str
16001591 :return: Jacobian matrix
16011592 :rtype: ndarray(3,3)
16021593
1603- - ``rpy2r (⍺, β, γ)`` is a Jacobian matrix (3x3) that maps roll-pitch-yaw angle
1594+ - ``rpy2jac (⍺, β, γ)`` is a Jacobian matrix (3x3) that maps roll-pitch-yaw angle
16041595 rates to angular velocity at the operating point (⍺, β, γ).
16051596 These correspond to successive rotations about the axes specified by
16061597 ``order``:
@@ -1615,7 +1606,7 @@ def rpy2jac(*angles, order='zyx'):
16151606 then by ⍺ about the new z-axis. Convention for a camera with z-axis
16161607 parallel to the optic axis and x-axis parallel to the pixel rows.
16171608
1618- - ``rpy2r (𝚪)`` as above but the roll, pitch, yaw angles are taken
1609+ - ``rpy2jac (𝚪)`` as above but the roll, pitch, yaw angles are taken
16191610 from ``𝚪`` which is a 3-vector with values (⍺, β, γ).
16201611
16211612 .. runblock:: pycon
@@ -1632,12 +1623,9 @@ def rpy2jac(*angles, order='zyx'):
16321623
16331624 :SymPy: supported
16341625
1635- :seealso: :func:`eul2jac`, :func:`rpy2r `
1626+ :seealso: :func:`eul2jac`, :func:`exp2jac`, :func:`rot2jac `
16361627 """
16371628
1638- if len (angles ) == 1 :
1639- angles = angles [0 ]
1640-
16411629 pitch = angles [1 ]
16421630 yaw = angles [2 ]
16431631
@@ -1667,7 +1655,112 @@ def rpy2jac(*angles, order='zyx'):
16671655 [ cp * cy , - sy , 0 ]
16681656 ])
16691657 return J
1658+
1659+ def exp2jac (v , theta = None ):
1660+ """
1661+ Jacobian from exponential coordinate rates to angular velocity
1662+
1663+ :param v: Exponential coordinates
1664+ :type v: array_like(3)
1665+ :return: Jacobian matrix
1666+ :rtype: ndarray(3,3)
1667+
1668+ - ``exp2jac(v)`` is a Jacobian matrix (3x3) that maps exponential coordinate
1669+ rates to angular velocity at the operating point ``v``.
1670+
1671+ .. runblock:: pycon
1672+
1673+ >>> from spatialmath.base import *
1674+ >>> expjac(0.3 * np.r_[1, 0, 0])
1675+
1676+ .. note::
1677+ - Used in the creation of an analytical Jacobian.
1678+
1679+ Reference::
1680+
1681+ - A compact formula for the derivative of a 3-D rotation in
1682+ exponential coordinate
1683+ Guillermo Gallego, Anthony Yezzi
1684+ https://arxiv.org/pdf/1312.0788v1.pdf
1685+
1686+ :SymPy: supported
1687+
1688+ :seealso: :func:`eul2jac`, :func:`rpy2jac`, :func:`rot2jac`
1689+ """
1690+ R = trexp (v )
1691+ if theta is None :
1692+ v , theta = base .unitvec_norm (v )
1693+
1694+ z = np .eye (3 ,3 ) - R
1695+ A = []
1696+ for i in range (3 ):
1697+ dRdvi = v [i ] * base .skew (v ) + base .skew (np .cross (v , z [:,i ])) / theta
1698+ x = base .vex (dRdvi )
1699+ A .append (x )
1700+ return np .c_ [A ].T
1701+
1702+ def rot2jac (R , representation = 'rpy-xyz' ):
1703+ """
1704+ Velocity transform for analytical Jacobian
1705+
1706+ :param R: SO(3) rotation matrix
1707+ :type R: ndarray(3,3)
1708+ :param representation: defaults to 'rpy-xyz'
1709+ :type representation: str, optional
1710+ :return: Jacobian matrix
1711+ :rtype: ndarray(6,6)
1712+
1713+ Computes the transformation from spatial velocity, where rotation rate is
1714+ expressed as angular velocity, to analytical rates where the rotational part
1715+ is expressed as rate of change in some other representation
1716+
1717+ .. math::
1718+
1719+ \dvec{x} = \mat{A} \v ec{\mu}
1720+
1721+ :math:`\mat{A}` is a block diagonal matrix
1722+
1723+ ================== ========================================
1724+ ``representation`` Rotational representation
1725+ ================== ========================================
1726+ ``'rpy-xyz'`` RPY angular rates in XYZ order (default)
1727+ ``'rpy-zyx'`` RPY angular rates in XYZ order
1728+ ``'eul'`` Euler angular rates in ZYZ order
1729+ ``'exp'`` exponential coordinate rates
1730+ ================= ========================================
1731+
1732+ .. note:: Compared to :func:`eul2jac`, :func:`rpy2jac`, :func:`exp2jac`
1733+ - This performs the inverse mapping
1734+ - This maps a 6-vector, the others map a 3-vector
1735+
1736+ :seealso: :func:`eul2jac`, :func:`rpy2r`, :func:`exp2jac`
1737+ """
1738+
1739+ if ishom (R ):
1740+ R = base .t2r (R )
16701741
1742+ R = R .T
1743+
1744+ if representation == 'rpy-xyz' :
1745+ rpy = tr2rpy (R , 'xyz' )
1746+ A = rpy2jac (rpy , 'xyz' )
1747+ elif representation == 'rpy-zyx' :
1748+ rpy = tr2rpy (R , 'zyx' )
1749+ A = rpy2jac (rpy , 'zyx' )
1750+ elif representation == 'eul' :
1751+ eul = tr2eul (R )
1752+ A = eul2jac (eul )
1753+ elif representation == 'exp' :
1754+ (theta , v ) = trlog (R )
1755+ A = exp2jac (v , theta )
1756+ else :
1757+ raise ValueError ('bad representation specified' )
1758+
1759+ return sp .linalg .block_diag (np .eye (3 ,3 ), np .linalg .inv (A ))
1760+
1761+
1762+
1763+
16711764def tr2adjoint (T ):
16721765 r"""
16731766 SE(3) adjoint matrix
0 commit comments