bindings_dynamics.py
Go to the documentation of this file.
1 import unittest
2 from test_case import PinocchioTestCase as TestCase
3 import pinocchio as pin
4 from pinocchio.utils import rand, zero
5 import numpy as np
6 
7 import warnings
8 
9 # common quantities for all tests.
10 # They correspond to the default values of the arguments, and they need to stay this way
11 r_coeff = 0.0
12 inv_damping = 0.0
13 
14 
15 class TestDynamicsBindings(TestCase):
16 
17  def setUp(self):
18  self.model = pin.buildSampleModelHumanoidRandom()
19  self.data = self.model.createData()
20 
21  qmax = np.full((self.model.nq,1),np.pi)
22  self.q = pin.randomConfiguration(self.model,-qmax,qmax)
23  self.v = rand(self.model.nv)
24  self.tau = rand(self.model.nv)
25 
26  self.v0 = zero(self.model.nv)
27  self.tau0 = zero(self.model.nv)
28  self.tolerance = 1e-9
29 
30  # we compute J on a different self.data
31  self.J = pin.computeJointJacobian(self.model,self.model.createData(),self.q,self.model.getJointId('lleg6_joint'))
32  self.gamma = zero(6)
33 
35  data_no_q = self.model.createData()
36 
37  self.model.gravity = pin.Motion.Zero()
38  ddq = pin.forwardDynamics(self.model,self.data,self.q,self.v0,self.tau0,self.J,self.gamma)
39  self.assertLess(np.linalg.norm(ddq), self.tolerance)
40 
41  KKT_inverse = pin.getKKTContactDynamicMatrixInverse(self.model,self.data,self.J)
42  M = pin.crba(self.model,self.model.createData(),self.q)
43 
44  self.assertApprox(M,np.linalg.inv(KKT_inverse)[:self.model.nv,:self.model.nv])
45 
46  pin.computeAllTerms(self.model,data_no_q,self.q,self.v0)
47  ddq_no_q = pin.forwardDynamics(self.model,data_no_q,self.tau0,self.J,self.gamma)
48  self.assertLess(np.linalg.norm(ddq_no_q), self.tolerance)
49 
50  self.assertApprox(ddq,ddq_no_q)
51 
53  model = self.model
54  data = model.createData()
55  data_ref = model.createData()
56 
57  q = self.q
58  v = self.v
59  tau = self.tau0
60  J = self.J
61  gamma = self.gamma
62 
63  pin.forwardDynamics(model,data_ref,q,v,tau,J,gamma)
64  KKT_inverse_ref = pin.getKKTContactDynamicMatrixInverse(model,data_ref,J)
65 
66  KKT_inverse = pin.computeKKTContactDynamicMatrixInverse(model,data,q,J)
67  KKT_inverse2 = pin.computeKKTContactDynamicMatrixInverse(model,data,q,J,0.)
68  self.assertApprox(KKT_inverse,KKT_inverse_ref)
69  self.assertApprox(KKT_inverse2,KKT_inverse_ref)
70 
72  data_no_q = self.model.createData()
73 
74  self.model.gravity = pin.Motion.Zero()
75  ddq = pin.forwardDynamics(self.model,self.data,self.q,self.v0,self.tau0,self.J,self.gamma,r_coeff)
76  self.assertLess(np.linalg.norm(ddq), self.tolerance)
77 
78  pin.computeAllTerms(self.model,data_no_q,self.q,self.v0)
79  ddq_no_q = pin.forwardDynamics(self.model,data_no_q,self.tau0,self.J,self.gamma,r_coeff)
80  self.assertLess(np.linalg.norm(ddq_no_q), self.tolerance)
81 
82  self.assertApprox(ddq,ddq_no_q)
83 
85  data7 = self.data
86  data8 = self.model.createData()
87  data9_deprecated = self.model.createData()
88  ddq7 = pin.forwardDynamics(self.model,data7,self.q,self.v,self.tau,self.J,self.gamma)
89  ddq8 = pin.forwardDynamics(self.model,data8,self.q,self.v,self.tau,self.J,self.gamma,r_coeff)
90  with warnings.catch_warnings(record=True) as warning_list:
91  ddq9_deprecated = pin.forwardDynamics(self.model,data9_deprecated,self.q,self.v,self.tau,self.J,self.gamma,r_coeff,True)
92  self.assertTrue(any(item.category == pin.DeprecatedWarning for item in warning_list))
93  self.assertTrue((ddq7==ddq8).all())
94  self.assertTrue((ddq7==ddq9_deprecated).all())
95 
97  data5 = self.data
98  data6 = self.model.createData()
99  data9_deprecated = self.model.createData()
100  pin.computeAllTerms(self.model,data5,self.q,self.v0)
101  pin.computeAllTerms(self.model,data6,self.q,self.v0)
102  pin.computeAllTerms(self.model,data9_deprecated,self.q,self.v0)
103  ddq5 = pin.forwardDynamics(self.model,data5,self.tau,self.J,self.gamma)
104  ddq6 = pin.forwardDynamics(self.model,data6,self.tau,self.J,self.gamma,r_coeff)
105  with warnings.catch_warnings(record=True) as warning_list:
106  ddq9_deprecated = pin.forwardDynamics(self.model,data9_deprecated,self.q,self.v,self.tau,self.J,self.gamma,r_coeff,False)
107  self.assertTrue(any(item.category == pin.DeprecatedWarning for item in warning_list))
108  self.assertTrue((ddq5==ddq6).all())
109  self.assertTrue((ddq5==ddq9_deprecated).all())
110 
112  data_no_q = self.model.createData()
113 
114  vnext = pin.impulseDynamics(self.model,self.data,self.q,self.v0,self.J)
115  self.assertLess(np.linalg.norm(vnext), self.tolerance)
116 
117  pin.crba(self.model,data_no_q,self.q)
118  vnext_no_q = pin.impulseDynamics(self.model,data_no_q,self.v0,self.J)
119  self.assertLess(np.linalg.norm(vnext_no_q), self.tolerance)
120 
121  self.assertApprox(vnext, vnext_no_q)
122 
124  data_no_q = self.model.createData()
125 
126  vnext = pin.impulseDynamics(self.model,self.data,self.q,self.v0,self.J,r_coeff)
127  self.assertLess(np.linalg.norm(vnext), self.tolerance)
128 
129  pin.crba(self.model,data_no_q,self.q)
130  vnext_no_q = pin.impulseDynamics(self.model,data_no_q,self.v0,self.J,r_coeff)
131  self.assertLess(np.linalg.norm(vnext_no_q), self.tolerance)
132 
133  self.assertApprox(vnext, vnext_no_q)
134 
136  data_no_q = self.model.createData()
137 
138  vnext = pin.impulseDynamics(self.model,self.data,self.q,self.v0,self.J,r_coeff,inv_damping)
139  self.assertLess(np.linalg.norm(vnext), self.tolerance)
140 
141  pin.crba(self.model,data_no_q,self.q)
142  vnext_no_q = pin.impulseDynamics(self.model,data_no_q,self.v0,self.J,r_coeff,inv_damping)
143  self.assertLess(np.linalg.norm(vnext_no_q), self.tolerance)
144 
145  self.assertApprox(vnext, vnext_no_q)
146 
148  data5 = self.data
149  data6 = self.model.createData()
150  data7 = self.model.createData()
151  data7_deprecated = self.model.createData()
152  vnext5 = pin.impulseDynamics(self.model,data5,self.q,self.v,self.J)
153  vnext6 = pin.impulseDynamics(self.model,data6,self.q,self.v,self.J,r_coeff)
154  vnext7 = pin.impulseDynamics(self.model,data7,self.q,self.v,self.J,r_coeff,inv_damping)
155  with warnings.catch_warnings(record=True) as warning_list:
156  vnext7_deprecated = pin.impulseDynamics(self.model,data7_deprecated,self.q,self.v,self.J,r_coeff,True)
157  self.assertTrue(any(item.category == pin.DeprecatedWarning for item in warning_list))
158  self.assertTrue((vnext5==vnext6).all())
159  self.assertTrue((vnext5==vnext7).all())
160  self.assertTrue((vnext6==vnext7).all())
161  self.assertTrue((vnext7_deprecated==vnext7).all())
162 
164  data4 = self.data
165  data5 = self.model.createData()
166  data6 = self.model.createData()
167  data7_deprecated = self.model.createData()
168  pin.crba(self.model,data4,self.q)
169  pin.crba(self.model,data5,self.q)
170  pin.crba(self.model,data6,self.q)
171  pin.crba(self.model,data7_deprecated,self.q)
172  vnext4 = pin.impulseDynamics(self.model,data4,self.v,self.J)
173  vnext5 = pin.impulseDynamics(self.model,data5,self.v,self.J,r_coeff)
174  vnext6 = pin.impulseDynamics(self.model,data6,self.v,self.J,r_coeff,inv_damping)
175  with warnings.catch_warnings(record=True) as warning_list:
176  vnext7_deprecated = pin.impulseDynamics(self.model,data7_deprecated,self.q,self.v,self.J,r_coeff,False)
177  self.assertTrue(any(item.category == pin.DeprecatedWarning for item in warning_list))
178  self.assertTrue((vnext4==vnext5).all())
179  self.assertTrue((vnext4==vnext6).all())
180  self.assertTrue((vnext5==vnext6).all())
181  self.assertTrue((vnext7_deprecated==vnext6).all())
182 
183 if __name__ == '__main__':
184  unittest.main()


pinocchio
Author(s):
autogenerated on Tue Jun 1 2021 02:45:02