def test_forwardDynamics789(self):
data7 = self.data
data8 = self.model.createData()
data9 = self.model.createData()
ddq7 = pin.forwardDynamics(self.model,data7,self.q,self.v,self.tau,self.J,self.gamma)
ddq8 = pin.forwardDynamics(self.model,data8,self.q,self.v,self.tau,self.J,self.gamma,r_coeff)
ddq9 = pin.forwardDynamics(self.model,data9,self.q,self.v,self.tau,self.J,self.gamma,r_coeff,update_kinematics)
self.assertTrue((ddq7==ddq8).all())
self.assertTrue((ddq7==ddq9).all())
self.assertTrue((ddq8==ddq9).all())
def test_forwardDynamics789(self):
data7 = self.data
data8 = self.model.createData()
data9 = self.model.createData()
ddq7 = pin.forwardDynamics(self.model, data7, self.q, self.v, self.tau,
self.J, self.gamma)
ddq8 = pin.forwardDynamics(self.model, data8, self.q, self.v, self.tau,
self.J, self.gamma, r_coeff)
ddq9 = pin.forwardDynamics(self.model, data9, self.q, self.v, self.tau,
self.J, self.gamma, r_coeff,
update_kinematics)
self.assertTrue((ddq7 == ddq8).all())
self.assertTrue((ddq7 == ddq9).all())
self.assertTrue((ddq8 == ddq9).all())
def test_forwardDynamics_rcoeff(self):
data_no_q = self.model.createData()
self.model.gravity = pin.Motion.Zero()
ddq = pin.forwardDynamics(self.model, self.data, self.q, self.v0,
self.tau0, self.J, self.gamma, r_coeff)
self.assertLess(np.linalg.norm(ddq), self.tolerance)
pin.computeAllTerms(self.model, data_no_q, self.q, self.v0)
ddq_no_q = pin.forwardDynamics(self.model, data_no_q, self.tau0,
self.J, self.gamma, r_coeff)
self.assertLess(np.linalg.norm(ddq_no_q), self.tolerance)
self.assertApprox(ddq, ddq_no_q)
def test_forwardDynamics_q(self):
data7 = self.data
data8 = self.model.createData()
data9_deprecated = self.model.createData()
ddq7 = pin.forwardDynamics(self.model, data7, self.q, self.v, self.tau,
self.J, self.gamma)
ddq8 = pin.forwardDynamics(self.model, data8, self.q, self.v, self.tau,
self.J, self.gamma, r_coeff)
with warnings.catch_warnings(record=True) as warning_list:
ddq9_deprecated = pin.forwardDynamics(self.model, data9_deprecated,
self.q, self.v, self.tau,
self.J, self.gamma, r_coeff,
True)
self.assertTrue(
any(item.category == pin.DeprecatedWarning
for item in warning_list))
self.assertTrue((ddq7 == ddq8).all())
self.assertTrue((ddq7 == ddq9_deprecated).all())
def test_forwardDynamics_no_q(self):
data5 = self.data
data6 = self.model.createData()
data9_deprecated = self.model.createData()
pin.computeAllTerms(self.model, data5, self.q, self.v0)
pin.computeAllTerms(self.model, data6, self.q, self.v0)
pin.computeAllTerms(self.model, data9_deprecated, self.q, self.v0)
ddq5 = pin.forwardDynamics(self.model, data5, self.tau, self.J,
self.gamma)
ddq6 = pin.forwardDynamics(self.model, data6, self.tau, self.J,
self.gamma, r_coeff)
with warnings.catch_warnings(record=True) as warning_list:
ddq9_deprecated = pin.forwardDynamics(self.model, data9_deprecated,
self.q, self.v, self.tau,
self.J, self.gamma, r_coeff,
False)
self.assertTrue(
any(item.category == pin.DeprecatedWarning
for item in warning_list))
self.assertTrue((ddq5 == ddq6).all())
self.assertTrue((ddq5 == ddq9_deprecated).all())
def test_computeKKTMatrix(self):
model = self.model
data = model.createData()
data_ref = model.createData()
q = self.q
v = self.v
tau = self.tau0
J = self.J
gamma = self.gamma
pin.forwardDynamics(model, data_ref, q, v, tau, J, gamma)
KKT_inverse_ref = pin.getKKTContactDynamicMatrixInverse(
model, data_ref, J)
KKT_inverse = pin.computeKKTContactDynamicMatrixInverse(
model, data, q, J)
KKT_inverse2 = pin.computeKKTContactDynamicMatrixInverse(
model, data, q, J, 0.)
self.assertApprox(KKT_inverse, KKT_inverse_ref)
self.assertApprox(KKT_inverse2, KKT_inverse_ref)
def test_forwardDynamics_default(self):
data_no_q = self.model.createData()
self.model.gravity = pin.Motion.Zero()
ddq = pin.forwardDynamics(self.model, self.data, self.q, self.v0,
self.tau0, self.J, self.gamma)
self.assertLess(np.linalg.norm(ddq), self.tolerance)
KKT_inverse = pin.getKKTContactDynamicMatrixInverse(
self.model, self.data, self.J)
M = pin.crba(self.model, self.model.createData(), self.q)
self.assertApprox(
M,
np.linalg.inv(KKT_inverse)[:self.model.nv, :self.model.nv])
pin.computeAllTerms(self.model, data_no_q, self.q, self.v0)
ddq_no_q = pin.forwardDynamics(self.model, data_no_q, self.tau0,
self.J, self.gamma)
self.assertLess(np.linalg.norm(ddq_no_q), self.tolerance)
self.assertApprox(ddq, ddq_no_q)
def test_forwardDynamics9(self):
self.model.gravity = pin.Motion.Zero()
ddq = pin.forwardDynamics(self.model, self.data, self.q, self.v0,
self.tau0, self.J, self.gamma, r_coeff,
update_kinematics)
self.assertLess(np.linalg.norm(ddq), self.tolerance)
def test_forwardDynamics9(self):
self.model.gravity = pin.Motion.Zero()
ddq = pin.forwardDynamics(self.model,self.data,self.q,self.v0,self.tau0,self.J,self.gamma,r_coeff,update_kinematics)
self.assertLess(np.linalg.norm(ddq), self.tolerance)
