2 from test_case
import PinocchioTestCase
as TestCase
3 import pinocchio
as pin
17 self.
model = pin.buildSampleModelHumanoidRandom()
20 qmax = np.full((self.
model.nq, 1), np.pi)
21 self.
q = pin.randomConfiguration(self.
model, -qmax, qmax)
30 self.
J = pin.computeJointJacobian(
34 self.
model.getJointId(
"lleg6_joint"),
41 self.
model.gravity = pin.Motion.Zero()
42 ddq = pin.forwardDynamics(
45 self.assertLess(np.linalg.norm(ddq), self.
tolerance)
47 KKT_inverse = pin.getKKTContactDynamicMatrixInverse(
53 M, np.linalg.inv(KKT_inverse)[: self.
model.nv, : self.
model.nv]
56 pin.computeAllTerms(self.
model, data_no_q, self.
q, self.
v0)
57 ddq_no_q = pin.forwardDynamics(
60 self.assertLess(np.linalg.norm(ddq_no_q), self.
tolerance)
62 self.assertApprox(ddq, ddq_no_q)
66 data = model.createData()
67 data_ref = model.createData()
75 pin.forwardDynamics(model, data_ref, q, v, tau, J, gamma)
76 KKT_inverse_ref = pin.getKKTContactDynamicMatrixInverse(model, data_ref, J)
78 KKT_inverse = pin.computeKKTContactDynamicMatrixInverse(model, data, q, J)
79 KKT_inverse2 = pin.computeKKTContactDynamicMatrixInverse(model, data, q, J, 0.0)
80 self.assertApprox(KKT_inverse, KKT_inverse_ref)
81 self.assertApprox(KKT_inverse2, KKT_inverse_ref)
86 self.
model.gravity = pin.Motion.Zero()
87 ddq = pin.forwardDynamics(
97 self.assertLess(np.linalg.norm(ddq), self.
tolerance)
99 pin.computeAllTerms(self.
model, data_no_q, self.
q, self.
v0)
100 ddq_no_q = pin.forwardDynamics(
103 self.assertLess(np.linalg.norm(ddq_no_q), self.
tolerance)
105 self.assertApprox(ddq, ddq_no_q)
111 ddq7 = pin.forwardDynamics(
114 ddq8 = pin.forwardDynamics(
118 self.assertTrue((ddq7 == ddq8).all())
124 pin.computeAllTerms(self.
model, data5, self.
q, self.
v0)
125 pin.computeAllTerms(self.
model, data6, self.
q, self.
v0)
126 pin.computeAllTerms(self.
model, data9_deprecated, self.
q, self.
v0)
127 ddq5 = pin.forwardDynamics(self.
model, data5, self.
tau, self.
J, self.
gamma)
128 ddq6 = pin.forwardDynamics(
131 self.assertTrue((ddq5 == ddq6).all())
136 vnext = pin.impulseDynamics(self.
model, self.
data, self.
q, self.
v0, self.
J)
137 self.assertLess(np.linalg.norm(vnext), self.
tolerance)
139 pin.crba(self.
model, data_no_q, self.
q)
140 vnext_no_q = pin.impulseDynamics(self.
model, data_no_q, self.
v0, self.
J)
141 self.assertLess(np.linalg.norm(vnext_no_q), self.
tolerance)
143 self.assertApprox(vnext, vnext_no_q)
148 vnext = pin.impulseDynamics(
151 self.assertLess(np.linalg.norm(vnext), self.
tolerance)
153 pin.crba(self.
model, data_no_q, self.
q)
154 vnext_no_q = pin.impulseDynamics(
155 self.
model, data_no_q, self.
v0, self.
J, r_coeff
157 self.assertLess(np.linalg.norm(vnext_no_q), self.
tolerance)
159 self.assertApprox(vnext, vnext_no_q)
164 vnext = pin.impulseDynamics(
165 self.
model, self.
data, self.
q, self.
v0, self.
J, r_coeff, inv_damping
167 self.assertLess(np.linalg.norm(vnext), self.
tolerance)
169 pin.crba(self.
model, data_no_q, self.
q)
170 vnext_no_q = pin.impulseDynamics(
171 self.
model, data_no_q, self.
v0, self.
J, r_coeff, inv_damping
173 self.assertLess(np.linalg.norm(vnext_no_q), self.
tolerance)
175 self.assertApprox(vnext, vnext_no_q)
182 vnext5 = pin.impulseDynamics(self.
model, data5, self.
q, self.
v, self.
J)
183 vnext6 = pin.impulseDynamics(self.
model, data6, self.
q, self.
v, self.
J, r_coeff)
184 vnext7 = pin.impulseDynamics(
185 self.
model, data7, self.
q, self.
v, self.
J, r_coeff, inv_damping
187 self.assertTrue((vnext5 == vnext6).all())
188 self.assertTrue((vnext5 == vnext7).all())
189 self.assertTrue((vnext6 == vnext7).all())
196 pin.crba(self.
model, data4, self.
q)
197 pin.crba(self.
model, data5, self.
q)
198 pin.crba(self.
model, data6, self.
q)
199 pin.crba(self.
model, data7_deprecated, self.
q)
200 vnext4 = pin.impulseDynamics(self.
model, data4, self.
v, self.
J)
201 vnext5 = pin.impulseDynamics(self.
model, data5, self.
v, self.
J, r_coeff)
202 vnext6 = pin.impulseDynamics(
203 self.
model, data6, self.
v, self.
J, r_coeff, inv_damping
206 self.assertTrue((vnext4 == vnext5).all())
207 self.assertTrue((vnext4 == vnext6).all())
208 self.assertTrue((vnext5 == vnext6).all())
211 if __name__ ==
"__main__":