Commit 8e7f06f0 by Gabriele Buondonno

### [unittest] Test new bindings for forwardDynamics

parent cccbb211
 ... ... @@ -85,7 +85,8 @@ namespace pinocchio "Contact Jacobian J (size nb_constraint * Model::nv)", "Contact drift gamma (size nb_constraint)", "(double) Damping factor for cholesky decomposition of JMinvJt. Set to zero if constraints are full rank."), "Solves the forward dynamics problem with contacts, puts the result in Data::ddq and return it. The contact forces are stored in data.lambda_c" "Solves the forward dynamics problem with contacts, puts the result in Data::ddq and return it. The contact forces are stored in data.lambda_c." " Internally, pinocchio.computeAllTerms is called." )); bp::def("forwardDynamics", ... ... @@ -96,7 +97,8 @@ namespace pinocchio "Contact Jacobian J (size nb_constraint * Model::nv)", "Contact drift gamma (size nb_constraint)", "(double) Damping factor for cholesky decomposition of JMinvJt. Set to zero if constraints are full rank."), "Solves the forward dynamics problem with contacts, puts the result in Data::ddq and return it. The contact forces are stored in data.lambda_c" "Solves the forward dynamics problem with contacts, puts the result in Data::ddq and return it. The contact forces are stored in data.lambda_c." " Assumes pinocchio.computeAllTerms has been called." )); bp::def("impulseDynamics", ... ... @@ -109,7 +111,8 @@ namespace pinocchio "Coefficient of restitution r_coeff (0 = rigid impact; 1 = fully elastic impact)", "Damping factor when J is rank deficient." ), "Solves the impact dynamics problem with contacts, put the result in Data::dq_after and return it. The contact impulses are stored in data.impulse_c" "Solves the impact dynamics problem with contacts, put the result in Data::dq_after and return it. The contact impulses are stored in data.impulse_c." " Internally, pinocchio.crba is called." )); bp::def("impulseDynamics", ... ... @@ -120,7 +123,8 @@ namespace pinocchio "Contact Jacobian J (size nb_constraint * Model::nv)", "Coefficient of restitution r_coeff (0 = rigid impact; 1 = fully elastic impact)", "Damping factor when J is rank deficient."), "Solves the impact dynamics problem with contacts, put the result in Data::dq_after and return it. The contact impulses are stored in data.impulse_c" "Solves the impact dynamics problem with contacts, put the result in Data::dq_after and return it. The contact impulses are stored in data.impulse_c." " Assumes pinocchio.crba has been called." )); bp::def("getKKTContactDynamicMatrixInverse",getKKTContactDynamicMatrixInverse_proxy, ... ...
 ... ... @@ -12,7 +12,7 @@ namespace pinocchio { /// /// \brief Compute the forward dynamics with contact constraints. /// \brief Compute the forward dynamics with contact constraints. Internally, pinocchio::computeAllTerms is called. /// \note It computes the following problem:
///
\f$\begin{eqnarray} \underset{\ddot{q}}{\min} & & \| \ddot{q} - \ddot{q}_{\text{free}} \|_{M(q)} \\ /// \text{s.t.} & & J (q) \ddot{q} + \gamma (q, \dot{q}) = 0 \end{eqnarray} \f$

... ... @@ -53,7 +53,7 @@ namespace pinocchio const Scalar inv_damping = 0.); /// /// \brief Compute the forward dynamics with contact constraints, assuming pinocchio::computeAllTerms has been called /// \brief Compute the forward dynamics with contact constraints, assuming pinocchio::computeAllTerms has been called. /// \note It computes the following problem:
///
\f$\begin{eqnarray} \underset{\ddot{q}}{\min} & & \| \ddot{q} - \ddot{q}_{\text{free}} \|_{M(q)} \\ /// \text{s.t.} & & J (q) \ddot{q} + \gamma (q, \dot{q}) = 0 \end{eqnarray} \f$

... ... @@ -163,7 +163,7 @@ namespace pinocchio const Eigen::MatrixBase & MJtJ_inv); /// /// \brief Compute the impulse dynamics with contact constraints. /// \brief Compute the impulse dynamics with contact constraints. Internally, pinocchio::crba is called. /// \note It computes the following problem:
///
\f$\begin{eqnarray} \underset{\dot{q}^{+}}{\min} & & \| \dot{q}^{+} - \dot{q}^{-} \|_{M(q)} \\ /// \text{s.t.} & & J (q) \dot{q}^{+} = - \epsilon J (q) \dot{q}^{-} \end{eqnarray} \f$

... ... @@ -196,7 +196,7 @@ namespace pinocchio const Scalar inv_damping = 0.); /// /// \brief Compute the impulse dynamics with contact constraints. /// \brief Compute the impulse dynamics with contact constraints, assuming pinocchio::crba has been called. /// \note It computes the following problem:
///
\f$\begin{eqnarray} \underset{\dot{q}^{+}}{\min} & & \| \dot{q}^{+} - \dot{q}^{-} \|_{M(q)} \\ /// \text{s.t.} & & J (q) \dot{q}^{+} = - \epsilon J (q) \dot{q}^{-} \end{eqnarray} \f$

... ...
 ... ... @@ -11,7 +11,6 @@ import warnings # They correspond to the default values of the arguments, and they need to stay this way r_coeff = 0.0 inv_damping = 0.0 update_kinematics = True class TestDynamicsBindings(TestCase): ... ... @@ -33,31 +32,58 @@ class TestDynamicsBindings(TestCase): self.J = pin.jointJacobian(self.model,self.model.createData(),self.q,self.model.getJointId('lleg6_joint')) self.gamma = zero(6) def test_forwardDynamics7(self): 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) def test_forwardDynamics8(self): 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_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) 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) 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) def test_forwardDynamics789(self): self.assertApprox(ddq,ddq_no_q) def test_forwardDynamics_q(self): data7 = self.data data8 = self.model.createData() data9 = 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) ddq9 = pin.forwardDynamics(self.model,data9,self.q,self.v,self.tau,self.J,self.gamma,r_coeff,update_kinematics) 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).all()) self.assertTrue((ddq8==ddq9).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_impulseDynamics_default(self): data_no_q = self.model.createData() ... ...
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!