bindings_aba.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 TestABA(TestCase):
9  def setUp(self):
10  self.model = pin.buildSampleModelHumanoidRandom()
11  self.data = self.model.createData()
12 
13  qmax = np.full((self.model.nq, 1), np.pi)
14  self.q = pin.randomConfiguration(self.model, -qmax, qmax)
15  self.v = np.random.rand(self.model.nv)
16  self.ddq = np.random.rand(self.model.nv)
17 
18  self.fext = []
19  for _ in range(self.model.njoints):
20  self.fext.append(pin.Force.Random())
21 
22  def test_aba(self):
23  model = self.model
24  ddq = pin.aba(self.model, self.data, self.q, self.v, self.ddq)
25 
26  null_fext = pin.StdVec_Force()
27  for _ in range(model.njoints):
28  null_fext.append(pin.Force.Zero())
29 
30  ddq_null_fext = pin.aba(
31  self.model, self.data, self.q, self.v, self.ddq, null_fext
32  )
33  self.assertApprox(ddq_null_fext, ddq)
34 
35  null_fext_list = []
36  for f in null_fext:
37  null_fext_list.append(f)
38 
39  print("size:", len(null_fext_list))
40  ddq_null_fext_list = pin.aba(
41  self.model, self.data, self.q, self.v, self.ddq, null_fext_list
42  )
43  self.assertApprox(ddq_null_fext_list, ddq)
44 
46  model = self.model
47  Minv = pin.computeMinverse(model, self.data, self.q)
48 
49  data2 = model.createData()
50  M = pin.crba(model, data2, self.q)
51 
52  self.assertApprox(np.linalg.inv(M), Minv)
53 
54 
55 if __name__ == "__main__":
56  unittest.main()
bindings_aba.TestABA.setUp
def setUp(self)
Definition: bindings_aba.py:9
bindings_aba.TestABA.test_aba
def test_aba(self)
Definition: bindings_aba.py:22
bindings_aba.TestABA
Definition: bindings_aba.py:8
bindings_aba.TestABA.data
data
Definition: bindings_aba.py:11
bindings_aba.TestABA.fext
fext
Definition: bindings_aba.py:18
bindings_aba.TestABA.ddq
ddq
Definition: bindings_aba.py:16
pinocchio::createData
ConstraintDataTpl< Scalar, Options, ConstraintCollectionTpl > createData(const ConstraintModelTpl< Scalar, Options, ConstraintCollectionTpl > &cmodel)
Definition: constraint-model-visitor.hpp:248
bindings_aba.TestABA.test_computeMinverse
def test_computeMinverse(self)
Definition: bindings_aba.py:45
bindings_aba.TestABA.v
v
Definition: bindings_aba.py:15
boost::fusion::append
result_of::push_front< V const, T >::type append(T const &t, V const &v)
Append the element T at the front of boost fusion vector V.
Definition: fusion.hpp:32
bindings_aba.TestABA.model
model
Definition: bindings_aba.py:10
bindings_aba.TestABA.q
q
Definition: bindings_aba.py:14


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