Commit 8e7f06f0 authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

[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: <BR>
/// <CENTER> \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$ </CENTER> <BR>
......@@ -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: <BR>
/// <CENTER> \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$ </CENTER> <BR>
......@@ -163,7 +163,7 @@ namespace pinocchio
const Eigen::MatrixBase<KKTMatrixType> & 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: <BR>
/// <CENTER> \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$ </CENTER> <BR>
......@@ -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: <BR>
/// <CENTER> \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$ </CENTER> <BR>
......
......@@ -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!
Please register or to comment