@@ -961,15 +961,7 @@ def __init__(self, x=None, y=None, z=None, *, check=True):
961
961
elif isinstance (x , SO3 ):
962
962
self .data = [smb .r2t (_x ) for _x in x .data ]
963
963
elif isinstance (x , SE2 ): # type(x).__name__ == "SE2":
964
-
965
- def convert (x ):
966
- # convert SE(2) to SE(3)
967
- out = np .identity (4 , dtype = x .dtype )
968
- out [:2 , :2 ] = x [:2 , :2 ]
969
- out [:2 , 3 ] = x [:2 , 2 ]
970
- return out
971
-
972
- self .data = [convert (_x ) for _x in x .data ]
964
+ self .data = x .SE3 ().data
973
965
elif smb .isvector (x , 3 ):
974
966
# SE3( [x, y, z] )
975
967
self .data = [smb .transl (x )]
@@ -1170,6 +1162,39 @@ def inv(self) -> SE3:
1170
1162
else :
1171
1163
return SE3 ([smb .trinv (x ) for x in self .A ], check = False )
1172
1164
1165
+ def yaw_SE2 (self , order : str = "zyx" ) -> SE2 :
1166
+ """
1167
+ Create SE(2) from SE(3) yaw angle.
1168
+
1169
+ :param order: angle sequence order, default to 'zyx'
1170
+ :type order: str
1171
+ :return: SE(2) with same rotation as the yaw angle using the roll-pitch-yaw convention,
1172
+ and translation along the roll-pitch axes.
1173
+ :rtype: SE2 instance
1174
+
1175
+ Roll-pitch-yaw corresponds to successive rotations about the axes specified by ``order``:
1176
+
1177
+ - ``'zyx'`` [default], rotate by yaw about the z-axis, then by pitch about the new y-axis,
1178
+ then by roll about the new x-axis. Convention for a mobile robot with x-axis forward
1179
+ and y-axis sideways.
1180
+ - ``'xyz'``, rotate by yaw about the x-axis, then by pitch about the new y-axis,
1181
+ then by roll about the new z-axis. Convention for a robot gripper with z-axis forward
1182
+ and y-axis between the gripper fingers.
1183
+ - ``'yxz'``, rotate by yaw about the y-axis, then by pitch about the new x-axis,
1184
+ then by roll about the new z-axis. Convention for a camera with z-axis parallel
1185
+ to the optic axis and x-axis parallel to the pixel rows.
1186
+
1187
+ """
1188
+ if len (self ) == 1 :
1189
+ if order == "zyx" :
1190
+ return SE2 (self .x , self .y , self .rpy (order = order )[2 ])
1191
+ elif order == "xyz" :
1192
+ return SE2 (self .z , self .y , self .rpy (order = order )[2 ])
1193
+ elif order == "yxz" :
1194
+ return SE2 (self .z , self .x , self .rpy (order = order )[2 ])
1195
+ else :
1196
+ return SE2 ([e .yaw_SE2 () for e in self ])
1197
+
1173
1198
def delta (self , X2 : Optional [SE3 ] = None ) -> R6 :
1174
1199
r"""
1175
1200
Infinitesimal difference of SE(3) values
0 commit comments