Skip to content

Commit 71764dc

Browse files
committed
add Euler and RPY angle Jacobian
1 parent d1dc9c4 commit 71764dc

1 file changed

Lines changed: 142 additions & 0 deletions

File tree

‎spatialmath/base/transforms3d.py‎

Lines changed: 142 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1525,6 +1525,148 @@ def tr2jac(T):
15251525
R = base.t2r(T)
15261526
return np.block([[R, Z], [Z, R]])
15271527

1528+
def eul2jac(*angles):
1529+
"""
1530+
Euler angle rate Jacobian
1531+
1532+
:param phi: Z-axis rotation
1533+
:type phi: float
1534+
:param theta: Y-axis rotation
1535+
:type theta: float
1536+
:param psi: Z-axis rotation
1537+
:type psi: float
1538+
:return: Jacobian matrix
1539+
:rtype: ndarray(3,3)
1540+
1541+
- ``eul2jac(φ, θ, ψ)`` is a Jacobian matrix (3x3) that maps ZYZ Euler angle
1542+
rates to angular velocity at the operating point specified by the Euler
1543+
angles φ, ϴ, ψ.
1544+
- ``eul2jac(𝚪)`` as above but the Euler angles are taken from ``𝚪`` which
1545+
is a 3-vector with values (φ θ ψ).
1546+
1547+
Example:
1548+
1549+
.. runblock:: pycon
1550+
1551+
>>> from spatialmath.base import *
1552+
>>> eul2jac(0.1, 0.2, 0.3)
1553+
1554+
.. note::
1555+
- Used in the creation of an analytical Jacobian.
1556+
- Angles in radians, rates in radians/sec.
1557+
1558+
Reference::
1559+
- Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p232-3.
1560+
1561+
:SymPy: supported
1562+
1563+
:seealso: :func:`rpy2jac`, :func:`eul2r`
1564+
"""
1565+
1566+
if len(angles) == 1:
1567+
angles = angles[0]
1568+
1569+
phi = angles[0]
1570+
theta = angles[1]
1571+
1572+
ctheta = base.sym.cos(theta)
1573+
stheta = base.sym.sin(theta)
1574+
cphi = base.sym.cos(phi)
1575+
sphi = base.sym.sin(phi)
1576+
1577+
return np.array([
1578+
[ 0, -sphi, cphi * stheta],
1579+
[ 0, cphi, sphi * stheta],
1580+
[ 1, 0, ctheta ]
1581+
])
1582+
1583+
1584+
def rpy2jac(*angles, order='zyx'):
1585+
"""
1586+
Jacobian from RPY angle rates to angular velocity
1587+
1588+
:param order: [description], defaults to 'zyx'
1589+
:type order: str, optional
1590+
1591+
:param roll: roll angle
1592+
:type roll: float
1593+
:param pitch: pitch angle
1594+
:type pitch: float
1595+
:param yaw: yaw angle
1596+
:type yaw: float
1597+
:param order: rotation order: 'zyx' [default], 'xyz', or 'yxz'
1598+
:type order: str
1599+
:return: Jacobian matrix
1600+
:rtype: ndarray(3,3)
1601+
1602+
- ``rpy2r(⍺, β, γ)`` is a Jacobian matrix (3x3) that maps roll-pitch-yaw angle
1603+
rates to angular velocity at the operating point (⍺, β, γ).
1604+
These correspond to successive rotations about the axes specified by
1605+
``order``:
1606+
1607+
- 'zyx' [default], rotate by γ about the z-axis, then by β about the new
1608+
y-axis, then by ⍺ about the new x-axis. Convention for a mobile robot
1609+
with x-axis forward and y-axis sideways.
1610+
- 'xyz', rotate by γ about the x-axis, then by β about the new y-axis,
1611+
then by ⍺ about the new z-axis. Convention for a robot gripper with
1612+
z-axis forward and y-axis between the gripper fingers.
1613+
- 'yxz', rotate by γ about the y-axis, then by β about the new x-axis,
1614+
then by ⍺ about the new z-axis. Convention for a camera with z-axis
1615+
parallel to the optic axis and x-axis parallel to the pixel rows.
1616+
1617+
- ``rpy2r(𝚪)`` as above but the roll, pitch, yaw angles are taken
1618+
from ``𝚪`` which is a 3-vector with values (⍺, β, γ).
1619+
1620+
.. runblock:: pycon
1621+
1622+
>>> from spatialmath.base import *
1623+
>>> rpy2jac(0.1, 0.2, 0.3)
1624+
1625+
.. note::
1626+
- Used in the creation of an analytical Jacobian.
1627+
- Angles in radians, rates in radians/sec.
1628+
1629+
Reference::
1630+
- Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p232-3.
1631+
1632+
:SymPy: supported
1633+
1634+
:seealso: :func:`eul2jac`, :func:`rpy2r`
1635+
"""
1636+
1637+
if len(angles) == 1:
1638+
angles = angles[0]
1639+
1640+
pitch = angles[1]
1641+
yaw = angles[2]
1642+
1643+
cp = base.sym.cos(pitch)
1644+
sp = base.sym.sin(pitch)
1645+
cy = base.sym.cos(yaw)
1646+
sy = base.sym.sin(yaw)
1647+
1648+
if order == 'xyz':
1649+
J = np.array([
1650+
[ sp, 0, 1],
1651+
[-cp * sy, cy, 0],
1652+
[ cp * cy, sy, 0]
1653+
])
1654+
1655+
elif order == 'zyx':
1656+
J = np.array([
1657+
[ cp * cy, -sy, 0],
1658+
[ cp * sy, cy, 0],
1659+
[-sp, 0, 1],
1660+
])
1661+
1662+
elif order == 'yxz':
1663+
J = np.array([
1664+
[ cp * sy, cy, 0],
1665+
[-sp, 0, 1],
1666+
[ cp * cy, -sy, 0]
1667+
])
1668+
return J
1669+
15281670
def tr2adjoint(T):
15291671
r"""
15301672
SE(3) adjoint matrix

0 commit comments

Comments
 (0)