Skip to content

Commit c445ca7

Browse files
committed
allow 3 scalar angles to be passed, alternative to array_like(3)
1 parent 3e5738c commit c445ca7

1 file changed

Lines changed: 52 additions & 15 deletions

File tree

‎spatialmath/pose3d.py‎

Lines changed: 52 additions & 15 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
@@ -419,35 +419,54 @@ def Eul(cls, angles, *, unit='rad'):
419419
:return: SO(3) rotation
420420
:rtype: SO3 instance
421421
422-
``SO3.Eul(𝚪)`` is an SO(3) rotation defined by a 3-vector of Euler angles :math:`\Gamma = (\phi, \theta, \psi)` which
423-
correspond to consecutive rotations about the Z, Y, Z axes respectively.
422+
``SO3.Eul(𝚪)`` is an SO(3) rotation defined by a 3-vector of Euler
423+
angles :math:`\Gamma = (\phi, \theta, \psi)` which correspond to
424+
consecutive rotations about the Z, Y, Z axes respectively. If ``𝚪``
425+
is an Nx3 matrix then the result is a sequence of rotations each
426+
defined by Euler angles corresponding to the rows of ``angles``.
427+
428+
``SO3.Eul(φ, θ, ψ)`` as above but the angles are provided as three
429+
scalars.
430+
431+
Example:
424432
425-
If ``𝚪`` is an Nx3 matrix then the result is a sequence of rotations each defined by Euler angles
426-
corresponding to the rows of ``angles``.
433+
.. runblock:: pycon
434+
435+
>>> from spatialmath import SO3
436+
>>> SO3.Eul(0.1, 0.2, 0.3)
437+
>>> SO3.Eul([0.1, 0.2, 0.3])
438+
>>> SO3.Eul(10, 20, 30, 'deg')
427439
428440
:seealso: :func:`~spatialmath.pose3d.SE3.eul`, :func:`~spatialmath.pose3d.SE3.Eul`, :func:`~spatialmath.base.transforms3d.eul2r`
429441
"""
442+
if len(angles) == 1:
443+
angles = angles[0]
444+
430445
if base.isvector(angles, 3):
431446
return cls(base.eul2r(angles, unit=unit), check=False)
432447
else:
433448
return cls([base.eul2r(a, unit=unit) for a in angles], check=False)
434449

435450
@classmethod
436-
def RPY(cls, angles, *, order='zyx', unit='rad'):
451+
def RPY(cls, *angles, unit='rad', order='zyx', ):
437452
r"""
438453
Construct a new SO(3) from roll-pitch-yaw angles
439454
440455
:param angles: roll-pitch-yaw angles
441-
:type angles: array_like or numpy.ndarray with shape=(N,3)
456+
:type angles: array_like(3), array_like(n,3)
442457
:param unit: angular units: 'rad' [default], or 'deg'
443458
:type unit: str
444459
:param order: rotation order: 'zyx' [default], 'xyz', or 'yxz'
445460
:type order: str
446461
:return: SO(3) rotation
447462
:rtype: SO3 instance
448463
449-
``SO3.RPY(angles)`` is an SO(3) rotation defined by a 3-vector of roll, pitch, yaw angles :math:`(r, p, y)`
450-
which correspond to successive rotations about the axes specified by ``order``:
464+
- ``SO3.RPY(angles)`` is an SO(3) rotation defined by a 3-vector of
465+
roll, pitch, yaw angles :math:`(\alpha, \beta, \gamma)`. If ``angles``
466+
is an Nx3 matrix then the result is a sequence of rotations each
467+
defined by RPY angles corresponding to the rows of angles. The angles
468+
correspond to successive rotations about the axes specified by
469+
``order``:
451470
452471
- ``'zyx'`` [default], rotate by yaw about the z-axis, then by pitch about the new y-axis,
453472
then by roll about the new x-axis. Convention for a mobile robot with x-axis forward
@@ -459,15 +478,28 @@ def RPY(cls, angles, *, order='zyx', unit='rad'):
459478
then by roll about the new z-axis. Convention for a camera with z-axis parallel
460479
to the optic axis and x-axis parallel to the pixel rows.
461480
462-
If ``angles`` is an Nx3 matrix then the result is a sequence of rotations each defined by RPY angles
463-
corresponding to the rows of angles.
481+
- ``SO3.RPY(⍺, β, 𝛾)`` as above but the angles are provided as three
482+
scalars.
483+
484+
Example:
485+
486+
.. runblock:: pycon
487+
488+
>>> from spatialmath import SO3
489+
>>> SO3.RPY(0.1, 0.2, 0.3)
490+
>>> SO3.RPY([0.1, 0.2, 0.3])
491+
>>> SO3.RPY(0.1, 0.2, 0.3, order='xyz')
492+
>>> SO3.RPY(10, 20, 30, 'deg')
493+
464494
465495
:seealso: :func:`~spatialmath.pose3d.SE3.rpy`, :func:`~spatialmath.pose3d.SE3.RPY`, :func:`spatialmath.base.transforms3d.rpy2r`
466496
"""
467-
if base.isvector(angles, 3):
468-
return cls(base.rpy2r(angles, order=order, unit=unit), check=False)
469-
else:
470-
return cls([base.rpy2r(a, order=order, unit=unit) for a in angles], check=False)
497+
if len(angles) == 1:
498+
angles = angles[0]
499+
500+
angles = base.getmatrix(angles, (None, 3))
501+
return cls(base.rpy2r(angles, order=order, unit=unit), check=False)
502+
471503

472504
@classmethod
473505
def OA(cls, o, a):
@@ -1323,6 +1355,11 @@ def SO3(cls, R, check=True):
13231355

13241356
import pathlib
13251357

1358+
a = SO3.RPY([.1, .2, .3])
1359+
print(a)
1360+
a = SO3.RPY(.1, .2, .3)
1361+
print(a)
1362+
13261363
SE3._bgcolor = 'yellow'
13271364
# SE3._format = '{:< 6.2f}'
13281365
SE3._supress_small = False

0 commit comments

Comments
 (0)