@@ -408,7 +408,7 @@ def Rand(cls, N=1):
408
408
return cls ([base .q2r (base .rand ()) for _ in range (0 , N )], check = False )
409
409
410
410
@classmethod
411
- def Eul (cls , angles , unit = 'rad' ):
411
+ def Eul (cls , * angles , unit = 'rad' ):
412
412
r"""
413
413
Construct a new SO(3) from Euler angles
414
414
@@ -1029,7 +1029,7 @@ def Rand(cls, *, xrange=(-1, 1), yrange=(-1, 1), zrange=(-1, 1), N=1): # pylint
1029
1029
return cls ([base .transl (x , y , z ) @ base .r2t (r .A ) for (x , y , z , r ) in zip (X , Y , Z , R )], check = False )
1030
1030
1031
1031
@classmethod
1032
- def Eul (cls , angles , * , unit = 'rad' ):
1032
+ def Eul (cls , * angles , unit = 'rad' ):
1033
1033
r"""
1034
1034
Create an SE(3) pure rotation from Euler angles
1035
1035
@@ -1040,24 +1040,38 @@ def Eul(cls, angles, *, unit='rad'):
1040
1040
:return: SE(3) matrix
1041
1041
:rtype: SE3 instance
1042
1042
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.
1046
1046
1047
1047
If ``𝚪`` is an Nx3 matrix then the result is a sequence of
1048
1048
rotations each defined by Euler angles corresponding to the rows of
1049
1049
``𝚪``.
1050
1050
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
+
1051
1063
:seealso: :func:`~spatialmath.pose3d.SE3.eul`, :func:`~spatialmath.base.transforms3d.eul2r`
1052
1064
:SymPy: supported
1053
1065
"""
1066
+ if len (angles ) == 1 :
1067
+ angles = angles [0 ]
1054
1068
if base .isvector (angles , 3 ):
1055
1069
return cls (base .eul2tr (angles , unit = unit ), check = False )
1056
1070
else :
1057
1071
return cls ([base .eul2tr (a , unit = unit ) for a in angles ], check = False )
1058
1072
1059
1073
@classmethod
1060
- def RPY (cls , angles , * , order = 'zyx ' , unit = 'rad ' ):
1074
+ def RPY (cls , * angles , unit = 'rad ' , order = 'zyx ' ):
1061
1075
r"""
1062
1076
Create an SE(3) pure rotation from roll-pitch-yaw angles
1063
1077
@@ -1070,9 +1084,9 @@ def RPY(cls, angles, *, order='zyx', unit='rad'):
1070
1084
:return: SE(3) matrix
1071
1085
:rtype: SE3 instance
1072
1086
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``:
1076
1090
1077
1091
- ``'zyx'`` [default], rotate by yaw about the z-axis, then by pitch about the new y-axis,
1078
1092
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'):
1087
1101
If ``𝚪`` is an Nx3 matrix then the result is a sequence of rotations each defined by RPY angles
1088
1102
corresponding to the rows of ``𝚪``.
1089
1103
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
+
1090
1117
:seealso: :func:`~spatialmath.pose3d.SE3.rpy`, :func:`~spatialmath.base.transforms3d.rpy2r`
1091
1118
:SymPy: supported
1092
1119
"""
1120
+ if len (angles ) == 1 :
1121
+ angles = angles [0 ]
1122
+
1093
1123
if base .isvector (angles , 3 ):
1094
1124
return cls (base .rpy2tr (angles , order = order , unit = unit ), check = False )
1095
1125
else :
0 commit comments