def test(self): 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) fVec = sva.ForceVecd(n, f) mm = mVec.vector() mf = fVec.vector() self.assertAlmostEqual(mVec.dot(fVec) - (mm.transpose() * mf)[0], 0, delta=TOL) w2 = eigen.Vector3d.Random() * 100 v2 = eigen.Vector3d.Random() * 100 mVec2 = sva.MotionVecd(w2, v2) mm2 = mVec2.vector() crossM = mVec.cross(mVec2) self.assertTrue(isinstance(crossM, sva.MotionVecd)) self.assertAlmostEqual( (crossM.vector() - sva.vector6ToCrossMatrix(mm) * mm2).norm(), 0, delta=TOL) crossF = mVec.crossDual(fVec) self.assertTrue(isinstance(crossF, sva.ForceVecd)) self.assertAlmostEqual( (crossF.vector() - sva.vector6ToCrossDualMatrix(mm) * mf).norm(), 0, delta=TOL)
def test(self): w = eigen.Vector3d.Random() v = eigen.Vector3d.Random() vec = sva.AdmittanceVecd(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.AdmittanceVecd(w2, v2) z2 = vec2.vector() self.assertEqual((vec + vec2).vector(), z + z2) vec_pluseq = sva.AdmittanceVecd(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() fv = sva.ForceVecd(eigen.Vector3d.Random(), eigen.Vector3d.Random()) mv = vec * fv self.assertTrue(isinstance(mv, sva.MotionVecd)) res = eigen.Vector6d( [x * y for x, y in zip(vec.vector(), fv.vector())]) self.assertEqual(res, mv.vector()) mv2 = fv * vec self.assertEqual(mv, mv2) hv = sva.AdmittanceVecd(11., 42.) self.assertEqual(hv.angular(), eigen.Vector3d(11., 11., 11.)) self.assertEqual(hv.linear(), eigen.Vector3d(42., 42., 42.)) z = sva.AdmittanceVecd(0., 0.) self.assertEqual(sva.AdmittanceVecd.Zero(), z)
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): # 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): 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)
rbd.forwardKinematics(mb, mbc) X_s = sva.PTransformd(sva.RotY(-np.pi / 2.), e.Vector3d(0.1, 0., 0.)) mbv = MultiBodyViz(mbg, mb, endEffectorDict={'b4': (X_s, 0.1, (0., 1., 0.))}) # test MultiBodyViz from tvtk.tools import ivtk viewer = ivtk.viewer() viewer.size = (640, 480) mbv.addActors(viewer.scene) mbv.display(mb, mbc) # test axis from axis import Axis a1 = Axis(text='test', length=0.2) a1.addActors(viewer.scene) a1.X = sva.PTransformd(sva.RotX(np.pi / 2.), e.Vector3d.UnitX()) # test vector6d from vector6d import ForceVecViz, MotionVecViz M = sva.MotionVecd(e.Vector3d(0.2, 0.1, 0.), e.Vector3d(0.1, 0., 0.2)) F = sva.ForceVecd(e.Vector3d(-0.2, -0.1, 0.), e.Vector3d(-0.1, 0., -0.2)) MV = MotionVecViz(M, a1.X) FV = ForceVecViz( F, sva.PTransformd(sva.RotX(np.pi / 2.), e.Vector3d.UnitX() * 1.4)) MV.addActors(viewer.scene) FV.addActors(viewer.scene)
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))