import unittest
from test_case import PinocchioTestCase as TestCase

import pinocchio as pin
import numpy as np

class TestJointsAlgo(TestCase):

    def setUp(self):
        self.model = pin.buildSampleModelHumanoidRandom()

        qmax = np.full((self.model.nq,1),np.pi)
        self.q = pin.randomConfiguration(self.model,-qmax,qmax)
        self.v = np.random.rand(self.model.nv)

    def test_basic(self):
        model = self.model

        q_ones = np.ones((model.nq))
        self.assertFalse(pin.isNormalized(model, q_ones))
        self.assertFalse(pin.isNormalized(model, q_ones, 1e-8))
        self.assertTrue(pin.isNormalized(model, q_ones, 1e2))

        q_rand = np.random.rand((model.nq))
        q_rand = pin.normalize(model,q_rand)
        self.assertTrue(pin.isNormalized(model, q_rand))
        self.assertTrue(pin.isNormalized(model, q_rand, 1e-8))

        self.assertTrue(abs(np.linalg.norm(q_rand[3:7])-1.) <= 1e-8)

        q_next = pin.integrate(model,self.q,np.zeros((model.nv)))
        self.assertApprox(q_next,self.q)

        v_diff = pin.difference(model,self.q,q_next)
        self.assertApprox(v_diff,np.zeros((model.nv)))

        q_next = pin.integrate(model,self.q,self.v)
        q_int = pin.interpolate(model,self.q,q_next,0.5)

        self.assertApprox(q_int,q_int)

        value = pin.squaredDistance(model,self.q,self.q)
        self.assertTrue((value <= 1e-8).all())

        dist = pin.distance(model,self.q,self.q)
        self.assertTrue(dist <= 1e-8)

        q_neutral = pin.neutral(model)
        self.assertApprox(q_neutral,q_neutral)

        q_rand1 = pin.randomConfiguration(model)
        q_rand2 = pin.randomConfiguration(model,-np.ones((model.nq)),np.ones((model.nq)))

        self.assertTrue(pin.isSameConfiguration(model,self.q,self.q,1e-8))

        self.assertFalse(pin.isSameConfiguration(model,q_rand1,q_rand2,1e-8))

    def test_derivatives(self):
        model = self.model

        q = self.q
        v = self.v

        J0, J1 = pin.dIntegrate(model,q,v)
        res_0 = pin.dIntegrate(model,q,v,pin.ArgumentPosition.ARG0)
        res_1 = pin.dIntegrate(model,q,v,pin.ArgumentPosition.ARG1)

        self.assertApprox(J0,res_0)
        self.assertApprox(J1,res_1)

        q_next = pin.integrate(model,q,v)

        J0, J1 = pin.dDifference(model,q,q_next)
        res_0 = pin.dDifference(model,q,q_next,pin.ARG0)
        res_1 = pin.dDifference(model,q,q_next,pin.ARG1)

        self.assertApprox(J0,res_0)
        self.assertApprox(J1,res_1)

    def test_transport(self):
        model = self.model
        Jq, Jv = pin.dIntegrate(model, self.q, self.v)
        mat = np.random.randn(model.nv, 2)

        mat_transported_q = pin.dIntegrateTransport(model, self.q, self.v, mat, pin.ARG0)
        mat_transported_v = pin.dIntegrateTransport(model, self.q, self.v, mat, pin.ARG1)

        self.assertApprox(mat_transported_q, np.dot(Jq, mat))
        self.assertApprox(mat_transported_v, np.dot(Jv, mat))

if __name__ == '__main__':
    unittest.main()

5c9c603790f429a0a20d97cfd890ab968010b596
<mask token>


