bindings.py

Go to the documentation of this file.

import unittest
 
import pinocchio as pin
from pinocchio.utils import np, npl, rand, zero
from test_case import PinocchioTestCase as TestCase
 
# This whole file seems to be outdated and superseded by more recent tests
# Probably it should be removed and its contents moved or split somewhere else
 
 
class TestSE3(TestCase):
    def setUp(self):
        self.R = rand([3, 3])
        self.R, _, _ = npl.svd(self.R)
        self.p = rand(3)
        self.m = pin.SE3(self.R, self.p)
 
    def test_se3(self):
        R, p, m = self.R, self.p, self.m
        X = np.vstack(
            [np.hstack([R, pin.skew(p).dot(R)]), np.hstack([zero([3, 3]), R])]
        )
        self.assertApprox(m.action, X)
        M = np.vstack(
            [np.hstack([R, np.expand_dims(p, 1)]), np.array([[0.0, 0.0, 0.0, 1.0]])]
        )
        self.assertApprox(m.homogeneous, M)
        m2 = pin.SE3.Random()
        self.assertApprox((m * m2).homogeneous, m.homogeneous.dot(m2.homogeneous))
        self.assertApprox((~m).homogeneous, npl.inv(m.homogeneous))
 
        p = rand(3)
        self.assertApprox(m * p, m.rotation.dot(p) + m.translation)
        self.assertApprox(
            m.actInv(p), m.rotation.T.dot(p) - m.rotation.T.dot(m.translation)
        )
 
        # Currently, the different cases do not throw the same exception type.
        # To have a more robust test, only Exception is checked.
        # In the comments, the most specific actual exception class at the time of
        # writing
        p = rand(5)
        with self.assertRaises(Exception):  # RuntimeError
            m * p
        with self.assertRaises(Exception):  # RuntimeError
            m.actInv(p)
        with self.assertRaises(
            Exception
        ):  # Boost.Python.ArgumentError (subclass of TypeError)
            m.actInv("42")
 
    def test_motion(self):
        m = self.m
        self.assertApprox(pin.Motion.Zero().vector, zero(6))
        v = pin.Motion.Random()
        self.assertApprox((m * v).vector, m.action.dot(v.vector))
        self.assertApprox((m.actInv(v)).vector, npl.inv(m.action).dot(v.vector))
        vv = v.linear
        vw = v.angular
        self.assertApprox(v.vector, np.concatenate([vv, vw]))
        self.assertApprox((v ^ v).vector, zero(6))
 
    def test_force(self):
        m = self.m
        self.assertApprox(pin.Force.Zero().vector, zero(6))
        f = pin.Force.Random()
        ff = f.linear
        ft = f.angular
        self.assertApprox(f.vector, np.concatenate([ff, ft]))
 
        self.assertApprox((m * f).vector, npl.inv(m.action.T).dot(f.vector))
        self.assertApprox((m.actInv(f)).vector, m.action.T.dot(f.vector))
        v = pin.Motion.Random()
        f = pin.Force(np.concatenate([v.vector[3:], v.vector[:3]]))
        self.assertApprox((v ^ f).vector, zero(6))
 
    def test_inertia(self):
        m = self.m
        Y1 = pin.Inertia.Random()
        Y2 = pin.Inertia.Random()
        Y = Y1 + Y2
        self.assertApprox(Y1.matrix() + Y2.matrix(), Y.matrix())
        v = pin.Motion.Random()
        self.assertApprox((Y * v).vector, Y.matrix().dot(v.vector))
        self.assertApprox(
            (m * Y).matrix(),
            m.inverse().action.T.dot(Y.matrix()).dot(m.inverse().action),
        )
        self.assertApprox(
            (m.actInv(Y)).matrix(), m.action.T.dot(Y.matrix()).dot(m.action)
        )
 
    def test_cross(self):
        m = pin.Motion.Random()
        f = pin.Force.Random()
        self.assertApprox(m ^ m, m.cross(m))
        self.assertApprox(m ^ f, m.cross(f))
        with self.assertRaises(TypeError):
            m ^ 2
 
 
if __name__ == "__main__":
    unittest.main()