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))