def SE3(self, z=0):
"""
Create SE(3) from SE(2)
:param z: default z coordinate, defaults to 0
:type z: float
:return: SE(2) with same rotation but zero translation
:rtype: SE2 instance
"Lifts" 2D rigid-body motion to 3D, rotation in the xy-plane (about the z-axis) and
z-coordinate is settable.
"""
def lift3(x):
y = np.eye(4)
y[:2, :2] = x.A[:2, :2]
y[:2, 3] = x.A[:2, 2]
y[2, 3] = z
return y
return p3.SE3([lift3(x) for x in self])
def SE3(self):
return p3d.SE3(tr.r2t(self.R), check=False)
@property
def eul(self, unit='rad', order='zyx'):
return tr.tr2eul(self.R, unit=unit)
@property
def angvec(self, unit='rad'):
return tr.tr2angvec(self.R)
@property
def SO3(self):
return p3d.SO3(self.R, check=False)
@property
def SE3(self):
return p3d.SE3(tr.r2t(self.R), check=False)
if __name__ == '__main__': # pragma: no cover
x = p3d.SE3()
x.append(x)
q = UnitQuaternion(x)
print(q)
import pathlib
import os.path
exec(
open(
os.path.join(
pathlib.Path(__file__).parent.absolute(),
"test_quaternion.py")).read())