def test(self):
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)
fVec = sva.ForceVecd(n, f)
mm = mVec.vector()
mf = fVec.vector()
self.assertAlmostEqual(mVec.dot(fVec) - (mm.transpose() * mf)[0],
0,
delta=TOL)
w2 = eigen.Vector3d.Random() * 100
v2 = eigen.Vector3d.Random() * 100
mVec2 = sva.MotionVecd(w2, v2)
mm2 = mVec2.vector()
crossM = mVec.cross(mVec2)
self.assertTrue(isinstance(crossM, sva.MotionVecd))
self.assertAlmostEqual(
(crossM.vector() - sva.vector6ToCrossMatrix(mm) * mm2).norm(),
0,
delta=TOL)
crossF = mVec.crossDual(fVec)
self.assertTrue(isinstance(crossF, sva.ForceVecd))
self.assertAlmostEqual(
(crossF.vector() - sva.vector6ToCrossDualMatrix(mm) * mf).norm(),
0,
delta=TOL)
def test(self):
create_random_mv = lambda: sva.MotionVecd(
eigen.Vector3d().Random() * 100,
eigen.Vector3d().Random() * 100)
# Check empty creation and filling
v1 = sva.MotionVecdVector()
for i in range(100):
v1.append(create_random_mv())
assert (len(v1) == 100)
# Check creation from a single MotionVecd object
v2 = sva.MotionVecdVector(create_random_mv())
assert (len(v2) == 1)
# Check creation from a list
v3 = sva.MotionVecdVector([create_random_mv() for i in range(100)])
assert (len(v3) == 100)
# Check creation from a lot of MotionVecd
v3 = sva.MotionVecdVector(*[create_random_mv() for i in range(100)])
assert (len(v3) == 100)
# Check access
mv = create_random_mv()
v4 = sva.MotionVecdVector([mv] * 100)
assert (all([mv == vi for vi in v4]))
def test(self):
M = eigen.Matrix3d([[1, 2, 3],[2, 1, 4], [3, 4, 1]])
H = eigen.Matrix3d.Random()
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()
v = eigen.Vector3d.Random()
mVec = sva.MotionVecd(w, v)
mVec6d = mVec.vector()
abRes = ab + rb
abRes6d = ab6d + rb6d
self.assertTrue(isinstance(abRes, sva.ABInertiad))
self.assertAlmostEqual((abRes6d - abRes.matrix()).norm(), 0, delta = TOL)
self.assertTrue(isUpperNull(abRes.lowerTriangularMassMatrix()))
self.assertTrue(isUpperNull(abRes.lowerTriangularInertia()))
fVec = ab*mVec
fVec6d = ab6d*mVec6d
self.assertTrue(isinstance(fVec, sva.ForceVecd))
self.assertAlmostEqual((fVec6d - fVec.vector()).norm(), 0, delta = TOL)
def test(self):
w = eigen.Vector3d.Random()
v = eigen.Vector3d.Random()
vec = sva.ImpedanceVecd(w, v)
z = vec.vector()
self.assertEqual(w, vec.angular())
self.assertEqual(v, vec.linear())
self.assertEqual(
z, eigen.Vector6d(w.x(), w.y(), w.z(), v.x(), v.y(), v.z()))
self.assertEqual((5. * vec).vector(), 5. * z)
self.assertEqual((vec * 5.).vector(), 5. * z)
self.assertEqual((vec / 5.).vector(), z / 5.)
vec *= 5.
self.assertEqual(vec.vector(), 5. * z)
vec /= 5.
[self.assertAlmostEqual(x, 0, delta=TOL) for x in vec.vector() - z]
w2 = eigen.Vector3d.Random()
v2 = eigen.Vector3d.Random()
vec2 = sva.ImpedanceVecd(w2, v2)
z2 = vec2.vector()
self.assertAlmostEqual(((vec + vec2).vector() - (z + z2)).norm(),
0,
delta=TOL)
vec_pluseq = sva.ImpedanceVecd(vec)
self.assertEqual(vec_pluseq, vec)
vec_pluseq += vec2
self.assertEqual(vec_pluseq, vec + vec2)
self.assertEqual(vec, vec)
self.assertTrue(not (vec != vec))
w = eigen.Vector3d.Random()
v = eigen.Vector3d.Random()
mv = sva.MotionVecd(eigen.Vector3d.Random(), eigen.Vector3d.Random())
fv = vec * mv
self.assertTrue(isinstance(fv, sva.ForceVecd))
res = eigen.Vector6d(
[x * y for x, y in zip(vec.vector(), mv.vector())])
self.assertEqual(res, fv.vector())
fv2 = mv * vec
self.assertEqual(fv, fv2)
hv = sva.ImpedanceVecd(11., 42.)
self.assertEqual(hv.angular(), eigen.Vector3d(11., 11., 11.))
self.assertEqual(hv.linear(), eigen.Vector3d(42., 42., 42.))
z = sva.ImpedanceVecd(0., 0.)
self.assertEqual(sva.ImpedanceVecd.Zero(), z)
def test(self):
w = eigen.Vector3d.Random()
v = eigen.Vector3d.Random()
vec = sva.MotionVecd(w, v)
m = vec.vector()
self.assertEqual(w, vec.angular())
self.assertEqual(v, vec.linear())
self.assertEqual(m, eigen.Vector6d(list(w) + list(v)))
self.assertEqual((5. * vec).vector(), 5. * m)
self.assertEqual((vec * 5.).vector(), 5. * m)
self.assertEqual((vec / 5.).vector(), m / 5)
self.assertEqual((-vec).vector(), -m)
w2 = eigen.Vector3d.Random()
v2 = eigen.Vector3d.Random()
vec2 = sva.MotionVecd(w2, v2)
m2 = vec2.vector()
self.assertEqual((vec + vec2).vector(), (m + m2))
self.assertEqual((vec - vec2).vector(), (m - m2))
vec_pluseq = sva.MotionVecd(vec)
vec_pluseq += vec2
self.assertEqual(vec_pluseq, vec + vec2)
vec_minuseq = sva.MotionVecd(vec)
vec_minuseq -= vec2
self.assertEqual(vec_minuseq, vec - vec2)
self.assertEqual(vec, vec)
self.assertNotEqual(vec, -vec)
self.assertTrue(vec != (-vec))
self.assertTrue(not (vec != vec))
z = sva.MotionVecd([0, 0, 0], [0, 0, 0])
self.assertEqual(sva.MotionVecd.Zero(), z)
def test(self):
mass = 1.
I = eigen.Matrix3d([[1, 2, 3],[2, 1, 4], [3, 4, 1]])
h = eigen.Vector3d.Random()
rb = sva.RBInertiad(mass, h, I)
rb6d = rb.matrix()
w = eigen.Vector3d.Random()
v = eigen.Vector3d.Random()
mVec = sva.MotionVecd(w, v)
mVec6d = mVec.vector()
fVec = rb*mVec
fVec6d = rb6d*mVec6d
self.assertTrue(isinstance(fVec, sva.ForceVecd))
self.assertAlmostEqual((fVec6d - fVec.vector()).norm(), 0, delta = TOL)
def test(self):
# register custom pickle function
sva.copy_reg_pickle()
mv = sva.MotionVecd(e3.Vector6d.Random())
fv = sva.ForceVecd(e3.Vector6d.Random())
pt = sva.PTransformd(e3.Quaterniond(e3.Vector4d.Random().normalized()),
e3.Vector3d.Random())
rb = sva.RBInertiad(3., e3.Vector3d.Random(), e3.Matrix3d.Random())
ab = sva.ABInertiad(e3.Matrix3d.Random(), e3.Matrix3d.Random(),
e3.Matrix3d.Random())
def test_pickle(v):
pickled = pickle.dumps(v)
v2 = pickle.loads(pickled)
self.assertEqual(v, v2)
test_pickle(mv)
test_pickle(fv)
test_pickle(pt)
test_pickle(rb)
test_pickle(ab)
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)
rbd.forwardKinematics(mb, mbc)
X_s = sva.PTransformd(sva.RotY(-np.pi / 2.), e.Vector3d(0.1, 0., 0.))
mbv = MultiBodyViz(mbg,
mb,
endEffectorDict={'b4': (X_s, 0.1, (0., 1., 0.))})
# test MultiBodyViz
from tvtk.tools import ivtk
viewer = ivtk.viewer()
viewer.size = (640, 480)
mbv.addActors(viewer.scene)
mbv.display(mb, mbc)
# test axis
from axis import Axis
a1 = Axis(text='test', length=0.2)
a1.addActors(viewer.scene)
a1.X = sva.PTransformd(sva.RotX(np.pi / 2.), e.Vector3d.UnitX())
# test vector6d
from vector6d import ForceVecViz, MotionVecViz
M = sva.MotionVecd(e.Vector3d(0.2, 0.1, 0.), e.Vector3d(0.1, 0., 0.2))
F = sva.ForceVecd(e.Vector3d(-0.2, -0.1, 0.), e.Vector3d(-0.1, 0., -0.2))
MV = MotionVecViz(M, a1.X)
FV = ForceVecViz(
F, sva.PTransformd(sva.RotX(np.pi / 2.),
e.Vector3d.UnitX() * 1.4))
MV.addActors(viewer.scene)
FV.addActors(viewer.scene)
