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 test(self):
mb1, mbc1Init = arms.makeZXZArm(True, sva.PTransformd(sva.RotZ(-math.pi/4), eigen.Vector3d(-0.5, 0, 0)))
rbdyn.forwardKinematics(mb1, mbc1Init)
rbdyn.forwardVelocity(mb1, mbc1Init)
mb2, mbc2Init = arms.makeZXZArm(False, sva.PTransformd(sva.RotZ(math.pi/2), eigen.Vector3d(0.5, 0, 0)))
rbdyn.forwardKinematics(mb2, mbc2Init)
rbdyn.forwardVelocity(mb2, mbc2Init)
if not LEGACY:
mbs = rbdyn.MultiBodyVector([mb1, mb2])
mbcs = rbdyn.MultiBodyConfigVector([mbc1Init, mbc2Init])
else:
mbs = [mb1, mb2]
mbcs = [rbdyn.MultiBodyConfig(mbc1Init), rbdyn.MultiBodyConfig(mbc2Init)]
solver = tasks.qp.QPSolver()
if not LEGACY:
posture1Task = tasks.qp.PostureTask(mbs, 0, mbc1Init.q, 0.1, 10)
posture2Task = tasks.qp.PostureTask(mbs, 1, mbc2Init.q, 0.1, 10)
else:
posture1Task = tasks.qp.PostureTask(mbs, 0, rbdList(mbc1Init.q), 2, 1)
posture2Task = tasks.qp.PostureTask(mbs, 1, rbdList(mbc2Init.q), 2, 1)
mrtt = tasks.qp.MultiRobotTransformTask(mbs, 0, 1, "b3", "b3", sva.PTransformd(sva.RotZ(-math.pi/8)), sva.PTransformd.Identity(), 100, 1000)
if not LEGACY:
mrtt.dimWeight(eigen.VectorXd(0, 0, 1, 1, 1, 0))
else:
mrtt.dimWeight(eigen.Vector6d(0, 0, 1, 1, 1, 0))
solver.addTask(posture1Task)
solver.addTask(posture2Task)
solver.addTask(mrtt)
solver.nrVars(mbs, [], [])
solver.updateConstrSize()
# 3 dof + 9 dof
self.assertEqual(solver.nrVars(), 3 + 9)
for i in range(2000):
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])
self.assertAlmostEqual(mrtt.eval().norm(), 0, delta = 1e-3)
solver.removeTask(posture1Task)
solver.removeTask(posture2Task)
solver.removeTask(mrtt)
def __init__(self):
self.nrvar = 6
self.nreq = 3
self.nrineq = 2
self.Q = eigen.MatrixXd.Identity(self.nrvar, self.nrvar)
self.Aeq = eigen.MatrixXd([ [1., -1., 1., 0., 3., 1.],
[-1., 0., -3., -4., 5., 6.],
[2., 5., 3., 0., 1., 0.] ])
self.Beq = eigen.VectorXd([1., 2., 3.])
self.Aineq = eigen.MatrixXd([ [0., 1., 0., 1., 2., -1.],
[-1., 0., 2., 1., 1., 0.] ])
self.Bineq = eigen.VectorXd([-1., 2.5])
self.XL = eigen.VectorXd([ -1000., -10000., 0., -1000., -1000.,-1000. ])
self.XU = eigen.VectorXd([ 10000., 100., 1.5, 100., 100., 1000. ])
self.C = eigen.VectorXd([ 1., 2., 3., 4., 5., 6. ])
self.X = eigen.VectorXd([ 1.7975426, -0.3381487, 0.1633880, -4.9884023, 0.6054943, -3.1155623 ])
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)
print dir(y) x1 = x() print x1 y1 = y() print y1 print dir(x1) print dir(y1) x2 = x(10) print x2[0] print dir(x2[0]) print dir(eigen) s = eigen.MatrixXd(3, 4) print s r = eigen.VectorXd(10) print r print dir(r) r.__setitem__(0, 20) print r print type(s) print type(r)
# -*- encoding=utf-8 -*- import numpy as np import sys import eigen from ctypes import * # 查看python模块默认安装路径 print sys.path A = np.random.random((2000, 50)) B = eigen.MatrixXd(A) n = np.linalg.norm(B) # Implicit conversion to numpy object vector_1D = eigen.VectorXd(3) vector_1D.coeff(1, 2) print type(vector_1D) print vector_1D matrix_1D = eigen.MatrixXd(3, 1) matrix_1D.setZero() print matrix_1D