Skip to content

Commit 3a93de3

Browse files
Merge pull request #79 from bdaiinstitute/se3-to-se2-flattening
Add "flattening" from SE3 to SE2.
2 parents e886c3d + f57881a commit 3a93de3

File tree

2 files changed

+45
-10
lines changed

2 files changed

+45
-10
lines changed

spatialmath/pose3d.py

Lines changed: 34 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -961,15 +961,7 @@ def __init__(self, x=None, y=None, z=None, *, check=True):
961961
elif isinstance(x, SO3):
962962
self.data = [smb.r2t(_x) for _x in x.data]
963963
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
973965
elif smb.isvector(x, 3):
974966
# SE3( [x, y, z] )
975967
self.data = [smb.transl(x)]
@@ -1170,6 +1162,39 @@ def inv(self) -> SE3:
11701162
else:
11711163
return SE3([smb.trinv(x) for x in self.A], check=False)
11721164

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+
11731198
def delta(self, X2: Optional[SE3] = None) -> R6:
11741199
r"""
11751200
Infinitesimal difference of SE(3) values

tests/test_pose3d.py

Lines changed: 11 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -659,7 +659,17 @@ def test_arith_vect(self):
659659
def test_functions(self):
660660
# inv
661661
# .T
662-
pass
662+
663+
# conversion to SE2
664+
poseSE3 = SE3.Tx(3.3) * SE3.Rz(1.5)
665+
poseSE2 = poseSE3.yaw_SE2()
666+
nt.assert_almost_equal(poseSE3.R[0:2,0:2], poseSE2.R[0:2,0:2])
667+
nt.assert_equal(poseSE3.x , poseSE2.x)
668+
nt.assert_equal(poseSE3.y , poseSE2.y)
669+
670+
posesSE3 = SE3([poseSE3, poseSE3])
671+
posesSE2 = posesSE3.yaw_SE2()
672+
nt.assert_equal(len(posesSE2), 2)
663673

664674
def test_functions_vect(self):
665675
# inv

0 commit comments

Comments
 (0)