Skip to content

Commit 95578c8

Browse files
committed
polishing angular rates to angular velocity functions
1 parent 651f7c5 commit 95578c8

2 files changed

Lines changed: 122 additions & 23 deletions

File tree

‎spatialmath/base/__init__.py‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@
1111
from spatialmath.base.symbolic import * # lgtm [py/polluting-import]
1212
from spatialmath.base.animate import * # lgtm [py/polluting-import]
1313
from spatialmath.base.graphics import * # lgtm [py/polluting-import]
14+
from spatialmath.base.matrix import * # lgtm [py/polluting-import]
1415

1516
__all__ = [
1617
# spatialmath.base.argcheck
@@ -93,6 +94,8 @@
9394
'tr2jac',
9495
'rpy2jac',
9596
'eul2jac',
97+
'exp2jac',
98+
'rot2jac',
9699
'trprint',
97100
'trplot',
98101
'tranimate',
@@ -117,6 +120,7 @@
117120
# spatialmath.base.vectors
118121
'colvec',
119122
'unitvec',
123+
'unitvec_norm',
120124
'norm',
121125
'normsq',
122126
'isunitvec',
@@ -148,5 +152,7 @@
148152
'plot_sphere',
149153
'plot_ellipsoid',
150154
'isnotebook',
155+
# spatial.base.matrix
156+
'numjac',
151157

152158
]

‎spatialmath/base/transforms3d.py‎

Lines changed: 116 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717
import math
1818
from math import sin, cos
1919
import numpy as np
20+
import scipy as sp
2021
from spatialmath import base
2122
from 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} \vec{\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+
16711764
def tr2adjoint(T):
16721765
r"""
16731766
SE(3) adjoint matrix

0 commit comments

Comments
 (0)