bindings_joints.py
Go to the documentation of this file.
1 import unittest
2 from test_case import PinocchioTestCase as TestCase
3 
4 import pinocchio as pin
5 import numpy as np
6 
7 
8 class TestJointsAlgo(TestCase):
9  def setUp(self):
10  self.model = pin.buildSampleModelHumanoidRandom()
11 
12  qmax = np.full((self.model.nq, 1), np.pi)
13  self.q = pin.randomConfiguration(self.model, -qmax, qmax)
14  self.v = np.random.rand(self.model.nv)
15 
16  def test_basic(self):
17  model = self.model
18 
19  q_ones = np.ones((model.nq))
20  self.assertFalse(pin.isNormalized(model, q_ones))
21  self.assertFalse(pin.isNormalized(model, q_ones, 1e-8))
22  self.assertTrue(pin.isNormalized(model, q_ones, 1e2))
23 
24  q_rand = np.random.rand((model.nq))
25  q_rand = pin.normalize(model, q_rand)
26  self.assertTrue(pin.isNormalized(model, q_rand))
27  self.assertTrue(pin.isNormalized(model, q_rand, 1e-8))
28 
29  self.assertTrue(abs(np.linalg.norm(q_rand[3:7]) - 1.0) <= 1e-8)
30 
31  q_next = pin.integrate(model, self.q, np.zeros((model.nv)))
32  self.assertApprox(q_next, self.q)
33 
34  v_diff = pin.difference(model, self.q, q_next)
35  self.assertApprox(v_diff, np.zeros((model.nv)))
36 
37  q_next = pin.integrate(model, self.q, self.v)
38  q_int = pin.interpolate(model, self.q, q_next, 0.5)
39 
40  self.assertApprox(q_int, q_int)
41 
42  value = pin.squaredDistance(model, self.q, self.q)
43  self.assertTrue((value <= 1e-8).all())
44 
45  dist = pin.distance(model, self.q, self.q)
46  self.assertTrue(dist <= 1e-8)
47 
48  q_neutral = pin.neutral(model)
49  self.assertApprox(q_neutral, q_neutral)
50 
51  q_rand1 = pin.randomConfiguration(model)
52  q_rand2 = pin.randomConfiguration(
53  model, -np.ones((model.nq)), np.ones((model.nq))
54  )
55 
56  self.assertTrue(pin.isSameConfiguration(model, self.q, self.q, 1e-8))
57 
58  self.assertFalse(pin.isSameConfiguration(model, q_rand1, q_rand2, 1e-8))
59 
60  def test_derivatives(self):
61  model = self.model
62 
63  q = self.q
64  v = self.v
65 
66  J0, J1 = pin.dIntegrate(model, q, v)
67  res_0 = pin.dIntegrate(model, q, v, pin.ARG0)
68  res_1 = pin.dIntegrate(model, q, v, pin.ARG1)
69 
70  self.assertApprox(J0, res_0)
71  self.assertApprox(J1, res_1)
72 
73  q_next = pin.integrate(model, q, v)
74 
75  J0, J1 = pin.dDifference(model, q, q_next)
76  res_0 = pin.dDifference(model, q, q_next, pin.ARG0)
77  res_1 = pin.dDifference(model, q, q_next, pin.ARG1)
78 
79  self.assertApprox(J0, res_0)
80  self.assertApprox(J1, res_1)
81 
82 
83 if __name__ == "__main__":
84  unittest.main()
bindings_joints.TestJointsAlgo.test_derivatives
def test_derivatives(self)
Definition: bindings_joints.py:60
bindings_joints.TestJointsAlgo
Definition: bindings_joints.py:8
bindings_joints.TestJointsAlgo.setUp
def setUp(self)
Definition: bindings_joints.py:9
bindings_joints.TestJointsAlgo.model
model
Definition: bindings_joints.py:10
bindings_joints.TestJointsAlgo.v
v
Definition: bindings_joints.py:14
bindings_joints.TestJointsAlgo.test_basic
def test_basic(self)
Definition: bindings_joints.py:16
bindings_joints.TestJointsAlgo.q
q
Definition: bindings_joints.py:13


pinocchio
Author(s):
autogenerated on Sun Jun 16 2024 02:43:06