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 matrix_to_vector_quaternion_array(matrix, inverse=False, verbose=0):
"""Convert a 4x4 Rt transformation matrix into a vector quaternion array
containing 3 vector entries (x, y, z) and 4 quaternion entries (x, y, z, w)
# Params
matrix: The 4x4 Rt rigid body transformation matrix to convert into a vector quaternion array.
inverse: Inverts the full matrix before loading into the array.
Useful when the transformation to be reversed and for testing/debugging purposes.
# Returns
np.array([x, y, z, qx, qy, qz, qw])
"""
rot = eigen.Matrix3d(matrix[:3, :3])
quaternion = eigen.Quaterniond(rot)
translation = matrix[:3, 3].transpose()
if inverse:
quaternion = quaternion.inverse()
translation *= -1
# coeffs are in xyzw order
q_floats_array = np.array(quaternion.coeffs())
# q_floats_array = np.array([quaternion.x(), quaternion.y(), quaternion.z(), quaternion.w()]).astype(np.float32)
vec_quat_7 = np.append(translation, q_floats_array)
if verbose > 0:
print(vec_quat_7)
return vec_quat_7
def test(self):
mass = 1.
I = eigen.Matrix3d([[1, 2, 3],[2, 1, 4], [3, 4, 1]])
h = eigen.Vector3d.Random()
rb2 = sva.RBInertiad(mass, h, I)
self.assertEqual(rb2.mass(), mass)
self.assertEqual(rb2.momentum(), h)
self.assertEqual(rb2.inertia(), I)
self.assertTrue(isUpperNull(rb2.lowerTriangularInertia()))
rb3 = sva.RBInertiad(mass, h, rb2.lowerTriangularInertia())
self.assertEqual(rb3.mass(), mass)
self.assertEqual(rb3.momentum(), h)
self.assertEqual(rb3.inertia(), I)
self.assertTrue(isUpperNull(rb3.lowerTriangularInertia()))
rb4 = rb2 + rb3
self.assertEqual(rb4.mass(), mass + mass)
self.assertEqual(rb4.momentum(), h + h)
self.assertEqual(rb4.inertia(), I + I)
self.assertTrue(isUpperNull(rb4.lowerTriangularInertia()))
rb5 = 2.*rb2
self.assertEqual(rb5.mass(), 2.*mass)
self.assertEqual(rb5.momentum(), 2.*h)
self.assertEqual(rb5.inertia(), 2.*I)
self.assertTrue(isUpperNull(rb5.lowerTriangularInertia()))
rb6 = rb2*2.
self.assertEqual(rb6.mass(), 2.*mass)
self.assertEqual(rb6.momentum(), 2.*h)
self.assertEqual(rb6.inertia(), 2.*I)
self.assertTrue(isUpperNull(rb6.lowerTriangularInertia()))
rb7 = rb2 - rb3
self.assertEqual(rb7.mass(), mass - mass)
self.assertEqual(rb7.momentum(), h - h)
self.assertEqual(rb7.inertia(), I - I)
self.assertTrue(isUpperNull(rb7.lowerTriangularInertia()))
rb8 = -rb2
self.assertEqual(rb8, rb2*-1)
self.assertTrue(isUpperNull(rb8.lowerTriangularInertia()))
rb9 = sva.RBInertiad(rb2)
rb9 += rb3
self.assertEqual(rb9, rb2 + rb3)
self.assertTrue(isUpperNull(rb9.lowerTriangularInertia()))
rb10 = sva.RBInertiad(rb2)
rb10 -= rb3
self.assertEqual(rb10, rb2 - rb3)
self.assertTrue(isUpperNull(rb10.lowerTriangularInertia()))
self.assertEqual(rb2, rb2)
self.assertNotEqual(rb2, rb6)
self.assertTrue(rb2 != rb6)
self.assertTrue(not(rb2 != rb2))
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):
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(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]])
ab1 = sva.ABInertiad(M, H, I)
self.assertEqual(ab1.massMatrix(), M)
self.assertTrue(isUpperNull(ab1.lowerTriangularMassMatrix()))
self.assertEqual(ab1.gInertia(), H)
self.assertEqual(ab1.inertia(), I)
self.assertTrue(isUpperNull(ab1.lowerTriangularInertia()))
ab2 = sva.ABInertiad(ab1.lowerTriangularMassMatrix(), H, ab1.lowerTriangularInertia())
self.assertEqual(ab2.massMatrix(), M)
self.assertTrue(isUpperNull(ab2.lowerTriangularMassMatrix()))
self.assertEqual(ab2.gInertia(), H)
self.assertEqual(ab2.inertia(), I)
self.assertTrue(isUpperNull(ab2.lowerTriangularInertia()))
ab3 = ab1 + ab2
self.assertEqual(ab3.massMatrix(), M + M)
self.assertTrue(isUpperNull(ab3.lowerTriangularMassMatrix()))
self.assertEqual(ab3.gInertia(), H + H)
self.assertEqual(ab3.inertia(), I + I)
self.assertTrue(isUpperNull(ab3.lowerTriangularInertia()))
ab4 = 2.*ab2
self.assertEqual(ab4.massMatrix(), 2.*M)
self.assertTrue(isUpperNull(ab4.lowerTriangularMassMatrix()))
self.assertEqual(ab4.gInertia(), 2.*H)
self.assertEqual(ab4.inertia(), 2.*I)
self.assertTrue(isUpperNull(ab4.lowerTriangularInertia()))
ab5 = ab2*2.
self.assertEqual(ab5.massMatrix(), 2.*M)
self.assertTrue(isUpperNull(ab5.lowerTriangularMassMatrix()))
self.assertEqual(ab5.gInertia(), 2.*H)
self.assertEqual(ab5.inertia(), 2.*I)
self.assertTrue(isUpperNull(ab5.lowerTriangularInertia()))
ab6 = ab1 - ab2
self.assertEqual(ab6.massMatrix(), M - M)
self.assertTrue(isUpperNull(ab6.lowerTriangularMassMatrix()))
self.assertEqual(ab6.gInertia(), H - H)
self.assertEqual(ab6.inertia(), I - I)
self.assertTrue(isUpperNull(ab6.lowerTriangularInertia()))
ab7 = -ab1
self.assertEqual(ab7, -1.*ab1)
self.assertTrue(isUpperNull(ab7.lowerTriangularMassMatrix()))
self.assertTrue(isUpperNull(ab7.lowerTriangularInertia()))
ab8 = sva.ABInertiad(ab1)
ab8 += ab2
self.assertEqual(ab8, ab1 + ab2)
self.assertTrue(isUpperNull(ab8.lowerTriangularMassMatrix()))
self.assertTrue(isUpperNull(ab8.lowerTriangularInertia()))
ab9 = sva.ABInertiad(ab1)
ab9 -= ab2
self.assertEqual(ab9, ab1 - ab2)
self.assertTrue(isUpperNull(ab9.lowerTriangularMassMatrix()))
self.assertTrue(isUpperNull(ab9.lowerTriangularInertia()))
self.assertEqual(ab2, ab2)
self.assertNotEqual(ab2, ab5)
self.assertTrue(ab2 != ab5)
self.assertTrue(not(ab2 != ab2))
def createRobot():
mb, mbc, mbg = rbdyn.MultiBody(), rbdyn.MultiBodyConfig(
), rbdyn.MultiBodyGraph()
limits = mc_rbdyn_urdf.Limits()
visual = {}
collision_tf = {}
I0 = eigen.Matrix3d([[0.1, 0.0, 0.0], [0.0, 0.05, 0.0], [0.0, 0.0, 0.001]])
I1 = eigen.Matrix3d([[1.35, 0.0, 0.0], [0.0, 0.05, 0.0], [0.0, 0.0,
1.251]])
I2 = eigen.Matrix3d([[0.6, 0.0, 0.0], [0.0, 0.05, 0.0], [0.0, 0.0, 0.501]])
I3 = eigen.Matrix3d([[0.475, 0.0, 0.0], [0.0, 0.05, 0.0],
[0.0, 0.0, 0.376]])
I4 = eigen.Matrix3d([[0.1, 0.0, 0.0], [0.0, 0.3, 0.0], [0.0, 0.0, 0.251]])
T0 = sva.PTransformd(eigen.Vector3d(0.1, 0.2, 0.3))
T1 = sva.PTransformd.Identity()
b0 = rbdyn.Body(1., eigen.Vector3d.Zero(), I0, "b0")
b1 = rbdyn.Body(5., eigen.Vector3d(0., 0.5, 0.), I1, "b1")
b2 = rbdyn.Body(2., eigen.Vector3d(0., 0.5, 0.), I2, "b2")
b3 = rbdyn.Body(1.5, eigen.Vector3d(0., 0.5, 0.), I3, "b3")
b4 = rbdyn.Body(1., eigen.Vector3d(0.5, 0., 0.), I4, "b4")
mbg.addBody(b0)
mbg.addBody(b1)
mbg.addBody(b2)
mbg.addBody(b3)
mbg.addBody(b4)
j0 = rbdyn.Joint(rbdyn.Joint.Rev, eigen.Vector3d.UnitX(), True, "j0")
j1 = rbdyn.Joint(rbdyn.Joint.Rev, eigen.Vector3d.UnitY(), True, "j1")
j2 = rbdyn.Joint(rbdyn.Joint.Rev, eigen.Vector3d.UnitZ(), True, "j2")
j3 = rbdyn.Joint(rbdyn.Joint.Rev, eigen.Vector3d.UnitX(), True, "j3")
mbg.addJoint(j0)
mbg.addJoint(j1)
mbg.addJoint(j2)
mbg.addJoint(j3)
to = sva.PTransformd(eigen.Vector3d(0, 1, 0))
from_ = sva.PTransformd.Identity()
mbg.linkBodies("b0", to, "b1", from_, "j0")
mbg.linkBodies("b1", to, "b2", from_, "j1")
mbg.linkBodies("b2", to, "b3", from_, "j2")
mbg.linkBodies("b1",
sva.PTransformd(sva.RotX(1.), eigen.Vector3d(1., 0., 0.)),
"b4", from_, "j3")
mb = mbg.makeMultiBody("b0", True)
mbc = rbdyn.MultiBodyConfig(mb)
mbc.zero(mb)
limits.lower = {
"j0": [-1.],
"j1": [-1.],
"j2": [-1.],
"j3": [-float('Inf')]
}
limits.upper = {"j0": [1.], "j1": [1.], "j2": [1.], "j3": [float('Inf')]}
limits.velocity = {
"j0": [10.],
"j1": [10.],
"j2": [10.],
"j3": [float('Inf')]
}
limits.torque = {
"j0": [50.],
"j1": [50.],
"j2": [50.],
"j3": [float('Inf')]
}
v1 = mc_rbdyn_urdf.Visual()
v1.origin = T0
geometry = mc_rbdyn_urdf.Geometry()
geometry.type = mc_rbdyn_urdf.Geometry.MESH
mesh = mc_rbdyn_urdf.GeometryMesh()
mesh.filename = "test_mesh1.dae"
geometry.data = mesh
v1.geometry = geometry
v2 = mc_rbdyn_urdf.Visual()
v2.origin = T1
mesh.filename = "test_mesh2.dae"
geometry.data = mesh
v2.geometry = geometry
visual = {b"b0": [v1, v2]}
return mb, mbc, mbg, limits, visual, collision_tf