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)