bindings_forward_dynamics_derivatives.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 TestDeriavtives(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.tau = 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 
23  res = pin.computeABADerivatives(self.model, self.data, self.q, self.v, self.tau)
24 
25  self.assertTrue(len(res) == 3)
26 
27  data2 = self.model.createData()
28  pin.aba(self.model, data2, self.q, self.v, self.tau)
29 
30  self.assertApprox(self.data.ddq, data2.ddq)
31 
32  # With external forces
33  res = pin.computeABADerivatives(
34  self.model, self.data, self.q, self.v, self.tau, self.fext
35  )
36 
37  self.assertTrue(len(res) == 3)
38 
39  pin.aba(self.model, data2, self.q, self.v, self.tau, self.fext)
40 
41  self.assertApprox(self.data.ddq, data2.ddq)
42 
43 
44 if __name__ == "__main__":
45  unittest.main()
bindings_forward_dynamics_derivatives.TestDeriavtives.model
model
Definition: bindings_forward_dynamics_derivatives.py:10
bindings_forward_dynamics_derivatives.TestDeriavtives.data
data
Definition: bindings_forward_dynamics_derivatives.py:11
bindings_forward_dynamics_derivatives.TestDeriavtives.test_aba_derivatives
def test_aba_derivatives(self)
Definition: bindings_forward_dynamics_derivatives.py:22
bindings_forward_dynamics_derivatives.TestDeriavtives
Definition: bindings_forward_dynamics_derivatives.py:8
bindings_forward_dynamics_derivatives.TestDeriavtives.setUp
def setUp(self)
Definition: bindings_forward_dynamics_derivatives.py:9
pinocchio::createData
ConstraintDataTpl< Scalar, Options, ConstraintCollectionTpl > createData(const ConstraintModelTpl< Scalar, Options, ConstraintCollectionTpl > &cmodel)
Definition: constraint-model-visitor.hpp:239
bindings_forward_dynamics_derivatives.TestDeriavtives.q
q
Definition: bindings_forward_dynamics_derivatives.py:14
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_forward_dynamics_derivatives.TestDeriavtives.tau
tau
Definition: bindings_forward_dynamics_derivatives.py:16
bindings_forward_dynamics_derivatives.TestDeriavtives.v
v
Definition: bindings_forward_dynamics_derivatives.py:15
bindings_forward_dynamics_derivatives.TestDeriavtives.fext
fext
Definition: bindings_forward_dynamics_derivatives.py:18


pinocchio
Author(s):
autogenerated on Sun Dec 22 2024 03:41:06