Commit 506ef5e3 authored by Justin Carpentier's avatar Justin Carpentier

algo/constrained-dynamics: add damping in impactDynamics

parent c10eedde
......@@ -35,7 +35,7 @@ namespace pinocchio
/// \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.
/// \param[in] inv_damping Damping factor for Cholesky decomposition of JMinvJt. Set to zero if constraints are full rank.
/// \param[in] updateKinematics If true, the algorithm calls first pinocchio::computeAllTerms. Otherwise, it uses the current dynamic values stored in data. \\
/// \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.
///
......@@ -72,6 +72,70 @@ namespace pinocchio
const Eigen::MatrixBase<ConstraintMatrixType> & J,
const Eigen::MatrixBase<KKTMatrixType> & MJtJ_inv);
///
/// \brief Compute the impulse dynamics with contact constraints.
/// \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>
/// where \f$ \dot{q}^{-} \f$ is the generalized velocity before impact,
/// \f$ M \f$ is the joint space mass matrix, \f$ J \f$ the constraint Jacobian and \f$ \epsilon \f$ is the coefficient of restitution (1 for a fully elastic impact or 0 for a rigid impact).
///
/// \tparam JointCollection Collection of Joint types.
/// \tparam ConfigVectorType Type of the joint configuration vector.
/// \tparam TangentVectorType Type of the joint velocity vector.
/// \tparam ConstraintMatrixType Type of the constraint matrix.
///
/// \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_before The joint velocity before impact (vector dim model.nv).
/// \param[in] J The Jacobian of the constraints (dim nb_constraints*model.nv).
/// \param[in] inv_damping Damping factor for Cholesky decomposition of JMinvJt. Set to zero if constraints are full rank.
/// \param[in] r_coeff The coefficient of restitution. Must be in [0;1].
///
/// \return A reference to the generalized velocity after impact stored in data.dq_after. The Lagrange Multipliers linked to the contact impulsed are available throw data.impulse_c vector.
///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename ConstraintMatrixType>
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 inv_damping,
const Scalar r_coeff);
///
/// \brief Compute the impulse dynamics with contact constraints.
/// \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>
/// where \f$ \dot{q}^{-} \f$ is the generalized velocity before impact,
/// \f$ M \f$ is the joint space mass matrix, \f$ J \f$ the constraint Jacobian and \f$ \epsilon \f$ is the coefficient of restitution (1 for a fully elastic impact or 0 for a rigid impact).
///
/// \tparam JointCollection Collection of Joint types.
/// \tparam ConfigVectorType Type of the joint configuration vector.
/// \tparam TangentVectorType Type of the joint velocity vector.
/// \tparam ConstraintMatrixType Type of the constraint matrix.
///
/// \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_before The joint velocity before impact (vector dim model.nv).
/// \param[in] J The Jacobian of the constraints (dim nb_constraints*model.nv).
/// \param[in] inv_damping Damping factor for Cholesky decomposition of JMinvJt. Set to zero if constraints are full rank.
/// \param[in] r_coeff The coefficient of restitution. Must be in [0;1].
///
/// \return A reference to the generalized velocity after impact stored in data.dq_after. The Lagrange Multipliers linked to the contact impulsed are available throw data.impulse_c vector.
///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename ConstraintMatrixType>
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType &
impulseDynamics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<TangentVectorType> & v_before,
const Eigen::MatrixBase<ConstraintMatrixType> & J,
const Scalar inv_damping,
const Scalar r_coeff);
///
/// \brief Compute the impulse dynamics with contact constraints.
/// \note It computes the following problem: <BR>
......@@ -96,6 +160,7 @@ namespace pinocchio
/// \return A reference to the generalized velocity after impact stored in data.dq_after. The Lagrange Multipliers linked to the contact impulsed are available throw data.impulse_c vector.
///
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,
......
......@@ -109,10 +109,26 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v_before,
const Eigen::MatrixBase<ConstraintMatrixType> & J,
const Scalar r_coeff,
const bool updateKinematics)
const Scalar inv_damping,
const Scalar r_coeff)
{
assert(q.size() == model.nq);
// Compute the mass matrix
crba(model, data, q);
return impulseDynamics(model,data,v_before,J,inv_damping,r_coeff);
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename TangentVectorType, typename ConstraintMatrixType>
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType &
impulseDynamics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<TangentVectorType> & v_before,
const Eigen::MatrixBase<ConstraintMatrixType> & J,
const Scalar inv_damping,
const Scalar r_coeff)
{
assert(v_before.size() == model.nv);
assert(J.cols() == model.nv);
assert(model.check(data) && "data is not consistent with model.");
......@@ -122,10 +138,6 @@ namespace pinocchio
typename Data::VectorXs & impulse_c = data.impulse_c;
typename Data::TangentVectorType & dq_after = data.dq_after;
// Compute the mass matrix
if (updateKinematics)
crba(model, data, q);
// Compute the UDUt decomposition of data.M
cholesky::decompose(model, data);
......@@ -135,6 +147,8 @@ namespace pinocchio
for(int k=0;k<model.nv;++k) data.sDUiJt.row(k) /= sqrt(data.D[k]);
data.JMinvJt.noalias() = data.sDUiJt.transpose() * data.sDUiJt;
data.JMinvJt.diagonal().array() += inv_damping;
data.llt_JMinvJt.compute(data.JMinvJt);
// Compute the Lagrange Multipliers related to the contact impulses
......@@ -148,6 +162,23 @@ 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,Scalar(0),r_coeff);
else
return impulseDynamics(model,data,v_before,J,Scalar(0),r_coeff);
}
} // namespace pinocchio
#endif // ifndef __pinocchio_contact_dynamics_hxx__
......@@ -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, r_coeff, true);
pinocchio::impulseDynamics(model, data, q, v_before, J, 0., r_coeff);
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, r_coeff, true);
pinocchio::impulseDynamics(model, data, q, v_before, J, 0., r_coeff);
data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
MatrixXd Minv (data.M.inverse());
......
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