bindings_rnea.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 TestRNEA(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.a = 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_rnea(self):
23  model = self.model
24  tau = pin.rnea(self.model, self.data, self.q, self.v, self.a)
25 
26  null_fext = pin.StdVec_Force()
27  for k in range(model.njoints):
28  null_fext.append(pin.Force.Zero())
29 
30  tau_null_fext = pin.rnea(
31  self.model, self.data, self.q, self.v, self.a, null_fext
32  )
33  self.assertApprox(tau_null_fext, tau)
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  tau_null_fext_list = pin.rnea(
41  self.model, self.data, self.q, self.v, self.a, null_fext_list
42  )
43  self.assertApprox(tau_null_fext_list, tau)
44 
45  def test_nle(self):
46  model = self.model
47 
48  tau = pin.nonLinearEffects(model, self.data, self.q, self.v)
49 
50  data2 = model.createData()
51  tau_ref = pin.rnea(model, data2, self.q, self.v, self.a * 0)
52 
53  self.assertApprox(tau, tau_ref)
54 
56  model = self.model
57 
58  tau = pin.computeGeneralizedGravity(model, self.data, self.q)
59 
60  data2 = model.createData()
61  tau_ref = pin.rnea(model, data2, self.q, self.v * 0, self.a * 0)
62 
63  self.assertApprox(tau, tau_ref)
64 
65  def test_static_torque(self):
66  model = self.model
67 
68  tau = pin.computeStaticTorque(model, self.data, self.q, self.fext)
69 
70  data2 = model.createData()
71  tau_ref = pin.rnea(model, data2, self.q, self.v * 0, self.a * 0, self.fext)
72 
73  self.assertApprox(tau, tau_ref)
74 
76  model = self.model
77 
78  C = pin.computeCoriolisMatrix(model, self.data, self.q, self.v)
79 
80  data2 = model.createData()
81  tau_coriolis_ref = pin.rnea(
82  model, data2, self.q, self.v, self.a * 0
83  ) - pin.rnea(model, data2, self.q, self.v * 0, self.a * 0)
84 
85  self.assertApprox(tau_coriolis_ref, C.dot(self.v))
86 
87 
88 if __name__ == "__main__":
89  unittest.main()
bindings_rnea.TestRNEA.test_static_torque
def test_static_torque(self)
Definition: bindings_rnea.py:65
bindings_rnea.TestRNEA.test_rnea
def test_rnea(self)
Definition: bindings_rnea.py:22
bindings_rnea.TestRNEA.test_coriolis_matrix
def test_coriolis_matrix(self)
Definition: bindings_rnea.py:75
bindings_rnea.TestRNEA.data
data
Definition: bindings_rnea.py:11
bindings_rnea.TestRNEA.model
model
Definition: bindings_rnea.py:10
bindings_rnea.TestRNEA.test_generalized_gravity
def test_generalized_gravity(self)
Definition: bindings_rnea.py:55
bindings_rnea.TestRNEA.q
q
Definition: bindings_rnea.py:14
bindings_rnea.TestRNEA.v
v
Definition: bindings_rnea.py:15
pinocchio::createData
ConstraintDataTpl< Scalar, Options, ConstraintCollectionTpl > createData(const ConstraintModelTpl< Scalar, Options, ConstraintCollectionTpl > &cmodel)
Definition: constraint-model-visitor.hpp:248
bindings_rnea.TestRNEA.fext
fext
Definition: bindings_rnea.py:18
bindings_rnea.TestRNEA
Definition: bindings_rnea.py:8
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_rnea.TestRNEA.a
a
Definition: bindings_rnea.py:16
bindings_rnea.TestRNEA.setUp
def setUp(self)
Definition: bindings_rnea.py:9
bindings_rnea.TestRNEA.test_nle
def test_nle(self)
Definition: bindings_rnea.py:45


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