def test_eigen_and_sva():
""" Test/demo of 3D geometry libraries eigen and sva
eigen # https://github.com/jrl-umi3218/Eigen3ToPython
sva # https://github.com/jrl-umi3218/SpaceVecAlg
"""
print('eigen and sva test')
if eigen is not None and sva is not None:
qa = np.array([0, 0, 0, 1])
v = eigen.Vector3d([1, 1, 1])
qa4 = eigen.Vector4d(qa)
q = eigen.Quaterniond(qa4)
pt = sva.PTransformd(q, v)
print(str(np.array(pt.rotation())))
rot = [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]
assert np.allclose(np.array(rot), np.array(pt.rotation()))
pt.translation()
trans = pt.translation()
assert trans.x() == 1.0
assert trans.y() == 1.0
assert trans.z() == 1.0
trans_np = np.array(trans)
assert np.allclose(trans_np, v)
def test(self):
create_random_pt = lambda: sva.PTransformd(
eigen.Quaterniond(eigen.Vector4d.Random().normalized()),
eigen.Vector3d().Random() * 100)
# Check empty creation and filling
v1 = sva.PTransformdVector()
for i in range(100):
v1.append(create_random_pt())
assert (len(v1) == 100)
# Check creation from a single PTransformd object
v2 = sva.PTransformdVector(create_random_pt())
assert (len(v2) == 1)
# Check creation from a list
v3 = sva.PTransformdVector([create_random_pt() for i in range(100)])
assert (len(v3) == 100)
# Check creation from a lot of PTransformd
v3 = sva.PTransformdVector(*[create_random_pt() for i in range(100)])
assert (len(v3) == 100)
# Check access
pt = create_random_pt()
v4 = sva.PTransformdVector([pt] * 100)
assert (all([pt == vi for vi in v4]))
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 ptransform_to_vector_quaternion_array(ptransform,
q_inverse=True,
dtype=np.float32):
"""Convert a PTransformD into a vector quaternion array
containing 3 vector entries (x, y, z) and 4 quaternion entries (x, y, z, w)
# Params
ptransform: The plucker transform from sva.PTransformd to be converted
q_inverse: Invert the quaternion after it is extracted, defaults to True.
With a PTransform the rotation component must be inverted before loading and after extraction.
See https://github.com/ahundt/grl/blob/master/include/grl/vrep/SpaceVecAlg.hpp#L22 for a well tested example.
See https://github.com/jrl-umi3218/Tasks/issues/10 for a detailed discussion leading to this conclusion.
The default for q_inverse should be True, do not switch the q_inverse default to False
without careful consideration and testing, though such a change may be appropriate when
loading into another transform representation in which the rotation component is expected
to be inverted.
TODO(ahundt) in process of debugging, correct docstring when all issues are resolved.
"""
rot = ptransform.rotation()
quaternion = eigen.Quaterniond(rot)
if q_inverse:
quaternion = quaternion.inverse()
translation = np.array(ptransform.translation()).reshape(3)
# coeffs are in xyzw order
q_floats_array = np.array(quaternion.coeffs())
vec_quat_7 = np.append(translation, q_floats_array)
return vec_quat_7.astype(dtype)
def test_configuration_writing():
tmpF = tempfile.NamedTemporaryFile(delete=False).name
config_ref = mc_rtc.Configuration()
ref_bool = False
config_ref.add("bool", ref_bool)
ref_uint = 42
config_ref.add("uint", ref_uint)
ref_int = -42
config_ref.add("int", ref_int)
ref_double = 42.5
config_ref.add("double", ref_double)
ref_string = "sometext"
config_ref.add("string", ref_string)
ref_v3d = eigen.Vector3d(1.2, 3.4, 5.6)
config_ref.add("v3d", ref_v3d)
ref_v6d = eigen.Vector6d(0.1, 1.2, 2.3, 3.4, 4.5, 5.6)
config_ref.add("v6d", ref_v6d)
ref_vxd = eigen.VectorXd(0.1, 3.2, 4.2, 4.5, 5.4)
config_ref.add("vxd", ref_vxd)
ref_quat = eigen.Quaterniond(0.71, 0, 0.71, 0)
ref_quat.normalize()
config_ref.add("quat", ref_quat)
ref_int_v = [0, 1, 2, 3, 4, 5]
config_ref.add("int_v", ref_int_v)
ref_double_v = [0.1, 1.0, 0.2, 2.0, 0.3]
config_ref.add("double_v", ref_double_v)
ref_double_v_v = [ref_double_v, ref_double_v, [0], [], [5.0, 4.0, 3.5]]
config_ref.add("double_v_v", ref_double_v_v)
ref_v3d_v = [eigen.Vector3d.Random() for i in range(10)]
config_ref.add("v3d_v", ref_v3d_v)
config_ref.add("dict")
config_ref("dict").add("int", ref_int)
config_ref.add("dict2").add("double_v", ref_double_v)
config_ref.save(tmpF)
config_test = mc_rtc.Configuration(tmpF)
assert (config_test("bool", bool) == ref_bool)
assert (config_test("uint", int) == ref_uint)
assert (config_test("int", int) == ref_int)
assert (config_test("double", float) == ref_double)
assert (config_test("string", str) == ref_string)
assert (config_test("v3d", eigen.Vector3d) == ref_v3d)
assert (config_test("v6d", eigen.Vector6d) == ref_v6d)
assert (config_test("vxd", eigen.VectorXd) == ref_vxd)
assert (config_test("quat", eigen.Quaterniond).isApprox(ref_quat))
assert (config_test("int_v", [int]) == ref_int_v)
assert (config_test("double_v", [float]) == ref_double_v)
assert (config_test("double_v_v", [[0.]]) == ref_double_v_v)
assert (all([
(v - ref_v).norm() < 1e-9
for v, ref_v in zip(config_test("v3d_v", [eigen.Vector3d]), ref_v3d_v)
]))
assert (config_test("dict")("int", int) == ref_int)
assert (config_test("dict2")("double_v", [float]) == ref_double_v)
config_test("dict2")("double_v").save(tmpF)
assert (mc_rtc.Configuration(tmpF).to([float]) == ref_double_v)
def run_callback(self):
hj1_q = self.robot().mbc.q[self.hj1_index][0]
self.v3d_data = eigen.Vector3d.Random()
self.d_data += 1.0
self.dv_data = [self.d_data] * 3
self.theta += 0.005
self.quat_data = eigen.Quaterniond(sva.RotZ(self.theta))
return True
def prismaticJoint(joint):
"""
Return a prism and the appropriate static transform.
"""
axis = e.Vector3d(joint.motionSubspace()[3:])
s = tvtk.CubeSource(x_length=0.02, y_length=0.1, z_length=0.02)
quat = e.Quaterniond()
quat.setFromTwoVectors(axis, e.Vector3d.UnitY())
return makeActor(s, tuple(axis)), sva.PTransformd(quat)
def revoluteJoint(joint):
"""
Return a cylinder and the appropriate static transform.
"""
axis = e.Vector3d(joint.motionSubspace()[:3])
s = tvtk.CylinderSource(height=0.1, radius=0.02)
quat = e.Quaterniond()
# Cylinder is around the Y axis
quat.setFromTwoVectors(axis, e.Vector3d.UnitY())
return makeActor(s, tuple(axis)), sva.PTransformd(quat)
def vector_quaternion_array_to_ptransform(vector_quaternion_array,
q_inverse=True,
t_inverse=False,
pt_inverse=False):
"""Convert a vector and quaternion pose array to a plucker transform.
# Params
vector_quaternion_array: A numpy array containing a pose.
A pose is a 6 degree of freedom rigid transform represented with 7 values:
[x, y, z, qx, qy, qz, qw] consisting of a
vector (x, y, z) for cartesian motion and quaternion (qx, qy, qz, qw) for rotation.
This is the data format used by the google brain robot data grasping dataset's
tfrecord poses.
eigen Quaterniond is also ordered xyzw.
q_inverse: Invert the quaternion before it is loaded into the PTransformd, defaults to True.
With a PTransform the rotation component must be inverted before loading and after extraction.
See https://github.com/ahundt/grl/blob/master/include/grl/vrep/SpaceVecAlg.hpp#L22 for a well tested example.
See https://github.com/jrl-umi3218/Tasks/issues/10 for a detailed discussion leading to this conclusion.
The default for q_inverse should be True, do not switch the q_inverse default to False
without careful consideration and testing, though such a change may be appropriate when
loading into another transform representation in which the rotation component is expected
to be inverted.
# Returns
A plucker transform PTransformd object as defined by the spatial vector algebra library.
https://github.com/jrl-umi3218/SpaceVecAlg
https://en.wikipedia.org/wiki/Pl%C3%BCcker_coordinates
"""
v = eigen.Vector3d(vector_quaternion_array[:3])
if t_inverse is True:
v *= -1
# TODO(ahundt) use following lines after https://github.com/jrl-umi3218/Eigen3ToPython/pull/15 is fixed
# qa4 = eigen.Vector4d()
# q = eigen.Quaterniond(qa4)
# Quaterniond important coefficient ordering details:
# scalar constructor is Quaterniond(w,x,y,z)
# vector constructor is Quaterniond(np.array([x,y,z,w]))
# Quaterniond.coeffs() is [x,y,z,w]
# https://eigen.tuxfamily.org/dox/classEigen_1_1Quaternion.html
xyzw = eigen.Vector4d(vector_quaternion_array[3:])
q = eigen.Quaterniond(xyzw)
# The ptransform needs the rotation component to inverted before construction.
# see https://github.com/ahundt/grl/blob/master/include/grl/vrep/SpaceVecAlg.hpp#L22 for a well tested example
# see https://github.com/jrl-umi3218/Tasks/issues/10 for a detailed discussion leading to this conclusion
if q_inverse is True:
q = q.inverse()
pt = sva.PTransformd(q, v)
if pt_inverse is True:
pt = pt.inv()
return pt
def vector_to_ptransform(XYZ):
"""Convert (x,y,z) translation to sva.Ptransformd.
Convert an xyz 3 element translation vector to an sva.PTransformd plucker
ptransform object with no rotation applied. In other words,
the rotation component will be the identity rotation.
"""
q = eigen.Quaterniond()
q.setIdentity()
v = eigen.Vector3d(XYZ)
ptransform = sva.PTransformd(q, v)
return ptransform
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 _createVector(self, vector, frame, color): source = tvtk.ArrowSource() pdm = tvtk.PolyDataMapper(input=source.output) actor = tvtk.Actor(mapper=pdm) actor.user_transform = tvtk.Transform() actor.property.color = color norm = vector.norm() actor.scale = (norm,)*3 quat = e.Quaterniond() # arrow are define on X axis quat.setFromTwoVectors(vector, e.Vector3d.UnitX()) X = sva.PTransformd(quat)*frame setActorTransform(actor, X) return actor, X
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 run_callback(self):
hj1_q = self.robot().mbc.q[self.hj1_index][0]
self.v3d_data = eigen.Vector3d.Random()
self.d_data += 1.0
self.dv_data = [self.d_data] * 3
self.theta += 0.005
self.quat_data = eigen.Quaterniond(sva.RotZ(self.theta))
assert (self.observerPipeline("FirstPipeline").success())
if abs(self.d_data - 2.0) < 1e-6:
self.removeAnchorFrameCallback("KinematicAnchorFrame::{}".format(
self.robot().name()))
self.addAnchorFrameCallback(
"KinematicAnchorFrame::{}".format(self.robot().name()),
self.anchorFrameCallback)
return True
def test(self):
X_a_b = sva.PTransformd(
eigen.Quaterniond(eigen.Vector4d.Random()).normalized(),
eigen.Vector3d.Random())
X_a_c = sva.PTransformd(
eigen.Quaterniond(eigen.Vector4d.Random()).normalized(),
eigen.Vector3d.Random())
V_a_b = sva.transformVelocity(X_a_b)
V_a_c = sva.transformVelocity(X_a_c)
self.assertAlmostEqual(
(V_a_b.angular() - sva.rotationVelocity(X_a_b.rotation())).norm(),
0,
delta=TOL)
self.assertAlmostEqual((V_a_b.linear() - X_a_b.translation()).norm(),
0,
delta=TOL)
self.assertAlmostEqual(
(V_a_c.angular() - sva.rotationVelocity(X_a_c.rotation())).norm(),
0,
delta=TOL)
self.assertAlmostEqual((V_a_c.linear() - X_a_c.translation()).norm(),
0,
delta=TOL)
V_b_c_a = sva.transformError(X_a_b, X_a_c)
w_b_c_a = sva.rotationError(X_a_b.rotation(), X_a_c.rotation())
v_b_c_a = X_a_c.translation() - X_a_b.translation()
self.assertAlmostEqual((V_b_c_a.angular() - w_b_c_a).norm(),
0,
delta=TOL)
self.assertAlmostEqual((V_b_c_a.linear() - v_b_c_a).norm(),
0,
delta=TOL)
def _createVector(self, vector, frame, color):
source = tvtk.ArrowSource()
# pdm = tvtk.PolyDataMapper(input=source.output)
pdm = tvtk.PolyDataMapper()
# https://github.com/enthought/mayavi/issues/521
pdm.input_connection = source.output_port
# configure_input_data(pdm, source)
prop = tvtk.Property(color=color)
actor = tvtk.Actor(mapper=pdm, property=prop)
actor.user_transform = tvtk.Transform()
# commented due to api change, new lines are above Actor creation
# actor.property.color = color
norm = vector.norm()
actor.scale = (norm, ) * 3
quat = e.Quaterniond()
# arrow are define on X axis
quat.setFromTwoVectors(vector, e.Vector3d.UnitX())
X = sva.PTransformd(quat) * frame
transform.setActorTransform(actor, X)
return actor, X
def __init__(self, rm, dt):
self.qpsolver.addConstraintSet(self.dynamicsConstraint)
self.qpsolver.addConstraintSet(self.contactConstraint)
self.qpsolver.addTask(self.postureTask)
self.positionTask = mc_tasks.PositionTask("R_WRIST_Y_S", self.robots(),
0)
self.qpsolver.addTask(self.positionTask)
self.qpsolver.setContacts([
mc_rbdyn.Contact(self.robots(), "LeftFoot", "AllGround"),
mc_rbdyn.Contact(self.robots(), "RightFoot", "AllGround")
])
self.hj1_name = "NECK_P"
self.hj1_index = self.robot().jointIndexByName(self.hj1_name)
self.hj1_q = 0.5
# Stuff to log
self.v3d_data = eigen.Vector3d.Random()
self.logger().addLogEntry("PYTHONV3D", lambda: self.v3d_data)
self.d_data = 0.0
self.dv_data = [self.d_data] * 3
self.theta = 0
self.quat_data = eigen.Quaterniond(sva.RotZ(self.theta))
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):
mb1, mbc1Init = arms.makeZXZArm()
rbdyn.forwardKinematics(mb1, mbc1Init)
rbdyn.forwardVelocity(mb1, mbc1Init)
mb2, mbc2Init = arms.makeZXZArm(False)
if not LEGACY:
mb2InitPos = mbc1Init.bodyPosW[-1].translation()
else:
mb2InitPos = list(mbc1Init.bodyPosW)[-1].translation()
mb2InitOri = eigen.Quaterniond(sva.RotY(math.pi / 2))
if not LEGACY:
mbc2Init.q[0] = [
mb2InitOri.w(),
mb2InitOri.x(),
mb2InitOri.y(),
mb2InitOri.z(),
mb2InitPos.x(),
mb2InitPos.y() + 1,
mb2InitPos.z()
]
mbc2Init.q[0] = [
mb2InitOri.w(),
mb2InitOri.x(),
mb2InitOri.y(),
mb2InitOri.z(),
mb2InitPos.x(),
mb2InitPos.y() + 1,
mb2InitPos.z()
]
rbdyn.forwardKinematics(mb2, mbc2Init)
rbdyn.forwardVelocity(mb2, mbc2Init)
if not LEGACY:
X_0_b1 = sva.PTransformd(mbc1Init.bodyPosW[-1])
X_0_b2 = sva.PTransformd(mbc2Init.bodyPosW[-1])
else:
X_0_b1 = sva.PTransformd(list(mbc1Init.bodyPosW)[-1])
X_0_b2 = sva.PTransformd(list(mbc2Init.bodyPosW)[-1])
X_b1_b2 = X_0_b2 * X_0_b1.inv()
if not LEGACY:
mbs = rbdyn.MultiBodyVector([mb1, mb2])
mbcs = rbdyn.MultiBodyConfigVector([mbc1Init, mbc2Init])
else:
mbs = [mb1, mb2]
mbcs = [
rbdyn.MultiBodyConfig(mbc1Init),
rbdyn.MultiBodyConfig(mbc2Init)
]
# Test ContactAccConstr constraint and PositionTask on the second robot
solver = tasks.qp.QPSolver()
points = [
eigen.Vector3d(0.1, 0, 0.1),
eigen.Vector3d(0.1, 0, -0.1),
eigen.Vector3d(-0.1, 0, -0.1),
eigen.Vector3d(-0.1, 0, 0.1),
]
biPoints = [
eigen.Vector3d.Zero(),
eigen.Vector3d.Zero(),
eigen.Vector3d.Zero(),
eigen.Vector3d.Zero(),
]
nrGen = 4
biFrames = [
sva.RotX(math.pi / 4),
sva.RotX(3 * math.pi / 4),
sva.RotX(math.pi / 4) * sva.RotY(math.pi / 2),
sva.RotX(3 * math.pi / 4) * sva.RotY(math.pi / 2),
]
# The fixed robot can pull the other
contVecFail = [
tasks.qp.UnilateralContact(0, 1, "b3", "b0", points,
sva.RotX(-math.pi / 2), X_b1_b2, nrGen,
0.7)
]
# The fixed robot can push the other
contVec = [
tasks.qp.UnilateralContact(0, 1, "b3", "b0", points,
sva.RotX(math.pi / 2), X_b1_b2, nrGen,
0.7)
]
# The fixed robot has non coplanar force apply on the other
contVecBi = [
tasks.qp.BilateralContact(tasks.qp.ContactId(0, 1, "b3", "b0"),
biPoints, biFrames, X_b1_b2, nrGen, 1)
]
if not LEGACY:
posture1Task = tasks.qp.PostureTask(mbs, 0, mbc1Init.q, 2, 1)
posture2Task = tasks.qp.PostureTask(mbs, 1, mbc2Init.q, 2, 1)
else:
posture1Task = tasks.qp.PostureTask(mbs, 0, rbdList(mbc1Init.q), 2,
1)
posture2Task = tasks.qp.PostureTask(mbs, 1, rbdList(mbc2Init.q), 2,
1)
contCstrSpeed = tasks.qp.ContactSpeedConstr(0.001)
Inf = float("inf")
torqueMin1 = [[], [-Inf], [-Inf], [-Inf]]
torqueMax1 = [[], [Inf], [Inf], [Inf]]
torqueMin2 = [[0, 0, 0, 0, 0, 0], [-Inf], [-Inf], [-Inf]]
torqueMax2 = [[0, 0, 0, 0, 0, 0], [Inf], [Inf], [Inf]]
motion1 = tasks.qp.MotionConstr(
mbs, 0, tasks.TorqueBound(torqueMin1, torqueMax1))
motion2 = tasks.qp.MotionConstr(
mbs, 1, tasks.TorqueBound(torqueMin2, torqueMax2))
plCstr = tasks.qp.PositiveLambda()
motion1.addToSolver(solver)
motion2.addToSolver(solver)
plCstr.addToSolver(solver)
contCstrSpeed.addToSolver(solver)
solver.addTask(posture1Task)
solver.addTask(posture2Task)
# Check the impossible motion
solver.nrVars(mbs, contVecFail, [])
solver.updateConstrSize()
self.assertEqual(solver.nrVars(), 3 + 9 + 4 * nrGen)
self.assertFalse(solver.solve(mbs, mbcs))
# Check the unilateral motion
if not LEGACY:
mbcs = rbdyn.MultiBodyConfigVector([mbc1Init, mbc2Init])
else:
mbcs = [
rbdyn.MultiBodyConfig(mbc1Init),
rbdyn.MultiBodyConfig(mbc2Init)
]
solver.nrVars(mbs, contVec, [])
solver.updateConstrSize()
for i in range(1000):
if not LEGACY:
self.assertTrue(solver.solve(mbs, mbcs))
else:
self.assertTrue(solver.solveNoMbcUpdate(mbs, mbcs))
solver.updateMbc(mbcs[0], 0)
solver.updateMbc(mbcs[1], 1)
for i in range(2):
rbdyn.eulerIntegration(mbs[i], mbcs[i], 0.001)
rbdyn.forwardKinematics(mbs[i], mbcs[i])
rbdyn.forwardVelocity(mbs[i], mbcs[i])
# Check that the link hold
if not LEGACY:
X_0_b1_post = mbcs[0].bodyPosW[-1]
X_0_b2_post = mbcs[1].bodyPosW[-1]
else:
X_0_b1_post = list(mbcs[0].bodyPosW)[-1]
X_0_b2_post = list(mbcs[1].bodyPosW)[-1]
X_b1_b2_post = X_0_b2 * X_0_b1.inv()
self.assertAlmostEqual(
(X_b1_b2.matrix() - X_b1_b2_post.matrix()).norm(),
0,
delta=1e-5)
# Force in the world frame must be the same
f1 = contVec[0].force(solver.lambdaVec(0), contVec[0].r1Cone)
f2 = contVec[0].force(solver.lambdaVec(0), contVec[0].r2Cone)
self.assertAlmostEqual((f1 + f2).norm(), 0, delta=1e-5)
# Check the bilateral motion
if not LEGACY:
mbcs = rbdyn.MultiBodyConfigVector([mbc1Init, mbc2Init])
else:
mbcs = [
rbdyn.MultiBodyConfig(mbc1Init),
rbdyn.MultiBodyConfig(mbc2Init)
]
solver.nrVars(mbs, contVec, [])
solver.updateConstrSize()
self.assertEqual(solver.nrVars(), 3 + 9 + 4 * nrGen)
for i in range(1000):
if not LEGACY:
self.assertTrue(solver.solve(mbs, mbcs))
else:
self.assertTrue(solver.solveNoMbcUpdate(mbs, mbcs))
solver.updateMbc(mbcs[0], 0)
solver.updateMbc(mbcs[1], 1)
for i in range(2):
rbdyn.eulerIntegration(mbs[i], mbcs[i], 0.001)
rbdyn.forwardKinematics(mbs[i], mbcs[i])
rbdyn.forwardVelocity(mbs[i], mbcs[i])
# Check that the link hold
if not LEGACY:
X_0_b1_post = mbcs[0].bodyPosW[-1]
X_0_b2_post = mbcs[1].bodyPosW[-1]
else:
X_0_b1_post = list(mbcs[0].bodyPosW)[-1]
X_0_b2_post = list(mbcs[1].bodyPosW)[-1]
X_b1_b2_post = X_0_b2 * X_0_b1.inv()
self.assertAlmostEqual(
(X_b1_b2.matrix() - X_b1_b2_post.matrix()).norm(),
0,
delta=1e-5)
# Force in the world frame must be the same
f1 = contVec[0].force(solver.lambdaVec(0), contVec[0].r1Cone)
f2 = contVec[0].force(solver.lambdaVec(0), contVec[0].r2Cone)
self.assertAlmostEqual((f1 + f2).norm(), 0, delta=1e-5)
plCstr.removeFromSolver(solver)
motion2.removeFromSolver(solver)
motion1.removeFromSolver(solver)
contCstrSpeed.removeFromSolver(solver)
solver.removeTask(posture1Task)
solver.removeTask(posture2Task)
def test_configuration_reading(config, fromDisk2):
@raises(RuntimeError)
def test_throw():
config("NONE")
test_throw()
assert (config("int", int) == 42)
assert (config("int", 0) == 42)
assert (config("NONE", 100) == 100)
assert (config("dict")("int", int) == 42)
assert (config("double", float) == 42.5)
assert (config("double", 0.) == 42.5)
assert (config("NONE", 42.42) == 42.42)
assert (config("dict")("double", float) == 42.5)
assert (config("string", str) == "sometext")
try:
assert (config("string", unicode) == "sometext")
except NameError:
pass
assert (config("string", "") == "sometext")
assert (config("string", u"") == "sometext")
assert (config("NONE", "another") == "another")
assert (config("dict")("string", "") == "sometext")
ref = eigen.Vector3d(1.0, 2.3, -100)
zero = eigen.Vector3d.Zero()
assert (config("v3d", eigen.Vector3d) == ref)
assert (config("v3d", zero) == ref)
assert (config("v6d", zero) == zero)
@raises(RuntimeError)
def test_v6d_to_v3d_throw():
config("v6d", eigen.Vector3d)
test_v6d_to_v3d_throw()
assert (config("dict")("v3d", eigen.Vector3d) == ref)
assert (config("dict")("v3d", zero) == ref)
ref = eigen.Vector6d(1.0, -1.5, 2.0, -2.5, 3.0, -3.5)
zero = eigen.Vector6d.Zero()
assert (config("v6d", eigen.Vector6d) == ref)
assert (config("v6d", zero) == ref)
assert (config("v3d", zero) == zero)
@raises(RuntimeError)
def test_v3d_to_v6d_throw():
config("v3d", eigen.Vector6d)
test_v3d_to_v6d_throw()
assert (config("dict")("v6d", eigen.Vector6d) == ref)
assert (config("dict")("v6d", zero) == ref)
ref3 = eigen.VectorXd(1, 2.3, -100)
ref6 = eigen.VectorXd(ref)
ref = eigen.VectorXd(0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10)
assert (config("v6d", eigen.VectorXd) == ref6)
assert (config("v3d", eigen.VectorXd) == ref3)
assert (config("vXd", eigen.VectorXd) == ref)
@raises(RuntimeError)
def test_int_tovxd_throw():
config("int", eigen.VectorXd)
test_int_tovxd_throw()
assert (config("dict")("v6d", eigen.VectorXd) == ref6)
assert (config("dict")("v3d", eigen.VectorXd) == ref3)
assert (config("emptyArray",
eigen.VectorXd.Zero(100)) == eigen.VectorXd.Zero(0))
ref = eigen.Quaterniond(0.71, 0, 0.71, 0)
ref.normalize()
assert (config("quat", eigen.Quaterniond).isApprox(ref))
assert (config("dict")("quat", eigen.Quaterniond).isApprox(ref))
assert (config("boolTrue", bool))
assert (config("boolTrue", False))
assert (not config("boolFalse", bool))
assert (not config("boolFalse", True))
assert (config("bool1", bool))
assert (not config("bool0", bool))
assert (config("dict")("boolTrue", bool))
assert (not config("dict")("bool0", bool))
ref = [0, 1, 2, 3, 4, 5]
assert ([c.to(int) for c in config("intV")] == ref)
assert ([c.to(int) for c in config("dict")("intV")] == ref)
assert ([c.to(float) for c in config("intV")] == ref)
assert ([c.to(float) for c in config("dict")("intV")] == ref)
assert (config("intV", [int]) == ref)
assert (config("intV", [0]) == ref)
ref = ["a", "b", "c", "foo", "bar"]
assert ([c.to(str) for c in config("stringV")] == ref)
assert ([c.to(str) for c in config("dict")("stringV")] == ref)
ref = [1.1, 2.2, 3.3]
assert ([c.to(float) for c in config("doubleA3")] == ref)
ref = [42.5, -42.5]
assert ([c.to(float) for c in config("doubleDoublePair")] == ref)
ref = [42.5, "sometext"]
c = config("doubleStringPair")
assert ([c[0].to(float), c[1].to(str)] == ref)
if fromDisk2:
config.load(sampleConfig2(True))
else:
config.loadData(sampleConfig2(False))
assert (config("int", int) == 12)
assert (config("sint", int) == -42)
ref = ["a2", "b2", "c2"]
assert ([c.to(str) for c in config("stringV")] == ref)
def rpyFromQuat(quat):
"""Same as mc_rbdyn::rpyFromQuat."""
import eigen
return rpyFromMat(list(eigen.Quaterniond(*quat).toRotationMatrix()))
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)
