Skip to content

Navigation Menu

Sign in
Appearance settings

Search code, repositories, users, issues, pull requests...

Provide feedback

We read every piece of feedback, and take your input very seriously.

Saved searches

Use saved searches to filter your results more quickly

Appearance settings

Commit 607b587

Browse filesBrowse files
committed
allow Eul and RPY methods to accept 3 angle args or one list arg
1 parent d2f63fc commit 607b587
Copy full SHA for 607b587

File tree

Expand file treeCollapse file tree

1 file changed

+39
-9
lines changed
Filter options
Expand file treeCollapse file tree

1 file changed

+39
-9
lines changed

‎spatialmath/pose3d.py

Copy file name to clipboardExpand all lines: spatialmath/pose3d.py
+39-9Lines changed: 39 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -408,7 +408,7 @@ def Rand(cls, N=1):
408408
return cls([base.q2r(base.rand()) for _ in range(0, N)], check=False)
409409

410410
@classmethod
411-
def Eul(cls, angles, unit='rad'):
411+
def Eul(cls, *angles, unit='rad'):
412412
r"""
413413
Construct a new SO(3) from Euler angles
414414
@@ -1029,7 +1029,7 @@ def Rand(cls, *, xrange=(-1, 1), yrange=(-1, 1), zrange=(-1, 1), N=1): # pylint
10291029
return cls([base.transl(x, y, z) @ base.r2t(r.A) for (x, y, z, r) in zip(X, Y, Z, R)], check=False)
10301030

10311031
@classmethod
1032-
def Eul(cls, angles, *, unit='rad'):
1032+
def Eul(cls, *angles, unit='rad'):
10331033
r"""
10341034
Create an SE(3) pure rotation from Euler angles
10351035
@@ -1040,24 +1040,38 @@ def Eul(cls, angles, *, unit='rad'):
10401040
:return: SE(3) matrix
10411041
:rtype: SE3 instance
10421042
1043-
``SE3.Eul(𝚪)`` is an SE(3) rotation defined by a 3-vector of Euler
1044-
angles :math:`\Gamma=(\phi, \theta, \psi)` which correspond to consecutive
1045-
rotations about the Z, Y, Z axes respectively.
1043+
- ``SE3.Eul(𝚪)`` is an SE(3) rotation defined by a 3-vector of Euler
1044+
angles :math:`\Gamma=(\phi, \theta, \psi)` which correspond to
1045+
consecutive rotations about the Z, Y, Z axes respectively.
10461046
10471047
If ``𝚪`` is an Nx3 matrix then the result is a sequence of
10481048
rotations each defined by Euler angles corresponding to the rows of
10491049
``𝚪``.
10501050
1051+
- ``SE3.Eul(φ, θ, ψ)`` as above but the angles are provided as three
1052+
scalars.
1053+
1054+
Example:
1055+
1056+
.. runblock:: pycon
1057+
1058+
>>> from spatialmath import SE3
1059+
>>> SE3.Eul(0.1, 0.2, 0.3)
1060+
>>> SE3.Eul([0.1, 0.2, 0.3])
1061+
>>> SE3.Eul(10, 20, 30, unit='deg')
1062+
10511063
:seealso: :func:`~spatialmath.pose3d.SE3.eul`, :func:`~spatialmath.base.transforms3d.eul2r`
10521064
:SymPy: supported
10531065
"""
1066+
if len(angles) == 1:
1067+
angles = angles[0]
10541068
if base.isvector(angles, 3):
10551069
return cls(base.eul2tr(angles, unit=unit), check=False)
10561070
else:
10571071
return cls([base.eul2tr(a, unit=unit) for a in angles], check=False)
10581072

10591073
@classmethod
1060-
def RPY(cls, angles, *, order='zyx', unit='rad'):
1074+
def RPY(cls, *angles, unit='rad', order='zyx'):
10611075
r"""
10621076
Create an SE(3) pure rotation from roll-pitch-yaw angles
10631077
@@ -1070,9 +1084,9 @@ def RPY(cls, angles, *, order='zyx', unit='rad'):
10701084
:return: SE(3) matrix
10711085
:rtype: SE3 instance
10721086
1073-
``SE3.RPY(𝚪)`` is an SE(3) rotation defined by a 3-vector of roll,
1074-
pitch, yaw angles :math:`\Gamma=(r, p, y)` which correspond to
1075-
successive rotations about the axes specified by ``order``:
1087+
- ``SE3.RPY(𝚪)`` is an SE(3) rotation defined by a 3-vector of roll,
1088+
pitch, yaw angles :math:`\Gamma=(r, p, y)` which correspond to
1089+
successive rotations about the axes specified by ``order``:
10761090
10771091
- ``'zyx'`` [default], rotate by yaw about the z-axis, then by pitch about the new y-axis,
10781092
then by roll about the new x-axis. This is the **convention** for a mobile robot with x-axis forward
@@ -1087,9 +1101,25 @@ def RPY(cls, angles, *, order='zyx', unit='rad'):
10871101
If ``𝚪`` is an Nx3 matrix then the result is a sequence of rotations each defined by RPY angles
10881102
corresponding to the rows of ``𝚪``.
10891103
1104+
- ``SE3.RPY(⍺, β, 𝛾)`` as above but the angles are provided as three
1105+
scalars.
1106+
1107+
Example:
1108+
1109+
.. runblock:: pycon
1110+
1111+
>>> from spatialmath import SE3
1112+
>>> SE3.RPY(0.1, 0.2, 0.3)
1113+
>>> SE3.RPY([0.1, 0.2, 0.3])
1114+
>>> SE3.RPY(0.1, 0.2, 0.3, order='xyz')
1115+
>>> SE3.RPY(10, 20, 30, unit='deg')
1116+
10901117
:seealso: :func:`~spatialmath.pose3d.SE3.rpy`, :func:`~spatialmath.base.transforms3d.rpy2r`
10911118
:SymPy: supported
10921119
"""
1120+
if len(angles) == 1:
1121+
angles = angles[0]
1122+
10931123
if base.isvector(angles, 3):
10941124
return cls(base.rpy2tr(angles, order=order, unit=unit), check=False)
10951125
else:

0 commit comments

Comments
0 (0)
Morty Proxy This is a proxified and sanitized view of the page, visit original site.