22 const ModelTpl<Scalar, Options> &model, DataTpl<Scalar, Options> &data,
23 const Eigen::MatrixBase<ConfigType> &q, Eigen::MatrixBase<VelType>
const &v,
24 const Eigen::MatrixBase<MatrixType> &actMatrix,
29 const Eigen::MatrixBase<OutType> &res_) {
30 namespace pin = pinocchio;
31 using MatrixXs = Eigen::Matrix<Scalar, -1, -1>;
32 assert(constraint_models.size() == constraint_datas.size() &&
33 "constraint_models and constraint_datas do not have the same size");
35 OutType &res = res_.const_cast_derived();
37 long nu = actMatrix.cols();
39 assert(nv == actMatrix.rows() &&
"Actuation matrix dimension inconsistent.");
41 pin::computeAllTerms(model, data, q, v);
42 const auto &nle = data.nle;
45 for (
size_t k = 0; k < constraint_models.size(); ++k) {
46 d += (int)constraint_models[k].size();
49 assert(res.size() == nu + d);
50 MatrixXs work(nv, nu + d);
52 work.leftCols(nu) = actMatrix;
53 auto JacT = work.rightCols(d);
54 pin::getConstraintsJacobian(model, data, constraint_models, constraint_datas,
58 Eigen::ColPivHouseholderQR<MatrixXs> qr(work);
::aligator::context::Scalar Scalar
pinocchio::ModelTpl< Scalar, Options > PinModel
static constexpr int Options
pinocchio::DataTpl< Scalar, Options > PinData
void underactuatedConstrainedInverseDynamics(const ModelTpl< Scalar, Options > &model, DataTpl< Scalar, Options > &data, const Eigen::MatrixBase< ConfigType > &q, Eigen::MatrixBase< VelType > const &v, const Eigen::MatrixBase< MatrixType > &actMatrix, const StdVectorEigenAligned< RigidConstraintModelTpl< Scalar, Options > > &constraint_models, StdVectorEigenAligned< RigidConstraintDataTpl< Scalar, Options > > &constraint_datas, const Eigen::MatrixBase< OutType > &res_)