Skip to content

GitLab

Commit e1be67c9 authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

[algorithm] Change signature of forwardDynamics

parent c216013f
Hide whitespace changes
Inline Side-by-side
src/algorithm/contact-dynamics.hpp
View file @ e1be67c9
......@@ -10,7 +10,7 @@
namespace pinocchio
{
///
/// \brief Compute the forward dynamics with contact constraints.
/// \note It computes the following problem: <BR>
......@@ -27,6 +27,89 @@ namespace pinocchio
/// \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>
/// 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.
......@@ -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.
......@@ -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"
......
src/algorithm/contact-dynamics.hxx
View file @ e1be67c9
......@@ -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__
unittest/contact-dynamics.cpp
View file @ e1be67c9
......@@ -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);
......
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