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