5#ifndef __pinocchio_algorithm_contact_inverse_dynamics__hpp__
6#define __pinocchio_algorithm_contact_inverse_dynamics__hpp__
8#include "pinocchio/multibody/model.hpp"
9#include "pinocchio/multibody/data.hpp"
10#include "pinocchio/algorithm/constraints/coulomb-friction-cone.hpp"
11#include "pinocchio/algorithm/rnea.hpp"
12#include <boost/optional/optional.hpp>
13#include <pinocchio/algorithm/contact-cholesky.hpp>
14#include <pinocchio/algorithm/contact-jacobian.hpp>
15#include "pinocchio/algorithm/proximal.hpp"
17#include <boost/optional.hpp>
46 template<
typename,
int>
class JointCollectionTpl,
48 class ConstraintModelAllocator,
49 class ConstraintDataAllocator,
50 class CoulombFrictionConelAllocator,
52 typename VectorLikeGamma,
53 typename VectorLikeImp>
54 PINOCCHIO_UNSUPPORTED_MESSAGE(
"The API will change towards more flexibility")
55 const typename DataTpl<Scalar, Options, JointCollectionTpl>::
58 DataTpl<Scalar, Options, JointCollectionTpl> & data,
70 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> MatrixXs;
71 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> VectorXs;
72 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
74 int problem_size = R.size();
79 PINOCCHIO_CHECK_INPUT_ARGUMENT(
81 MatrixXs J = MatrixXs::Zero(problem_size, model.nv);
89 PINOCCHIO_CHECK_ARGUMENT_SIZE(data.impulse_c.size(), problem_size);
93 data.impulse_c.setZero();
98 const size_t nc =
cones.size();
146 return data.impulse_c;
178 template<
typename,
int>
class JointCollectionTpl,
179 typename ConfigVectorType,
180 typename TangentVectorType1,
181 typename TangentVectorType2,
182 class ConstraintModelAllocator,
183 class ConstraintDataAllocator,
184 class CoulombFrictionConelAllocator,
185 typename VectorLikeR,
186 typename VectorLikeGamma,
187 typename VectorLikeLam>
188 PINOCCHIO_UNSUPPORTED_MESSAGE(
"The API will change towards more flexibility")
189 const typename DataTpl<Scalar, Options, JointCollectionTpl>::
192 DataTpl<Scalar, Options, JointCollectionTpl> & data,
207 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> MatrixXs;
208 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> VectorXs;
212 int problem_size = R.size();
214 MatrixXs J = MatrixXs::Zero(problem_size, model.nv);
223 data.impulse_c *=
dt;
229 data.lambda_c = data.impulse_c /
dt;
230 container::aligned_vector<Force>
fext(model.njoints);
231 for (
int i = 0;
i < model.njoints;
i++)
233 fext[
i] = Force::Zero();
238 const Eigen::DenseIndex
row_id = 3 *
i;
241 cmodel.joint1_placement.toActionMatrixInverse();
245 cmodel.joint2_placement.toActionMatrixInverse();
Main pinocchio namespace.
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & contactInverseDynamics(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 > &a, Scalar dt, const std::vector< RigidConstraintModelTpl< Scalar, Options >, ConstraintModelAllocator > &contact_models, std::vector< RigidConstraintDataTpl< Scalar, Options >, ConstraintDataAllocator > &contact_datas, const std::vector< CoulombFrictionConeTpl< Scalar >, CoulombFrictionConelAllocator > &cones, const Eigen::MatrixBase< VectorLikeR > &R, const Eigen::MatrixBase< VectorLikeGamma > &constraint_correction, ProximalSettingsTpl< Scalar > &settings, const boost::optional< VectorLikeLam > &lambda_guess=boost::none)
The Contact Inverse Dynamics algorithm. It computes the inverse dynamics in the presence of contacts,...
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & rnea(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 > &a)
The Recursive Newton-Euler algorithm. It computes the inverse dynamics, aka the joint torques accordi...
void getConstraintsJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const std::vector< RigidConstraintModelTpl< Scalar, Options >, ConstraintDataAllocator > &constraint_model, std::vector< RigidConstraintDataTpl< Scalar, Options >, ConstraintDataAllocator > &constraint_data, const Eigen::MatrixBase< DynamicMatrixLike > &J)
Computes the kinematic Jacobian associatied to a given set of constraint models.
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & computeContactImpulses(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< VectorLikeC > &c_ref, const std::vector< RigidConstraintModelTpl< Scalar, Options >, ConstraintModelAllocator > &contact_models, std::vector< RigidConstraintDataTpl< Scalar, Options >, ConstraintDataAllocator > &contact_datas, const std::vector< CoulombFrictionConeTpl< Scalar >, CoulombFrictionConelAllocator > &cones, const Eigen::MatrixBase< VectorLikeR > &R, const Eigen::MatrixBase< VectorLikeGamma > &constraint_correction, ProximalSettingsTpl< Scalar > &settings, const boost::optional< VectorLikeImp > &impulse_guess=boost::none)
Compute the contact impulses given a target velocity of contact points.
Structure containing all the settings parameters for the proximal algorithms.