Skip to content

Navigation Menu

Sign in
Appearance settings

Search code, repositories, users, issues, pull requests...

Provide feedback

We read every piece of feedback, and take your input very seriously.

Saved searches

Use saved searches to filter your results more quickly

Appearance settings

Commit e5daa93

Browse filesBrowse files
committed
Specificy yaw in funciton name, better doc string.
1 parent e9dc03f commit e5daa93
Copy full SHA for e5daa93

File tree

Expand file treeCollapse file tree

2 files changed

+21
-9
lines changed
Filter options
Expand file treeCollapse file tree

2 files changed

+21
-9
lines changed

‎spatialmath/pose3d.py

Copy file name to clipboardExpand all lines: spatialmath/pose3d.py
+19-7Lines changed: 19 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -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"""

‎tests/test_pose3d.py

Copy file name to clipboardExpand all lines: tests/test_pose3d.py
+2-2Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -662,13 +662,13 @@ def test_functions(self):
662662

663663
# conversion to SE2
664664
poseSE3 = SE3.Tx(3.3) * SE3.Rz(1.5)
665-
poseSE2 = poseSE3.SE2()
665+
poseSE2 = poseSE3.yaw_SE2()
666666
nt.assert_almost_equal(poseSE3.R[0:1,0:1], poseSE2.R[0:1,0:1])
667667
nt.assert_equal(poseSE3.x , poseSE2.x)
668668
nt.assert_equal(poseSE3.y , poseSE2.y)
669669

670670
posesSE3 = SE3([poseSE3, poseSE3])
671-
posesSE2 = posesSE3.SE2()
671+
posesSE2 = posesSE3.yaw_SE2()
672672
nt.assert_equal(len(posesSE2), 2)
673673

674674
def test_functions_vect(self):

0 commit comments

Comments
0 (0)
Morty Proxy This is a proxified and sanitized view of the page, visit original site.