Commit 9e3b335f by Guilhem Saurel

### fix python tests

parent 626a16ca
python/bindings.py 100644 → 100755
 #!/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)
python/test_model.py 100644 → 100755
 #!/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!