bindings_joint_algorithms.py 2.47 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
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

19
20
21
22
23
        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))

24
25
        q_rand = np.random.rand((model.nq))
        q_rand = pin.normalize(model,q_rand)
26
27
        self.assertTrue(pin.isNormalized(model, q_rand))
        self.assertTrue(pin.isNormalized(model, q_rand, 1e-8))
Justin Carpentier's avatar
Justin Carpentier committed
28

29
30
31
32
33
34
35
36
37
        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)
Justin Carpentier's avatar
Justin Carpentier committed
38
39
40
        q_int = pin.interpolate(model,self.q,q_next,0.5)

        self.assertApprox(q_int,q_int)
41
42
43
44
45
46
47
48

        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)
Justin Carpentier's avatar
Justin Carpentier committed
49
50
        self.assertApprox(q_neutral,q_neutral)

51
52
53
54
55
56
57
        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))

58
59
    def test_derivatives(self):
        model = self.model
60

61
62
63
        q = self.q
        v = self.v

Justin Carpentier's avatar
Justin Carpentier committed
64
        J0, J1 = pin.dIntegrate(model,q,v)
65
66
67
        res_0 = pin.dIntegrate(model,q,v,pin.ARG0)
        res_1 = pin.dIntegrate(model,q,v,pin.ARG1)

Justin Carpentier's avatar
Justin Carpentier committed
68
69
70
        self.assertApprox(J0,res_0)
        self.assertApprox(J1,res_1)

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

Justin Carpentier's avatar
Justin Carpentier committed
73
        J0, J1 = pin.dDifference(model,q,q_next)
74
75
76
        res_0 = pin.dDifference(model,q,q_next,pin.ARG0)
        res_1 = pin.dDifference(model,q,q_next,pin.ARG1)

Justin Carpentier's avatar
Justin Carpentier committed
77
78
79
        self.assertApprox(J0,res_0)
        self.assertApprox(J1,res_1)

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