bindings_centroidal_dynamics_derivatives.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 class TestDeriavtives(TestCase):
8 
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 
19 
20  res = pin.computeCentroidalDynamicsDerivatives(self.model,self.data,self.q,self.v,self.a)
21 
22  self.assertTrue(len(res) == 4)
23 
24  data2 = self.model.createData()
25  pin.computeCentroidalMomentumTimeVariation(self.model,data2,self.q,self.v,self.a)
26 
27  self.assertApprox(self.data.hg,data2.hg)
28  self.assertApprox(self.data.dhg,data2.dhg)
29 
30  data3 = self.model.createData()
31  pin.computeRNEADerivatives(self.model,data3,self.q,self.v,self.a)
32  res2 = pin.getCentroidalDynamicsDerivatives(self.model,data3)
33 
34  for k in range(4):
35  self.assertApprox(res[k],res2[k])
36 
37 if __name__ == '__main__':
38  unittest.main()
JointDataTpl< Scalar, Options, JointCollectionTpl > createData(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through CreateData visitor to create a JointDataTpl.


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:28