Commit 9e3b335f authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

fix python tests

parent 626a16ca
#!/usr/bin/env python
import pinocchio as se3
from pinocchio.utils import isapprox, np, npl, rand, skew, zero
......@@ -7,52 +9,52 @@ R = rand([3, 3])
p = rand(3)
m = se3.SE3(R, p)
X = np.vstack([np.hstack([R, skew(p) * R]), np.hstack([zero([3, 3]), R])])
assert isapprox(m.action(), X)
assert isapprox(m.action, X)
M = np.vstack([np.hstack([R, p]), np.matrix('0 0 0 1', np.double)])
assert isapprox(m.homogeneous(), M)
assert isapprox(m.homogeneous, M)
m2 = se3.SE3.Random()
assert isapprox((m * m2).homogeneous(), m.homogeneous() * m2.homogeneous())
assert isapprox((~m).homogeneous(), npl.inv(m.homogeneous()))
assert isapprox((m * m2).homogeneous, m.homogeneous * m2.homogeneous)
assert isapprox((~m).homogeneous, npl.inv(m.homogeneous))
p = rand(3)
assert isapprox(m * p, m.rotation * p + m.translation)
assert isapprox(m.actInv(p), m.rotation.T * p - m.rotation.T * m.translation)
p = np.vstack([p, 1])
assert isapprox(m * p, m.homogeneous() * p)
assert isapprox(m.actInv(p), npl.inv(m.homogeneous()) * p)
assert isapprox(m * p, m.homogeneous * p)
assert isapprox(m.actInv(p), npl.inv(m.homogeneous) * p)
p = rand(6)
assert isapprox(m * p, m.action() * p)
assert isapprox(m.actInv(p), npl.inv(m.action()) * p)
assert isapprox(m * p, m.action * p)
assert isapprox(m.actInv(p), npl.inv(m.action) * p)
# --- Motion
assert isapprox(se3.Motion.Zero().vector(), zero(6))
assert isapprox(se3.Motion.Zero().vector, zero(6))
v = se3.Motion.Random()
assert isapprox((m * v).vector(), m.action() * v.vector())
assert isapprox((m.actInv(v)).vector(), npl.inv(m.action()) * v.vector())
assert isapprox((m * v).vector, m.action * v.vector)
assert isapprox((m.actInv(v)).vector, npl.inv(m.action) * v.vector)
vv = v.linear
vw = v.angular
assert isapprox(v.vector(), np.vstack([vv, vw]))
assert isapprox((v ** v).vector(), zero(6))
assert isapprox(v.vector, np.vstack([vv, vw]))
assert isapprox((v ** v).vector, zero(6))
# --- Force ---
assert isapprox(se3.Force.Zero().vector(), zero(6))
assert isapprox(se3.Force.Zero().vector, zero(6))
f = se3.Force.Random()
ff = f.linear
ft = f.angular
assert isapprox(f.vector(), np.vstack([ff, ft]))
assert isapprox(f.vector, np.vstack([ff, ft]))
assert isapprox((m * f).vector(), npl.inv(m.action().T) * f.vector())
assert isapprox((m.actInv(f)).vector(), m.action().T * f.vector())
f = se3.Force(np.vstack([v.vector()[3:], v.vector()[:3]]))
assert isapprox((v ** f).vector(), zero(6))
assert isapprox((m * f).vector, npl.inv(m.action.T) * f.vector)
assert isapprox((m.actInv(f)).vector, m.action.T * f.vector)
f = se3.Force(np.vstack([v.vector[3:], v.vector[:3]]))
assert isapprox((v ** f).vector, zero(6))
# --- Inertia ---
Y1 = se3.Inertia.Random()
Y2 = se3.Inertia.Random()
Y = Y1 + Y2
assert isapprox(Y1.matrix() + Y2.matrix(), Y.matrix())
assert isapprox((Y * v).vector(), Y.matrix() * v.vector())
assert isapprox((m * Y).matrix(), m.inverse().action().T * Y.matrix() * m.inverse().action())
assert isapprox((m.actInv(Y)).matrix(), m.action().T * Y.matrix() * m.action())
assert isapprox((Y * v).vector, Y.matrix() * v.vector)
assert isapprox((m * Y).matrix(), m.inverse().action.T * Y.matrix() * m.inverse().action)
assert isapprox((m.actInv(Y)).matrix(), m.action.T * Y.matrix() * m.action)
#!/usr/bin/env python
import pinocchio as se3
from pinocchio.utils import isapprox, np, zero
......@@ -15,7 +17,7 @@ assert model.parents[0] == 0 and model.parents[1] == 0
model.parents[2] = model.parents[1]
assert model.parents[2] == model.parents[1]
assert model.names[0] == "universe"
assert isapprox(model.gravity.np, np.matrix('0; 0; -9.81; 0; 0; 0'))
assert isapprox(model.gravity.np, np.matrix('0 0 -9.81 0 0 0').T)
data = model.createData()
......@@ -29,5 +31,5 @@ for i in range(model.nbody):
se3.rnea(model, data, q, qdot, qddot)
for i in range(model.nbody):
assert isapprox(data.v[i].np, zero(6))
assert isapprox(data.a[0].np, -model.gravity.np)
assert isapprox(data.f[-1], model.inertias[-1] * data.a[-1])
assert isapprox(data.a_gf[0].np, -model.gravity.np)
assert isapprox(data.f[-1], model.inertias[-1] * data.a_gf[-1])
......@@ -34,9 +34,9 @@ def SE3act(m, x):
if x.shape[0] == 3:
return m.rotation * x + m.translation
if x.shape[0] == 4:
return m.homogeneous() * x
return m.homogeneous * x
if x.shape[0] == 6:
return m.action() * x
return m.action * x
raise ValueError('Error: m can only act on linear object of size 3, 4 and 6.')
if 'se3Action' in x.__class__.__dict__:
return x.se3Action(m)
......@@ -53,9 +53,9 @@ def SE3actinv(m, x):
if x.shape[0] == 3:
return m.rotation.T * x - m.rotation.T * m.translation
if x.shape[0] == 4:
return m.inverse().homogeneous() * x
return m.inverse().homogeneous * x
if x.shape[0] == 6:
return m.inverse().action() * x
return m.inverse().action * x
raise ValueError('Error: m can only act on linear object of size 3, 4 and 6.')
if 'se3Action' in x.__class__.__dict__:
return x.se3ActionInverse(m)
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment