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_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 test(self):
n = eigen.Vector3d.Random()
f = eigen.Vector3d.Random()
vec = sva.ForceVecd(n, f)
m = vec.vector()
self.assertEqual(n, vec.couple())
self.assertEqual(f, vec.force())
self.assertEqual(m, eigen.Vector6d(list(n) + list(f)))
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)
n2 = eigen.Vector3d.Random()
f2 = eigen.Vector3d.Random()
vec2 = sva.ForceVecd(n2, f2)
m2 = vec2.vector()
self.assertEqual((vec + vec2).vector(), (m + m2))
self.assertEqual((vec - vec2).vector(), (m - m2))
vec_pluseq = sva.ForceVecd(vec)
vec_pluseq += vec2
self.assertEqual(vec_pluseq, vec + vec2)
vec_minuseq = sva.ForceVecd(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.ForceVecd([0, 0, 0], [0, 0, 0])
self.assertEqual(sva.ForceVecd.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_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 reset_callback(self, reset_data):
assert (len(self.observerPipelines()) == 1)
assert (self.hasObserverPipeline("FirstPipeline"))
assert (not self.hasObserverPipeline("NotAPipeline"))
self.addAnchorFrameCallback(
"KinematicAnchorFrame::{}".format(self.robot().name().decode()),
lambda r: sva.interpolate(r.surfacePose("LeftFoot"),
r.surfacePose("RightFoot"), 0.5))
self.positionTask.reset()
self.positionTask.position(self.positionTask.position() +
eigen.Vector3d(0.1, 0, 0))
self.logger().addLogEntry("PYTHONDOUBLE", lambda: self.d_data)
self.logger().addLogEntry("PYTHONDOUBLEV", lambda: self.dv_data)
self.logger().addLogEntry("PYTHONQUAT", lambda: self.quat_data)
# Demonstrate use of partial
self.logger().addLogEntry("PYTHONPT", partial(self.get_pt))
# Alternative syntax for above call
# self.logger().addLogEntry("PYTHONPT", partial(SampleController.get_pt, self))
self.logger().addLogEntry(
"PYTHONFV",
lambda: sva.ForceVecd(eigen.Vector6d(0, 0, 0, 0, 0, 100)) + sva.
ForceVecd(eigen.Vector6d(0, 0, 0, 0, 0, 100)))
# Not a very efficient way to log a double value
self.logger().addLogEntry("PYTHONSTR", lambda: str(self.theta))
# GUI similar to dummyServer
self.gui().addElement(["Python"],
mc_rtc.gui.Label("theta", lambda: self.theta))
self.gui().addElement(
["Python"],
mc_rtc.gui.ArrayLabel(
"array", lambda: [self.theta, 2 * self.theta, 4 * self.theta]),
mc_rtc.gui.ArrayLabel(
"array with labels",
lambda: [self.theta, 2 * self.theta, 4 * self.theta],
["x", "y", "z"]))
self.gui().addElement(
["Python"],
mc_rtc.gui.Button("button",
lambda: sys.stdout.write("Button clicked\n")))
self.check_ = True
self.gui().addElement(["Python"],
mc_rtc.gui.Checkbox("checkbox",
lambda: self.check_,
self.check))
self.mystring_ = "test"
self.myint_ = 0
self.mynumber_ = 0.0
self.gui().addElement(["Python"],
mc_rtc.gui.StringInput("string input",
self.mystring,
self.mystring),
mc_rtc.gui.IntegerInput("integer input",
self.myint, self.myint),
mc_rtc.gui.NumberInput("number input",
self.mynumber,
self.mynumber))
self.myslider_ = 0.0
self.gui().addElement(["Python"],
mc_rtc.gui.NumberSlider("number slider",
self.myslider,
self.myslider, -100.0,
100.0))
self.myarray_ = [0., 1., 2., 3.]
self.gui().addElement(["Python"],
mc_rtc.gui.ArrayInput("array input",
self.myarray,
self.myarray))
self.gui().addElement(["Python"],
mc_rtc.gui.ArrayInput("array input with labels",
self.myarray, self.myarray,
["w", "x", "y", "z"]))
self.mycombo_ = "a"
self.gui().addElement(["Python"],
mc_rtc.gui.ComboInput("combo input",
["a", "b", "c", "d"],
self.mycombo,
self.mycombo))
self.mydatacombo_ = ""
self.gui().addElement(["Python"],
mc_rtc.gui.DataComboInput(
"data combo", ["robots"], self.mydatacombo,
self.mydatacombo))
self.mypoint3dro_ = [0., 1., 0.]
self.gui().addElement(["Python"],
mc_rtc.gui.Point3D("Point3D ro",
self.mypoint3dro))
self.mypoint3d_ = [0., 0., 1.]
self.gui().addElement(["Python"],
mc_rtc.gui.Point3D("Point3D", self.mypoint3d,
self.mypoint3d))
self.gui().addElement(
["Python", "Form"],
mc_rtc.gui.Form(
"Submit", lambda c: sys.stdout.write(c.dump(True) + '\n'),
mc_rtc.gui.FormCheckbox("Enabled", False, True),
mc_rtc.gui.FormIntegerInput("INT", False, 42),
mc_rtc.gui.FormNumberInput("NUMBER", False, 0.42),
mc_rtc.gui.FormStringInput("STRING", False,
"a certain string"),
mc_rtc.gui.FormNumberArrayInput("ARRAY_FIXED_SIZE", False,
[1, 2, 3]),
mc_rtc.gui.FormNumberArrayInput("ARRAY_UNBOUNDED", False),
mc_rtc.gui.FormComboInput("CHOOSE WISELY", False,
["A", "B", "C", "D"]),
mc_rtc.gui.FormDataComboInput("R0", False, ["robots"]),
mc_rtc.gui.FormDataComboInput("R0 surface", False,
["surfaces", "$R0"]),
mc_rtc.gui.FormDataComboInput("R1", False, ["robots"]),
mc_rtc.gui.FormDataComboInput("R1 surface", False,
["surfaces", "$R1"])))
self._selfDestructId = 0
self.gui().addElement(
["Python", "Experiment"],
mc_rtc.gui.Button(
"Self-destruct category",
lambda: self.gui().removeCategory(["Python", "Experiment"])))
self.gui().addElement(["Python", "Experiment"],
mc_rtc.gui.Button("Add self-destruct button",
self.addSelfDestructButton))
