Unverified Commit 411842d0 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #902 from gabrielebndn/topic/fixcontact

Fix Python impulseDynamics and modify dignature of forwardDynamics
parents 0ac0943d 8e7f06f0
......@@ -17,35 +17,50 @@ namespace pinocchio
const Eigen::VectorXd & tau,
const Eigen::MatrixXd & J,
const Eigen::VectorXd & gamma,
const double inv_damping = 0.0,
const bool updateKinematics = true)
const double inv_damping = 0.0)
{
return forwardDynamics(model, data, q, v, tau, J, gamma, inv_damping, updateKinematics);
return forwardDynamics(model, data, q, v, tau, J, gamma, inv_damping);
}
BOOST_PYTHON_FUNCTION_OVERLOADS(forwardDynamics_overloads, forwardDynamics_proxy, 7, 9)
BOOST_PYTHON_FUNCTION_OVERLOADS(forwardDynamics_overloads, forwardDynamics_proxy, 7, 8)
static const Eigen::VectorXd forwardDynamics_proxy_no_q(const Model & model,
Data & data,
const Eigen::VectorXd & tau,
const Eigen::MatrixXd & J,
const Eigen::VectorXd & gamma,
const double inv_damping = 0.0)
{
return forwardDynamics(model, data, tau, J, gamma, inv_damping);
}
BOOST_PYTHON_FUNCTION_OVERLOADS(forwardDynamics_overloads_no_q, forwardDynamics_proxy_no_q, 5, 6)
static const Eigen::VectorXd impulseDynamics_proxy(const Model & model,
Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v_before,
const Eigen::MatrixXd & J,
const double r_coeff,
const double inv_damping)
const double r_coeff = 0.,
const double inv_damping = 0.)
{
return impulseDynamics(model, data, q, v_before, J, r_coeff, inv_damping);
}
BOOST_PYTHON_FUNCTION_OVERLOADS(impulseDynamics_overloads, impulseDynamics_proxy, 5, 7)
static const Eigen::VectorXd impulseDynamics_proxy_no_q(const Model & model,
Data & data,
const Eigen::VectorXd & v_before,
const Eigen::MatrixXd & J,
const double r_coeff,
const double inv_damping)
const double r_coeff = 0.,
const double inv_damping = 0.)
{
return impulseDynamics(model, data, v_before, J, r_coeff, inv_damping);
}
BOOST_PYTHON_FUNCTION_OVERLOADS(impulseDynamics_overloads_no_q, impulseDynamics_proxy_no_q, 4, 6)
static const Eigen::MatrixXd getKKTContactDynamicMatrixInverse_proxy(const Model & model,
Data & data,
const Eigen::MatrixXd & J)
......@@ -69,13 +84,26 @@ namespace pinocchio
"Joint torque tau (size Model::nv)",
"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.",
"Update kinematics (if true, it updates the dynamic variable according to the current state)"),
"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"
"(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."
" Internally, pinocchio.computeAllTerms is called."
));
bp::def("forwardDynamics",
&forwardDynamics_proxy_no_q,
forwardDynamics_overloads_no_q(
bp::args("Model","Data",
"Joint torque tau (size Model::nv)",
"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."
" Assumes pinocchio.computeAllTerms has been called."
));
bp::def("impulseDynamics",
&impulseDynamics_proxy,
impulseDynamics_overloads(
bp::args("Model","Data",
"Joint configuration q (size Model::nq)",
"Joint velocity before impact v_before (size Model::nv)",
......@@ -83,18 +111,21 @@ 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",
&impulseDynamics_proxy_no_q,
impulseDynamics_overloads_no_q(
bp::args("Model","Data",
"Joint velocity before impact v_before (size Model::nv)",
"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,
bp::args("Model","Data",
......
......@@ -205,39 +205,54 @@ def getJacobian(model,data,jointId,local):
# This function is only deprecated when using a specific signature. Therefore, it needs special care
# Marked as deprecated on 16 Sept 2019
def impulseDynamics(model, data, q = None, *args):
v_before = args[0]
J = args[1]
if q is None:
r_coeff = args[2]
inv_damping = args[3]
return pin.impulseDynamics(model,data,v_before,J,r_coeff,inv_damping)
elif len(args)==4:
if type(args[3]) is bool:
message = ("This function signature has been deprecated and will be removed in future releases of Pinocchio. "
"Please change for the new signature of impulseDynamics(model,data,[q],v_before,J,r_coeff,inv_damping).")
_warnings.warn(message, category=DeprecatedWarning, stacklevel=2)
inv_damping = 0.
r_coeff = args[2]
if(args[3]):
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 = 0.
def impulseDynamics(model, data, *args):
if len(args)==5 and type(args[4]) is bool:
message = ("This function signature has been deprecated and will be removed in future releases of Pinocchio. "
"Please change for the new signature of impulseDynamics(model,data[,q],v_before,J[,r_coeff[,inv_damping]]).")
_warnings.warn(message, category=DeprecatedWarning, stacklevel=2)
q = args[0]
v_before = args[1]
J = args[2]
r_coeff = args[3]
updateKinematics = args[4]
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)
if updateKinematics:
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)
return pin.impulseDynamics(model, data, *args)
impulseDynamics.__doc__ = (
pin.impulseDynamics.__doc__
+ '\n\nimpulseDynamics( (Model)Model, (Data)Data, (object)Joint configuration q (size Model::nq), (object)Joint velocity before impact v_before (size Model::nv), (object)Contact Jacobian J (size nb_constraint * Model::nv), (float)Coefficient of restitution r_coeff (0 = rigid impact; 1 = fully elastic impact), (bool)updateKinematics) -> object :'
+ '\n This function signature has been deprecated and will be removed in future releases of Pinocchio.'
)
# This function is only deprecated when using a specific signature. Therefore, it needs special care
# Marked as deprecated on 2 Oct 2019
def forwardDynamics(model, data, *args):
if len(args)==7 and type(args[6]) is bool:
message = ("This function signature has been deprecated and will be removed in future releases of Pinocchio. "
"Please change for the new signature of forwardDynamics(model,data[,q],v,tau,J,gamma[,inv_damping]).")
_warnings.warn(message, category=DeprecatedWarning, stacklevel=2)
q = args[0]
v = args[1]
tau = args[2]
J = args[3]
gamma = args[4]
inv_damping = args[5]
updateKinematics = args[6]
if updateKinematics:
return pin.forwardDynamics(model,data,q,v,tau,J,gamma,inv_damping)
else:
return pin.forwardDynamics(model,data,tau,J,gamma,inv_damping)
return pin.forwardDynamics(model, data, *args)
forwardDynamics.__doc__ = (
pin.forwardDynamics.__doc__
+ '\n\nforwardDynamics( (Model)Model, (Data)Data, (object)Joint configuration q (size Model::nq), (object)Joint velocity v (size Model::nv), (object)Joint torque tau (size Model::nv), (object)Contact Jacobian J (size nb_constraint * Model::nv), (object)Contact drift gamma (size nb_constraint), (float)(double) Damping factor for cholesky decomposition of JMinvJt. Set to zero if constraints are full rank, (bool)Update kinematics) -> object :'
+ '\n This function signature has been deprecated and will be removed in future releases of Pinocchio.'
)
......@@ -10,9 +10,92 @@
namespace pinocchio
{
///
/// \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>
/// where \f$ \ddot{q}_{\text{free}} \f$ is the free acceleration (i.e. without constraints),
/// \f$ M \f$ is the mass matrix, \f$ J \f$ the constraint Jacobian and \f$ \gamma \f$ is the constraint drift.
/// By default, the constraint Jacobian is assumed to be full rank, and undamped Cholesky inverse is performed.
///
/// \tparam JointCollection Collection of Joint types.
/// \tparam ConfigVectorType Type of the joint configuration vector.
/// \tparam TangentVectorType1 Type of the joint velocity vector.
/// \tparam TangentVectorType2 Type of the joint torque vector.
/// \tparam ConstraintMatrixType Type of the constraint matrix.
/// \tparam DriftVectorType Type of the drift vector.
///
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
/// \param[in] q The joint configuration (vector dim model.nq).
/// \param[in] v The joint velocity (vector dim model.nv).
/// \param[in] tau The joint torque vector (dim model.nv).
/// \param[in] J The Jacobian of the constraints (dim nb_constraints*model.nv).
/// \param[in] gamma The drift of the constraints (dim nb_constraints).
/// \param[in] inv_damping Damping factor for Cholesky decomposition of JMinvJt. Set to zero if constraints are full rank.
/// \note A hint: 1e-12 as the damping factor gave good result in the particular case of redundancy in contact constraints on the two feet.
///
/// \return A reference to the joint acceleration stored in data.ddq. The Lagrange Multipliers linked to the contact forces are available throw data.lambda_c vector.
///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2,
typename ConstraintMatrixType, typename DriftVectorType>
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType &
forwardDynamics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType1> & v,
const Eigen::MatrixBase<TangentVectorType2> & tau,
const Eigen::MatrixBase<ConstraintMatrixType> & J,
const Eigen::MatrixBase<DriftVectorType> & gamma,
const Scalar inv_damping = 0.);
///
/// \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>
/// where \f$ \ddot{q}_{\text{free}} \f$ is the free acceleration (i.e. without constraints),
/// \f$ M \f$ is the mass matrix, \f$ J \f$ the constraint Jacobian and \f$ \gamma \f$ is the constraint drift.
/// By default, the constraint Jacobian is assumed to be full rank, and undamped Cholesky inverse is performed.
///
/// \tparam JointCollection Collection of Joint types.
/// \tparam ConfigVectorType Type of the joint configuration vector.
/// \tparam TangentVectorType1 Type of the joint velocity vector.
/// \tparam TangentVectorType2 Type of the joint torque vector.
/// \tparam ConstraintMatrixType Type of the constraint matrix.
/// \tparam DriftVectorType Type of the drift vector.
///
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
/// \param[in] v The joint velocity (vector dim model.nv).
/// \param[in] tau The joint torque vector (dim model.nv).
/// \param[in] J The Jacobian of the constraints (dim nb_constraints*model.nv).
/// \param[in] gamma The drift of the constraints (dim nb_constraints).
/// \param[in] inv_damping Damping factor for Cholesky decomposition of JMinvJt. Set to zero if constraints are full rank.
/// \note A hint: 1e-12 as the damping factor gave good result in the particular case of redundancy in contact constraints on the two feet.
///
/// \return A reference to the joint acceleration stored in data.ddq. The Lagrange Multipliers linked to the contact forces are available throw data.lambda_c vector.
///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename TangentVectorType,
typename ConstraintMatrixType, typename DriftVectorType>
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType &
forwardDynamics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<TangentVectorType> & tau,
const Eigen::MatrixBase<ConstraintMatrixType> & J,
const Eigen::MatrixBase<DriftVectorType> & gamma,
const Scalar inv_damping = 0.);
///
/// \brief Compute the forward dynamics with contact constraints.
///
/// \deprecated This function signature has been deprecated and will be removed in future releases of Pinocchio.
/// Please change for the new signature of forwardDynamics(model,data[,q],v,tau,J,gamma[,inv_damping]).
///
/// \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>
......@@ -43,6 +126,7 @@ namespace pinocchio
///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2,
typename ConstraintMatrixType, typename DriftVectorType>
PINOCCHIO_DEPRECATED
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType &
forwardDynamics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
......@@ -51,16 +135,22 @@ namespace pinocchio
const Eigen::MatrixBase<TangentVectorType2> & tau,
const Eigen::MatrixBase<ConstraintMatrixType> & J,
const Eigen::MatrixBase<DriftVectorType> & gamma,
const Scalar inv_damping = 0.,
const bool updateKinematics = true);
const Scalar inv_damping,
const bool updateKinematics)
{
if(updateKinematics)
return forwardDynamics(model,data,q,v,tau,J,gamma,inv_damping);
else
return forwardDynamics(model,data,tau,J,gamma,inv_damping);
}
///
/// \brief Computes the inverse of the KKT matrix for dynamics with contact constraints, [[M JT], [J 0]].
/// The matrix is defined when we call forwardDynamics/impulsedynamics. This method makes use of
/// the matrix decompositions performed during the forwardDynamics/impulasedynamics and returns the inverse.
/// The jacobian should be the same that was provided to forwardDynamics/impulasedynamics.
/// Thus you should call forward Dynamics/impulsedynamics first.
/// The matrix is defined when we call forwardDynamics/impulseDynamics. This method makes use of
/// the matrix decompositions performed during the forwardDynamics/impulseDynamics and returns the inverse.
/// The jacobian should be the same that was provided to forwardDynamics/impulseDynamics.
/// Thus you should call forward Dynamics/impulseDynamics first.
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
/// \param[out] MJtJ_inv inverse of the MJtJ matrix.
......@@ -73,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>
......@@ -106,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>
......@@ -138,6 +228,10 @@ namespace pinocchio
///
/// \brief Compute the impulse dynamics with contact constraints.
///
/// \deprecated This function signature has been deprecated and will be removed in future releases of Pinocchio.
/// Please change for the new signature of impulseDynamics(model,data[,q],v_before,J[,r_coeff[,inv_damping]]).
///
/// \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>
......@@ -168,8 +262,14 @@ namespace pinocchio
const Eigen::MatrixBase<TangentVectorType> & v_before,
const Eigen::MatrixBase<ConstraintMatrixType> & J,
const Scalar r_coeff,
const bool updateKinematics);
const bool updateKinematics)
{
if(updateKinematics)
return impulseDynamics(model,data,q,v_before,J,r_coeff,Scalar(0));
else
return impulseDynamics(model,data,v_before,J,r_coeff,Scalar(0));
}
} // namespace pinocchio
#include "pinocchio/algorithm/contact-dynamics.hxx"
......
......@@ -15,21 +15,16 @@
namespace pinocchio
{
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2,
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename TangentVectorType,
typename ConstraintMatrixType, typename DriftVectorType>
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType &
forwardDynamics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType1> & v,
const Eigen::MatrixBase<TangentVectorType2> & tau,
const Eigen::MatrixBase<TangentVectorType> & tau,
const Eigen::MatrixBase<ConstraintMatrixType> & J,
const Eigen::MatrixBase<DriftVectorType> & gamma,
const Scalar inv_damping,
const bool updateKinematics)
const Scalar inv_damping)
{
assert(q.size() == model.nq);
assert(v.size() == model.nv);
assert(tau.size() == model.nv);
assert(J.cols() == model.nv);
assert(J.rows() == gamma.size());
......@@ -40,9 +35,6 @@ namespace pinocchio
typename Data::TangentVectorType & a = data.ddq;
typename Data::VectorXs & lambda_c = data.lambda_c;
if (updateKinematics)
computeAllTerms(model, data, q, v);
// Compute the UDUt decomposition of data.M
cholesky::decompose(model, data);
......@@ -73,7 +65,27 @@ namespace pinocchio
return a;
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2,
typename ConstraintMatrixType, typename DriftVectorType>
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType &
forwardDynamics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType1> & v,
const Eigen::MatrixBase<TangentVectorType2> & tau,
const Eigen::MatrixBase<ConstraintMatrixType> & J,
const Eigen::MatrixBase<DriftVectorType> & gamma,
const Scalar inv_damping)
{
assert(q.size() == model.nq);
assert(v.size() == model.nv);
computeAllTerms(model, data, q, v);
return forwardDynamics(model,data,tau,J,gamma,inv_damping);
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl,
typename ConstraintMatrixType, typename KKTMatrixType>
inline void getKKTContactDynamicMatrixInverse(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
......@@ -162,23 +174,6 @@ namespace pinocchio
return dq_after;
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename ConstraintMatrixType>
PINOCCHIO_DEPRECATED
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType &
impulseDynamics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v_before,
const Eigen::MatrixBase<ConstraintMatrixType> & J,
const Scalar r_coeff,
const bool updateKinematics)
{
if(updateKinematics)
return impulseDynamics(model,data,q,v_before,J,r_coeff,Scalar(0));
else
return impulseDynamics(model,data,v_before,J,r_coeff,Scalar(0));
}
} // namespace pinocchio
#endif // ifndef __pinocchio_contact_dynamics_hxx__
......@@ -54,7 +54,7 @@ BOOST_AUTO_TEST_CASE ( test_FD )
Eigen::MatrixXd H(J.transpose());
pinocchio::forwardDynamics(model, data, q, v, tau, J, gamma, 0.,true);
pinocchio::forwardDynamics(model, data, q, v, tau, J, gamma, 0.);
data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
MatrixXd Minv (data.M.inverse());
......@@ -118,7 +118,7 @@ BOOST_AUTO_TEST_CASE (test_KKTMatrix)
Eigen::MatrixXd H(J.transpose());
//Check Forward Dynamics
pinocchio::forwardDynamics(model, data, q, v, tau, J, gamma, 0.,true);
pinocchio::forwardDynamics(model, data, q, v, tau, J, gamma, 0.);
data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
Eigen::MatrixXd MJtJ(model.nv+12, model.nv+12);
......@@ -175,7 +175,7 @@ BOOST_AUTO_TEST_CASE ( test_FD_with_damping )
Eigen::VectorXd gamma (VectorXd::Ones(12));
// Forward Dynamics with damping
pinocchio::forwardDynamics(model, data, q, v, tau, J, gamma, 1e-12,true);
pinocchio::forwardDynamics(model, data, q, v, tau, J, gamma, 1e-12);
// Matrix Definitions
Eigen::MatrixXd H(J.transpose());
......@@ -312,7 +312,7 @@ BOOST_AUTO_TEST_CASE (timings_fd_llt)
PinocchioTicToc timer(PinocchioTicToc::US); timer.tic();
SMOOTH(NBT)
{
pinocchio::forwardDynamics(model, data, q, v, tau, J, gamma, 0., true);
pinocchio::forwardDynamics(model, data, q, v, tau, J, gamma, 0.);
}
timer.toc(std::cout,NBT);
......
......@@ -5,11 +5,12 @@ pin.switchToNumpyMatrix()
from pinocchio.utils import rand, zero
import numpy as np
import warnings
# common quantities for all tests.
# 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):
......@@ -31,57 +32,130 @@ 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)
self.assertApprox(ddq,ddq_no_q)
def test_forwardDynamics789(self):
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()
def test_impulseDynamics5(self):
vnext = pin.impulseDynamics(self.model,self.data,self.q,self.v0,self.J)