class TestJointsAlgo(TestCase):

    def setUp(self):
        self.model = pin.buildSampleModelHumanoidRandom()
        qmax = np.full((self.model.nq, 1), np.pi)
        self.q = pin.randomConfiguration(self.model, -qmax, qmax)
        self.v = np.random.rand(self.model.nv)

    def test_basic(self):
        model = self.model
        q_ones = np.ones(model.nq)
        self.assertFalse(pin.isNormalized(model, q_ones))
        self.assertFalse(pin.isNormalized(model, q_ones, 1e-08))
        self.assertTrue(pin.isNormalized(model, q_ones, 100.0))
        q_rand = np.random.rand(model.nq)
        q_rand = pin.normalize(model, q_rand)
        self.assertTrue(pin.isNormalized(model, q_rand))
        self.assertTrue(pin.isNormalized(model, q_rand, 1e-08))
        self.assertTrue(abs(np.linalg.norm(q_rand[3:7]) - 1.0) <= 1e-08)
        q_next = pin.integrate(model, self.q, np.zeros(model.nv))
        self.assertApprox(q_next, self.q)
        v_diff = pin.difference(model, self.q, q_next)
        self.assertApprox(v_diff, np.zeros(model.nv))
        q_next = pin.integrate(model, self.q, self.v)
        q_int = pin.interpolate(model, self.q, q_next, 0.5)
        self.assertApprox(q_int, q_int)
        value = pin.squaredDistance(model, self.q, self.q)
        self.assertTrue((value <= 1e-08).all())
        dist = pin.distance(model, self.q, self.q)
        self.assertTrue(dist <= 1e-08)
        q_neutral = pin.neutral(model)
        self.assertApprox(q_neutral, q_neutral)
        q_rand1 = pin.randomConfiguration(model)
        q_rand2 = pin.randomConfiguration(model, -np.ones(model.nq), np.
            ones(model.nq))
        self.assertTrue(pin.isSameConfiguration(model, self.q, self.q, 1e-08))
        self.assertFalse(pin.isSameConfiguration(model, q_rand1, q_rand2, 
            1e-08))
    <mask token>

    def test_transport(self):
        model = self.model
        Jq, Jv = pin.dIntegrate(model, self.q, self.v)
        mat = np.random.randn(model.nv, 2)
        mat_transported_q = pin.dIntegrateTransport(model, self.q, self.v,
            mat, pin.ARG0)
        mat_transported_v = pin.dIntegrateTransport(model, self.q, self.v,
            mat, pin.ARG1)
        self.assertApprox(mat_transported_q, np.dot(Jq, mat))
        self.assertApprox(mat_transported_v, np.dot(Jv, mat))


<mask token>

<mask token>


class TestJointsAlgo(TestCase):

    def setUp(self):
        self.model = pin.buildSampleModelHumanoidRandom()
        qmax = np.full((self.model.nq, 1), np.pi)
        self.q = pin.randomConfiguration(self.model, -qmax, qmax)
        self.v = np.random.rand(self.model.nv)

    def test_basic(self):
        model = self.model
        q_ones = np.ones(model.nq)
        self.assertFalse(pin.isNormalized(model, q_ones))
        self.assertFalse(pin.isNormalized(model, q_ones, 1e-08))
        self.assertTrue(pin.isNormalized(model, q_ones, 100.0))
        q_rand = np.random.rand(model.nq)
        q_rand = pin.normalize(model, q_rand)
        self.assertTrue(pin.isNormalized(model, q_rand))
        self.assertTrue(pin.isNormalized(model, q_rand, 1e-08))
        self.assertTrue(abs(np.linalg.norm(q_rand[3:7]) - 1.0) <= 1e-08)
        q_next = pin.integrate(model, self.q, np.zeros(model.nv))
        self.assertApprox(q_next, self.q)
        v_diff = pin.difference(model, self.q, q_next)
        self.assertApprox(v_diff, np.zeros(model.nv))
        q_next = pin.integrate(model, self.q, self.v)
        q_int = pin.interpolate(model, self.q, q_next, 0.5)
        self.assertApprox(q_int, q_int)
        value = pin.squaredDistance(model, self.q, self.q)
        self.assertTrue((value <= 1e-08).all())
        dist = pin.distance(model, self.q, self.q)
        self.assertTrue(dist <= 1e-08)
        q_neutral = pin.neutral(model)
        self.assertApprox(q_neutral, q_neutral)
        q_rand1 = pin.randomConfiguration(model)
        q_rand2 = pin.randomConfiguration(model, -np.ones(model.nq), np.
            ones(model.nq))
        self.assertTrue(pin.isSameConfiguration(model, self.q, self.q, 1e-08))
        self.assertFalse(pin.isSameConfiguration(model, q_rand1, q_rand2, 
            1e-08))

    def test_derivatives(self):
        model = self.model
        q = self.q
        v = self.v
        J0, J1 = pin.dIntegrate(model, q, v)
        res_0 = pin.dIntegrate(model, q, v, pin.ArgumentPosition.ARG0)
        res_1 = pin.dIntegrate(model, q, v, pin.ArgumentPosition.ARG1)
        self.assertApprox(J0, res_0)
        self.assertApprox(J1, res_1)
        q_next = pin.integrate(model, q, v)
        J0, J1 = pin.dDifference(model, q, q_next)
        res_0 = pin.dDifference(model, q, q_next, pin.ARG0)
        res_1 = pin.dDifference(model, q, q_next, pin.ARG1)
        self.assertApprox(J0, res_0)
        self.assertApprox(J1, res_1)

    def test_transport(self):
        model = self.model
        Jq, Jv = pin.dIntegrate(model, self.q, self.v)
        mat = np.random.randn(model.nv, 2)
        mat_transported_q = pin.dIntegrateTransport(model, self.q, self.v,
            mat, pin.ARG0)
        mat_transported_v = pin.dIntegrateTransport(model, self.q, self.v,
            mat, pin.ARG1)
        self.assertApprox(mat_transported_q, np.dot(Jq, mat))
        self.assertApprox(mat_transported_v, np.dot(Jv, mat))


