def test_angle_axis(): aa = e.AngleAxisd() # quaternion xyzw coefficient order q = e.Quaterniond(e.Vector4d(0., 0., 0., 1.)) aa = e.AngleAxisd(q) assert(aa.angle() == 0) v = e.Vector3d.UnitX() aa = e.AngleAxisd(0.1, v) assert(aa.axis() == v) assert(aa.angle() == 0.1) aa2 = aa.inverse() assert(aa2.axis() == v) assert(aa2.angle() == -0.1)
def test(self):
Em = eigen.AngleAxisd(np.pi / 2,
eigen.Vector3d.UnitX()).inverse().matrix()
Eq = eigen.Quaterniond(
eigen.AngleAxisd(np.pi / 2, eigen.Vector3d.UnitX()).inverse())
r = eigen.Vector3d.Random() * 100
pt1 = sva.PTransformd.Identity()
self.assertEqual(pt1.rotation(), eigen.Matrix3d.Identity())
self.assertEqual(pt1.translation(), eigen.Vector3d.Zero())
pt2 = sva.PTransformd(Em, r)
self.assertEqual(pt2.rotation(), Em)
self.assertEqual(pt2.translation(), r)
pt3 = sva.PTransformd(Eq, r)
self.assertEqual(pt3.rotation(), Eq.toRotationMatrix())
self.assertEqual(pt3.translation(), r)
pt4 = sva.PTransformd(Eq)
self.assertEqual(pt4.rotation(), Eq.toRotationMatrix())
self.assertEqual(pt4.translation(), eigen.Vector3d.Zero())
pt5 = sva.PTransformd(Em)
self.assertEqual(pt5.rotation(), Em)
self.assertEqual(pt5.translation(), eigen.Vector3d.Zero())
pt6 = sva.PTransformd(r)
self.assertEqual(pt6.rotation(), eigen.Matrix3d.Identity())
self.assertEqual(pt6.translation(), r)
pttmp = sva.PTransformd(
eigen.AngleAxisd(np.pi / 4, eigen.Vector3d.UnitY()).matrix(),
eigen.Vector3d.Random() * 100.)
pt7 = pt2 * pttmp
ptm = pt2.matrix() * pttmp.matrix()
self.assertAlmostEqual((pt7.matrix() - ptm).norm(), 0, delta=TOL)
pt8 = pt2.inv()
self.assertAlmostEqual((pt8.matrix() - pt2.matrix().inverse()).norm(),
0,
delta=TOL)
self.assertEqual(pt2, pt2)
self.assertNotEqual(pt2, pt8)
self.assertTrue(pt2 != pt8)
self.assertTrue(not (pt2 != pt2))
def test(self):
from_ = sva.PTransformd(eigen.Matrix3d.Identity(),
eigen.Vector3d.Zero())
to = sva.PTransformd(eigen.AngleAxisd(np.pi, eigen.Vector3d.UnitZ()),
eigen.Vector3d(1, 2, -3))
res = sva.interpolate(from_, to, 0.5)
self.assertAlmostEqual((res.rotation() - eigen.AngleAxisd(
np.pi / 2, eigen.Vector3d.UnitZ()).matrix()).norm(),
0,
delta=TOL)
self.assertAlmostEqual(
(res.translation() - eigen.Vector3d(0.5, 1., -1.5)).norm(),
0,
delta=TOL)
def test(self):
theta2d = eigen.Vector2d.Random() * 10
theta = theta2d[0]
self.assertAlmostEqual(
(sva.RotX(theta) -
eigen.AngleAxisd(-theta, eigen.Vector3d.UnitX()).matrix()).norm(),
0,
delta=TOL)
self.assertAlmostEqual(
(sva.RotY(theta) -
eigen.AngleAxisd(-theta, eigen.Vector3d.UnitY()).matrix()).norm(),
0,
delta=TOL)
self.assertAlmostEqual(
(sva.RotZ(theta) -
eigen.AngleAxisd(-theta, eigen.Vector3d.UnitZ()).matrix()).norm(),
0,
delta=TOL)
def grasp_dataset_rotation_to_xyz_theta(rotation, verbose=0):
"""Convert a rotation to an angle axis theta specifically for brainrobotdata
From above, a rotation to the right should be a positive theta,
and a rotation to the left negative theta. The initial pose is with the
z axis pointing down, the y axis to the right and the x axis forward.
This format does not allow for arbitrary rotation commands to be defined,
and originates from the paper and dataset:
https://sites.google.com/site/brainrobotdata/home/grasping-dataset
https://arxiv.org/abs/1603.02199
In the google brain dataset the gripper is only commanded to
rotate around a single vertical axis,
so you might clearly visualize it, this also happens to
approximately match the vector defined by gravity.
Furthermore, the original paper had the geometry of the
arm joints on which params could easily be extracted,
which is not available here. To resolve this discrepancy
Here we assume that the gripper generally starts off at a
quaternion orientation of approximately [qx=-1, qy=0, qz=0, qw=0].
This is equivalent to the angle axis
representation of [a=np.pi, x=-1, y=0, z=0],
which I'll name default_rot.
It is also important to note the ambiguity of the
angular distance between any current pose
and the end pose. This angular distance will
always have a positive value so the network
could not naturally discriminate between
turning left and turning right.
For this reason, we use the angular distance
from default_rot to define the input angle parameter,
and if the angle axis x axis component is > 0
we will use theta for rotation,
but if the angle axis x axis component is < 0
we will use -theta.
"""
aa = eigen.AngleAxisd(rotation)
theta = aa.angle()
if aa.axis().z() < 0:
multiply = 1.0
else:
multiply = -1.0
if verbose > 0:
print("ANGLE_AXIS_MULTIPLY: ", aa.angle(), np.array(aa.axis()),
multiply)
theta *= multiply
return np.array([aa.x(), aa.y(), aa.z(), theta])
def test(self):
Eq = eigen.AngleAxisd(np.pi / 2, eigen.Vector3d.UnitX())
r = eigen.Vector3d.Random() * 100
pt = sva.PTransformd(Eq, r)
ptInv = pt.inv()
pt6d = pt.matrix()
ptInv6d = ptInv.matrix()
ptDual6d = pt.dualMatrix()
M = eigen.Matrix3d([[1, 2, 3], [2, 1, 4], [3, 4, 1]])
H = eigen.Matrix3d.Random() * 100
I = eigen.Matrix3d([[1, 2, 3], [2, 1, 4], [3, 4, 1]])
ab = sva.ABInertiad(M, H, I)
ab6d = ab.matrix()
mass = 1.
h = eigen.Vector3d.Random() * 100
rb = sva.RBInertiad(mass, h, I)
rb6d = rb.matrix()
w = eigen.Vector3d.Random() * 100
v = eigen.Vector3d.Random() * 100
n = eigen.Vector3d.Random() * 100
f = eigen.Vector3d.Random() * 100
mVec = sva.MotionVecd(w, v)
mVec6d = mVec.vector()
fVec = sva.ForceVecd(n, f)
fVec6d = fVec.vector()
mvRes1 = pt * mVec
mvRes16d = pt6d * mVec6d
self.assertAlmostEqual((mvRes1.vector() - mvRes16d).norm(),
0,
delta=TOL)
self.assertAlmostEqual((mvRes1.angular() - pt.angularMul(mVec)).norm(),
0,
delta=TOL)
self.assertAlmostEqual((mvRes1.linear() - pt.linearMul(mVec)).norm(),
0,
delta=TOL)
# Note: the C++ version would test the vectorized version here but this is not supported by the Python bindings
mvRes2 = pt.invMul(mVec)
mvRes26d = ptInv6d * mVec6d
self.assertAlmostEqual((mvRes2.vector() - mvRes26d).norm(),
0,
delta=TOL)
self.assertAlmostEqual(
(mvRes2.angular() - pt.angularInvMul(mVec)).norm(), 0, delta=TOL)
self.assertAlmostEqual(
(mvRes2.linear() - pt.linearInvMul(mVec)).norm(), 0, delta=TOL)
# Same note about the vectorized version
fvRes1 = pt.dualMul(fVec)
fvRes16d = ptDual6d * fVec6d
self.assertAlmostEqual((fvRes1.vector() - fvRes16d).norm(),
0,
delta=TOL)
self.assertAlmostEqual(
(fvRes1.couple() - pt.coupleDualMul(fVec)).norm(), 0, delta=TOL)
self.assertAlmostEqual((fvRes1.force() - pt.forceDualMul(fVec)).norm(),
0,
delta=TOL)
# Same note about the vectorized version
fvRes2 = pt.transMul(fVec)
fvRes26d = pt6d.transpose() * fVec6d
self.assertAlmostEqual((fvRes2.vector() - fvRes26d).norm(),
0,
delta=TOL)
self.assertAlmostEqual(
(fvRes2.couple() - pt.coupleTransMul(fVec)).norm(), 0, delta=TOL)
self.assertAlmostEqual(
(fvRes2.force() - pt.forceTransMul(fVec)).norm(), 0, delta=TOL)
# Same note about the vectorized version
rbRes1 = pt.dualMul(rb)
rbRes16d = ptDual6d * rb6d * ptInv6d
self.assertAlmostEqual((rbRes1.matrix() - rbRes16d).norm(),
0,
delta=TOL)
rbRes2 = pt.transMul(rb)
rbRes26d = pt6d.transpose() * rb6d * pt6d
self.assertAlmostEqual((rbRes2.matrix() - rbRes26d).norm(),
0,
delta=TOL)
abRes1 = pt.dualMul(ab)
abRes16d = ptDual6d * ab6d * ptInv6d
self.assertAlmostEqual((abRes1.matrix() - abRes16d).norm(),
0,
delta=TOL)
abRes2 = pt.transMul(ab)
abRes26d = pt6d.transpose() * ab6d * pt6d
self.assertAlmostEqual((abRes2.matrix() - abRes26d).norm(),
0,
delta=TOL)
def test_quaternion():
q = e.Quaterniond()
# Identity, xyzw coefficient order.
id_v = e.Vector4d(0., 0., 0., 1.)
# Identity, wxyz scalar constructor order.
q = e.Quaterniond(1., 0., 0., 0.)
q2 = e.Quaterniond(id_v)
# Both identity quaternions must be equal.
assert(q.angularDistance(q2) == 0)
q3 = q2*q
assert(q.angularDistance(q3) == 0)
v4 = e.Vector4d.Random()
while v4 == id_v:
v4 = e.Vector4d.Random()
v4.normalize()
q = e.Quaterniond(v4) # Vector4d ctor
assert(q.coeffs() == v4)
q = e.Quaterniond(v4[3], v4[0], v4[1], v4[2]) # 4 doubles ctor
assert(q.coeffs() == v4)
q_copy = e.Quaterniond(q) # Copy ctor
assert(q_copy.coeffs() == q.coeffs())
q_copy.setIdentity()
assert(q_copy.coeffs() != q.coeffs()) # Check the two objects are actually different
q = e.Quaterniond(np.pi, e.Vector3d.UnitZ()) # Angle-Axis
assert((q.coeffs() - e.Vector4d(0., 0., 1., 0.)).norm() < 1e-6)
q = e.Quaterniond(e.AngleAxisd(np.pi, e.Vector3d.UnitZ()))
assert((q.coeffs() - e.Vector4d(0., 0., 1., 0.)).norm() < 1e-6)
q = e.Quaterniond(e.Matrix3d.Identity())
assert(q.coeffs() == id_v)
q = e.Quaterniond.Identity()
assert(q.coeffs() == id_v)
# Check getters
assert(q.x() == 0)
assert(q.y() == 0)
assert(q.z() == 0)
assert(q.w() == 1)
assert(q.vec() == e.Vector3d.Zero())
assert(q.coeffs() == id_v)
# Check setters
q = e.Quaterniond(v4) # Rebuild from something other than Identity
q.setIdentity()
assert(q.coeffs() == id_v)
q.setFromTwoVectors(e.Vector3d.UnitX(), e.Vector3d.UnitY())
assert(q.isApprox(e.Quaterniond(np.pi/2, e.Vector3d.UnitZ())))
q.setIdentity()
# Operations
assert(e.Quaterniond(id_v).angularDistance(e.Quaterniond(np.pi, e.Vector3d.UnitZ())) == np.pi)
assert(e.Quaterniond(v4).conjugate().coeffs() == e.Vector4d(-v4.x(), -v4.y(), -v4.z(), v4.w()))
assert(e.Quaterniond(id_v).dot(e.Quaterniond(np.pi, e.Vector3d.UnitZ())) == np.cos(np.pi/2))
check_quaternion_almost_equals(e.Quaterniond(v4).inverse(), e.Quaterniond(v4).conjugate())
assert(q.isApprox(q))
assert(q.isApprox(q, 1e-8))
assert(q.matrix() == e.Matrix3d.Identity())
assert(q.toRotationMatrix() == e.Matrix3d.Identity())
v4_2 = e.Vector4d.Random()
v4_2 = 2 * v4_2 / v4_2.norm()
q = e.Quaterniond(v4_2)
assert(q.norm() != 1.0)
q_n = q.normalized()
assert(q.norm() != 1.0 and q_n.norm() == 1.0)
q.normalize()
assert(q.norm() == 1.0)
check_quaternion_almost_equals(e.Quaterniond.Identity().slerp(1.0, q), q)
assert(q.squaredNorm() == 1.0)
# Static method
q = e.Quaterniond.UnitRandom()
assert(q.norm() == 1.0)
