@@ -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+
15281670def tr2adjoint (T ):
15291671 r"""
15301672 SE(3) adjoint matrix
0 commit comments