<mask token>

<mask token>


class TestJointsAlgo(TestCase):

    def setUp(self):
        self.model = pin.buildSampleModelHumanoidRandom()
        qmax = np.full((self.model.nq, 1), np.pi)
        self.q = pin.randomConfiguration(self.model, -qmax, qmax)
        self.v = np.random.rand(self.model.nv)

    def test_basic(self):
        model = self.model
        q_ones = np.ones(model.nq)
        self.assertFalse(pin.isNormalized(model, q_ones))
        self.assertFalse(pin.isNormalized(model, q_ones, 1e-08))
        self.assertTrue(pin.isNormalized(model, q_ones, 100.0))
        q_rand = np.random.rand(model.nq)
        q_rand = pin.normalize(model, q_rand)
        self.assertTrue(pin.isNormalized(model, q_rand))
        self.assertTrue(pin.isNormalized(model, q_rand, 1e-08))
        self.assertTrue(abs(np.linalg.norm(q_rand[3:7]) - 1.0) <= 1e-08)
        q_next = pin.integrate(model, self.q, np.zeros(model.nv))
        self.assertApprox(q_next, self.q)
        v_diff = pin.difference(model, self.q, q_next)
        self.assertApprox(v_diff, np.zeros(model.nv))
        q_next = pin.integrate(model, self.q, self.v)
        q_int = pin.interpolate(model, self.q, q_next, 0.5)
        self.assertApprox(q_int, q_int)
        value = pin.squaredDistance(model, self.q, self.q)
        self.assertTrue((value <= 1e-08).all())
        dist = pin.distance(model, self.q, self.q)
        self.assertTrue(dist <= 1e-08)
        q_neutral = pin.neutral(model)
        self.assertApprox(q_neutral, q_neutral)
        q_rand1 = pin.randomConfiguration(model)
        q_rand2 = pin.randomConfiguration(model, -np.ones(model.nq), np.
            ones(model.nq))
        self.assertTrue(pin.isSameConfiguration(model, self.q, self.q, 1e-08))
        self.assertFalse(pin.isSameConfiguration(model, q_rand1, q_rand2, 
            1e-08))

    def test_derivatives(self):
        model = self.model
        q = self.q
        v = self.v
        J0, J1 = pin.dIntegrate(model, q, v)
        res_0 = pin.dIntegrate(model, q, v, pin.ArgumentPosition.ARG0)
        res_1 = pin.dIntegrate(model, q, v, pin.ArgumentPosition.ARG1)
        self.assertApprox(J0, res_0)
        self.assertApprox(J1, res_1)
        q_next = pin.integrate(model, q, v)
        J0, J1 = pin.dDifference(model, q, q_next)
        res_0 = pin.dDifference(model, q, q_next, pin.ARG0)
        res_1 = pin.dDifference(model, q, q_next, pin.ARG1)
        self.assertApprox(J0, res_0)
        self.assertApprox(J1, res_1)

    def test_transport(self):
        model = self.model
        Jq, Jv = pin.dIntegrate(model, self.q, self.v)
        mat = np.random.randn(model.nv, 2)
        mat_transported_q = pin.dIntegrateTransport(model, self.q, self.v,
            mat, pin.ARG0)
        mat_transported_v = pin.dIntegrateTransport(model, self.q, self.v,
            mat, pin.ARG1)
        self.assertApprox(mat_transported_q, np.dot(Jq, mat))
        self.assertApprox(mat_transported_v, np.dot(Jv, mat))


