8000 improve doco · krenshaw2018/spatialmath-python@03535db · GitHub
[go: up one dir, main page]

Skip to content

Commit 03535db

Browse files
committed
improve doco
1 parent 71764dc commit 03535db

File tree

1 file changed

+35
-36
lines changed

1 file changed

+35
-36
lines changed

spatialmath/base/transforms3d.py

Lines changed: 35 additions & 36 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@
1515

1616
import sys
1717
import math
18+
from math import sin, cos
1819
import numpy as np
1920
from spatialmath import base
2021

@@ -379,29 +380,29 @@ def rpy2r(roll, pitch=None, yaw=None, *, unit='rad', order='zyx'):
379380
:type yaw: float
380381
:param unit: angular units: 'rad' [default], or 'deg'
381382
:type unit: str
382-
:param unit: rotation order: 'zyx' [default], 'xyz', or 'yxz'
383-
:type unit: str
383+
:param order: rotation order: 'zyx' [default], 'xyz', or 'yxz'
384+
:type order: str
384385
:return: SO(3) rotation matrix
385386
:rtype: ndarray(3,3)
386387
:raises ValueError: bad argument
387388
388-
- ``rpy2r(ROLL, PITCH, YAW)`` is an SO(3) orthonormal rotation matrix
389-
(3x3) equivalent to the specified roll, pitch, yaw angles angles.
390-
These correspond to successive rotations about the axes specified by ``order``:
389+
- ``rpy2r(⍺, β, γ)`` is an SO(3) orthonormal rotation matrix (3x3)
390+
equivalent to the specified roll (⍺), pitch (β), yaw (γ) angles angles.
391+
These correspond to successive rotations about the axes specified by
392+
``order``:
391393
392-
- 'zyx' [default], rotate by yaw about the z-axis, then by pitch about the new y-axis,
393-
then by roll about the new x-axis. Convention for a mobile robot with x-axis forward
394-
and y-axis sideways.
395-
- 'xyz', rotate by yaw about the x-axis, then by pitch about the new y-axis,
396-
then by roll about the new z-axis. Covention for a robot gripper with z-axis forward
397-
and y-axis between the gripper fingers.
398-
- 'yxz', rotate by yaw about the y-axis, then by pitch about the new x-axis,
399-
then by roll about the new z-axis. Convention for a camera with z-axis parallel
400-
to the optic axis and x-axis parallel to the pixel rows.
394+
- 'zyx' [default], rotate by γ about the z-axis, then by β about the new
395+
y-axis, then by about the new x-axis. Convention for a mobile robot
396+
with x-axis forward and y-axis sideways.
397+
- 'xyz', rotate by γ about the x-axis, then by β about the new y-axis,
398+
then by about the new z-axis. Convention for a robot gripper with
399+
z-axis forward and y-axis between the gripper fingers.
400+
- 'yxz', rotate by γ about the y-axis, then by β about the new x-axis,
401+
then by about the new z-axis. Convention for a camera with z-axis
402+
parallel to the optic axis and x-axis parallel to the pixel rows.
401403
402404
- ``rpy2r(RPY)`` as above but the roll, pitch, yaw angles are taken
403-
from ``RPY`` which is a 3-vector with values
404-
(ROLL, PITCH, YAW).
405+
from ``RPY`` which is a 3-vector with values (⍺, β, γ).
405406
406407
.. runblock:: pycon
407408
@@ -445,28 +446,27 @@ def rpy2tr(roll, pitch=None, yaw=None, unit='rad', order='zyx'):
445446
:type yaw: float
446447
:param unit: angular units: 'rad' [default], or 'deg'
447448
:type unit: str
448-
:param unit: rotation order: 'zyx' [default], 'xyz', or 'yxz'
449-
:type unit: str
449+
:param order: rotation order: 'zyx' [default], 'xyz', or 'yxz'
450+
:type order: str
450451
:return: SE(3) transformation matrix
451452
:rtype: ndarray(4,4)
452453
453-
- ``rpy2tr(ROLL, PITCH, YAW)`` is an SO(3) orthonormal rotation matrix
454-
(3x3) equivalent to the specified roll, pitch, yaw angles angles.
455-
These correspond to successive rotations about the axes specified by ``order``:
454+
- ``rpy2tr(⍺, β, γ)`` is an SE(3) matrix (4x4) equivalent to the specified
455+
roll (⍺), pitch (β), yaw (γ) angles angles. These correspond to successive
456+
rotations about the axes specified by ``order``:
456457
457-
- 'zyx' [default], rotate by yaw about the z-axis, then by pitch about the new y-axis,
458-
then by roll about the new x-axis. Convention for a mobile robot with x-axis forward
459-
and y-axis sideways.
460-
- 'xyz', rotate by yaw about the x-axis, then by pitch about the new y-axis,
461-
then by roll about the new z-axis. Convention for a robot gripper with z-axis forward
462-
and y-axis between the gripper fingers.
463-
- 'yxz', rotate by yaw about the y-axis, then by pitch about the new x-axis,
464-
then by roll about the new z-axis. Convention for a camera with z-axis parallel
465-
to the optic axis and x-axis parallel to the pixel rows.
458+
- 'zyx' [default], rotate by γ about the z-axis, then by β about the new
459+
y-axis, then by about the new x-axis. Convention for a mobile robot
460+
with x-axis forward and y-axis sideways.
461+
- 'xyz', rotate by γ about the x-axis, then by β about the new y-axis,
462+
then by about the new z-axis. Convention for a robot gripper with
463+
z-axis forward and y-axis between the gripper fingers.
464+
- 'yxz', rotate by γ about the y-axis, then by β about the new x-axis,
465+
then by about the new z-axis. Convention for a camera with z-axis
466+
parallel to the optic axis and x-axis parallel to the pixel rows.
466467
467468
- ``rpy2tr(RPY)`` as above but the roll, pitch, yaw angles are taken
468-
from ``RPY`` which is a 3-vector with values
469-
(ROLL, PITCH, YAW).
469+
from ``RPY`` which is a 3-vector with values (⍺, β, γ).
470470
471471
.. runblock:: pycon
472472
@@ -502,12 +502,11 @@ def eul2r(phi, theta=None, psi=None, unit='rad'):
502502
:return: SO(3) rotation matrix
503503
:rtype: ndarray(3,3)
504504
505-
- ``R = eul2r(PHI, θ, PSI)`` is an SO(3) orthonornal rotation
505+
- ``R = eul2r(φ, θ, ψ)`` is an SO(3) orthonornal rotation
506506
matrix equivalent to the specified Euler angles. These correspond
507507
to rotations about the Z, Y, Z axes respectively.
508508
- ``R = eul2r(EUL)`` as above but the Euler angles are taken from
509-
``EUL`` which is a 3-vector with values
510-
(PHI θ PSI).
509+
``EUL`` which is a 3-vector with values (φ θ ψ).
511510
512511
.. runblock:: pycon
513512
@@ -892,7 +891,7 @@ def tr2rpy(T, unit='rad', order='zyx', check=False):
892891
:param unit: 'rad' or 'deg'
893892
:type unit: str
894893
:param order: 'xyz', 'zyx' or 'yxz' [default 'zyx']
895-
:type unit: str
894+
:type order: str
896895
:param check: check that rotation matrix is valid
897896
:type check: bool
898897
:return: Roll-pitch-yaw angles

0 commit comments

Comments
 (0)
0