@@ -1162,21 +1162,33 @@ def inv(self) -> SE3:
11621162 else :
11631163 return SE3 ([smb .trinv (x ) for x in self .A ], check = False )
11641164
1165- def SE2 (self ) -> SE2 :
1165+ def yaw_SE2 (self , order : str = "zyx" ) -> SE2 :
11661166 """
1167- Create SE(2) from SE(3)
1167+ Create SE(2) from SE(3) yaw angle.
11681168
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.
11711173 :rtype: SE2 instance
11721174
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.
11741186
11751187 """
11761188 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 ])
11781190 else :
1179- return SE2 ([e .SE2 () for e in self ])
1191+ return SE2 ([e .yaw_SE2 () for e in self ])
11801192
11811193 def delta (self , X2 : Optional [SE3 ] = None ) -> R6 :
11821194 r"""
0 commit comments