bindings_joints.py
Go to the documentation of this file.
1 import unittest
2 
3 import numpy as np
4 import pinocchio as pin
5 from test_case import PinocchioTestCase as TestCase
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(model, -np.ones(model.nq), np.ones(model.nq))
53 
54  self.assertTrue(pin.isSameConfiguration(model, self.q, self.q, 1e-8))
55 
56  self.assertFalse(pin.isSameConfiguration(model, q_rand1, q_rand2, 1e-8))
57 
58  def test_derivatives(self):
59  model = self.model
60 
61  q = self.q
62  v = self.v
63 
64  J0, J1 = pin.dIntegrate(model, q, v)
65  res_0 = pin.dIntegrate(model, q, v, pin.ARG0)
66  res_1 = pin.dIntegrate(model, q, v, pin.ARG1)
67 
68  self.assertApprox(J0, res_0)
69  self.assertApprox(J1, res_1)
70 
71  q_next = pin.integrate(model, q, v)
72 
73  J0, J1 = pin.dDifference(model, q, q_next)
74  res_0 = pin.dDifference(model, q, q_next, pin.ARG0)
75  res_1 = pin.dDifference(model, q, q_next, pin.ARG1)
76 
77  self.assertApprox(J0, res_0)
78  self.assertApprox(J1, res_1)
79 
80 
81 if __name__ == "__main__":
82  unittest.main()
bindings_joints.TestJointsAlgo.test_derivatives
def test_derivatives(self)
Definition: bindings_joints.py:58
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 Dec 22 2024 03:41:06