Commit 45510bf3 authored by Justin Carpentier's avatar Justin Carpentier

algo/contact-dyn: fix issue with signature and tests

parent 3034ebf9
......@@ -224,9 +224,16 @@ def impulseDynamics(model, data, q = None, *args):
return pin.impulseDynamics(model,data,q,v_before,J,r_coeff,inv_damping)
else:
return pin.impulseDynamics(model,data,v_before,J,r_coeff,inv_damping)
else:
r_coeff = args[2]
inv_damping = args[3]
return pin.impulseDynamics(model,data,q,v_before,J,r_coeff,inv_damping)
else:
r_coeff = args[2]
inv_damping = args[3]
r_coeff = 0.
inv_damping = 0.
if args:
if len(args) >= 3:
r_coeff = args[2]
return pin.impulseDynamics(model,data,q,v_before,J,r_coeff,inv_damping)
impulseDynamics.__doc__ = (
......
......@@ -133,7 +133,7 @@ BOOST_AUTO_TEST_CASE (test_KKTMatrix)
//Check Impulse Dynamics
const double r_coeff = 1.;
VectorXd v_before = VectorXd::Ones(model.nv);
pinocchio::impulseDynamics(model, data, q, v_before, J, 0., r_coeff);
pinocchio::impulseDynamics(model, data, q, v_before, J, r_coeff, 0.);
data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
MJtJ << data.M, J.transpose(),
J, Eigen::MatrixXd::Zero(12, 12);
......@@ -234,7 +234,7 @@ BOOST_AUTO_TEST_CASE ( test_ID )
Eigen::MatrixXd H(J.transpose());
pinocchio::impulseDynamics(model, data, q, v_before, J, 0., r_coeff);
pinocchio::impulseDynamics(model, data, q, v_before, J, r_coeff, 0.);
data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
MatrixXd Minv (data.M.inverse());
......
......@@ -61,23 +61,26 @@ class TestDynamicsBindings(TestCase):
self.assertLess(np.linalg.norm(vnext), self.tolerance)
def test_impulseDynamics6(self):
vnext = pin.impulseDynamics(self.model,self.data,self.q,self.v0,self.J,inv_damping)
vnext = pin.impulseDynamics(self.model,self.data,self.q,self.v0,self.J,r_coeff)
self.assertLess(np.linalg.norm(vnext), self.tolerance)
def test_impulseDynamics7(self):
vnext = pin.impulseDynamics(self.model,self.data,self.q,self.v0,self.J,inv_damping,update_kinematics)
vnext = pin.impulseDynamics(self.model,self.data,self.q,self.v0,self.J,r_coeff,inv_damping)
self.assertLess(np.linalg.norm(vnext), self.tolerance)
def test_impulseDynamics567(self):
data5 = self.data
data6 = self.model.createData()
data7 = self.model.createData()
data8 = self.model.createData()
vnext5 = pin.impulseDynamics(self.model,data5,self.q,self.v,self.J)
vnext6 = pin.impulseDynamics(self.model,data6,self.q,self.v,self.J,inv_damping)
vnext7 = pin.impulseDynamics(self.model,data7,self.q,self.v,self.J,inv_damping,update_kinematics)
vnext6 = pin.impulseDynamics(self.model,data6,self.q,self.v,self.J,r_coeff)
vnext7 = pin.impulseDynamics(self.model,data7,self.q,self.v,self.J,r_coeff,inv_damping)
vnext7_previous = pin.impulseDynamics(self.model,data8,self.q,self.v,self.J,r_coeff,True)
self.assertTrue((vnext5==vnext6).all())
self.assertTrue((vnext5==vnext7).all())
self.assertTrue((vnext6==vnext7).all())
self.assertTrue((vnext7_previous==vnext7).all())
if __name__ == '__main__':
unittest.main()
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment