@@ -1162,21 +1162,33 @@ def inv(self) -> SE3:
1162
1162
else :
1163
1163
return SE3 ([smb .trinv (x ) for x in self .A ], check = False )
1164
1164
1165
- def SE2 (self ) -> SE2 :
1165
+ def yaw_SE2 (self , order : str = "zyx" ) -> SE2 :
1166
1166
"""
1167
- Create SE(2) from SE(3)
1167
+ Create SE(2) from SE(3) yaw angle.
1168
1168
1169
- :return: SE(2) with same rotation as the yaw angle using the 'zyx' convention,
1170
- and translation in x,y
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 in x,y.
1171
1173
:rtype: SE2 instance
1172
1174
1173
- "Flattens" 3D rigid-body motion to 2D, along the z axis.
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.
1174
1186
1175
1187
"""
1176
1188
if len (self ) == 1 :
1177
- return SE2 (self .x , self .y , self .rpy ()[2 ])
1189
+ return SE2 (self .x , self .y , self .rpy (order = order )[2 ])
1178
1190
else :
1179
- return SE2 ([e .SE2 () for e in self ])
1191
+ return SE2 ([e .yaw_SE2 () for e in self ])
1180
1192
1181
1193
def delta (self , X2 : Optional [SE3 ] = None ) -> R6 :
1182
1194
r"""
0 commit comments