if __name__ == '__main__':
    unittest.main()

import unittest
from test_case import PinocchioTestCase as TestCase
import pinocchio as pin
import numpy as np


class TestJointsAlgo(TestCase):

    def setUp(self):
        self.model = pin.buildSampleModelHumanoidRandom()
        qmax = np.full((self.model.nq, 1), np.pi)
        self.q = pin.randomConfiguration(self.model, -qmax, qmax)
        self.v = np.random.rand(self.model.nv)

    def test_basic(self):
        model = self.model
        q_ones = np.ones(model.nq)
        self.assertFalse(pin.isNormalized(model, q_ones))
        self.assertFalse(pin.isNormalized(model, q_ones, 1e-08))
        self.assertTrue(pin.isNormalized(model, q_ones, 100.0))
        q_rand = np.random.rand(model.nq)
        q_rand = pin.normalize(model, q_rand)
        self.assertTrue(pin.isNormalized(model, q_rand))
        self.assertTrue(pin.isNormalized(model, q_rand, 1e-08))
        self.assertTrue(abs(np.linalg.norm(q_rand[3:7]) - 1.0) <= 1e-08)
        q_next = pin.integrate(model, self.q, np.zeros(model.nv))
        self.assertApprox(q_next, self.q)
        v_diff = pin.difference(model, self.q, q_next)
        self.assertApprox(v_diff, np.zeros(model.nv))
        q_next = pin.integrate(model, self.q, self.v)
        q_int = pin.interpolate(model, self.q, q_next, 0.5)
        self.assertApprox(q_int, q_int)
        value = pin.squaredDistance(model, self.q, self.q)
        self.assertTrue((value <= 1e-08).all())
        dist = pin.distance(model, self.q, self.q)
        self.assertTrue(dist <= 1e-08)
        q_neutral = pin.neutral(model)
        self.assertApprox(q_neutral, q_neutral)
        q_rand1 = pin.randomConfiguration(model)
        q_rand2 = pin.randomConfiguration(model, -np.ones(model.nq), np.
            ones(model.nq))
        self.assertTrue(pin.isSameConfiguration(model, self.q, self.q, 1e-08))
        self.assertFalse(pin.isSameConfiguration(model, q_rand1, q_rand2, 
            1e-08))

    def test_derivatives(self):
        model = self.model
        q = self.q
        v = self.v
        J0, J1 = pin.dIntegrate(model, q, v)
        res_0 = pin.dIntegrate(model, q, v, pin.ArgumentPosition.ARG0)
        res_1 = pin.dIntegrate(model, q, v, pin.ArgumentPosition.ARG1)
        self.assertApprox(J0, res_0)
        self.assertApprox(J1, res_1)
        q_next = pin.integrate(model, q, v)
        J0, J1 = pin.dDifference(model, q, q_next)
        res_0 = pin.dDifference(model, q, q_next, pin.ARG0)
        res_1 = pin.dDifference(model, q, q_next, pin.ARG1)
        self.assertApprox(J0, res_0)
        self.assertApprox(J1, res_1)

    def test_transport(self):
        model = self.model
        Jq, Jv = pin.dIntegrate(model, self.q, self.v)
        mat = np.random.randn(model.nv, 2)
        mat_transported_q = pin.dIntegrateTransport(model, self.q, self.v,
            mat, pin.ARG0)
        mat_transported_v = pin.dIntegrateTransport(model, self.q, self.v,
            mat, pin.ARG1)
        self.assertApprox(mat_transported_q, np.dot(Jq, mat))
        self.assertApprox(mat_transported_v, np.dot(Jv, mat))


if __name__ == '__main__':
    unittest.main()

import unittest
from test_case import PinocchioTestCase as TestCase

import pinocchio as pin
import numpy as np

class TestJointsAlgo(TestCase):

    def setUp(self):
        self.model = pin.buildSampleModelHumanoidRandom()

        qmax = np.full((self.model.nq,1),np.pi)
        self.q = pin.randomConfiguration(self.model,-qmax,qmax)
        self.v = np.random.rand(self.model.nv)

    def test_basic(self):
        model = self.model

        q_ones = np.ones((model.nq))
        self.assertFalse(pin.isNormalized(model, q_ones))
        self.assertFalse(pin.isNormalized(model, q_ones, 1e-8))
        self.assertTrue(pin.isNormalized(model, q_ones, 1e2))

        q_rand = np.random.rand((model.nq))
        q_rand = pin.normalize(model,q_rand)
        self.assertTrue(pin.isNormalized(model, q_rand))
        self.assertTrue(pin.isNormalized(model, q_rand, 1e-8))

        self.assertTrue(abs(np.linalg.norm(q_rand[3:7])-1.) <= 1e-8)

        q_next = pin.integrate(model,self.q,np.zeros((model.nv)))
        self.assertApprox(q_next,self.q)

        v_diff = pin.difference(model,self.q,q_next)
        self.assertApprox(v_diff,np.zeros((model.nv)))

        q_next = pin.integrate(model,self.q,self.v)
        q_int = pin.interpolate(model,self.q,q_next,0.5)

        self.assertApprox(q_int,q_int)

        value = pin.squaredDistance(model,self.q,self.q)
        self.assertTrue((value <= 1e-8).all())

        dist = pin.distance(model,self.q,self.q)
        self.assertTrue(dist <= 1e-8)

        q_neutral = pin.neutral(model)
        self.assertApprox(q_neutral,q_neutral)

        q_rand1 = pin.randomConfiguration(model)
        q_rand2 = pin.randomConfiguration(model,-np.ones((model.nq)),np.ones((model.nq)))

        self.assertTrue(pin.isSameConfiguration(model,self.q,self.q,1e-8))

        self.assertFalse(pin.isSameConfiguration(model,q_rand1,q_rand2,1e-8))

    def test_derivatives(self):
        model = self.model

        q = self.q
        v = self.v

        J0, J1 = pin.dIntegrate(model,q,v)
        res_0 = pin.dIntegrate(model,q,v,pin.ArgumentPosition.ARG0)
        res_1 = pin.dIntegrate(model,q,v,pin.ArgumentPosition.ARG1)

        self.assertApprox(J0,res_0)
        self.assertApprox(J1,res_1)

        q_next = pin.integrate(model,q,v)

        J0, J1 = pin.dDifference(model,q,q_next)
        res_0 = pin.dDifference(model,q,q_next,pin.ARG0)
        res_1 = pin.dDifference(model,q,q_next,pin.ARG1)

        self.assertApprox(J0,res_0)
        self.assertApprox(J1,res_1)

    def test_transport(self):
        model = self.model
        Jq, Jv = pin.dIntegrate(model, self.q, self.v)
        mat = np.random.randn(model.nv, 2)

        mat_transported_q = pin.dIntegrateTransport(model, self.q, self.v, mat, pin.ARG0)
        mat_transported_v = pin.dIntegrateTransport(model, self.q, self.v, mat, pin.ARG1)

        self.assertApprox(mat_transported_q, np.dot(Jq, mat))
        self.assertApprox(mat_transported_v, np.dot(Jv, mat))

if __name__ == '__main__':
    unittest.main()
