aligator  0.16.0
A versatile and efficient C++ library for real-time constrained trajectory optimization.
Loading...
Searching...
No Matches
constrained-rnea.hpp
Go to the documentation of this file.
1#pragma once
2
4
5#include <pinocchio/algorithm/compute-all-terms.hpp>
6#include <pinocchio/algorithm/contact-jacobian.hpp>
7
8#include <Eigen/LU>
9
10namespace aligator {
11
12namespace {
13using pinocchio::DataTpl;
14using pinocchio::ModelTpl;
15using pinocchio::RigidConstraintDataTpl;
16using pinocchio::RigidConstraintModelTpl;
17} // namespace
18
19template <typename Scalar, typename ConfigType, typename VelType,
20 typename MatrixType, typename OutType, int Options>
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,
25 const StdVectorEigenAligned<RigidConstraintModelTpl<Scalar, Options>>
26 &constraint_models,
27 StdVectorEigenAligned<RigidConstraintDataTpl<Scalar, Options>>
28 &constraint_datas,
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");
34
35 OutType &res = res_.const_cast_derived();
36
37 long nu = actMatrix.cols();
38 long nv = model.nv;
39 assert(nv == actMatrix.rows() && "Actuation matrix dimension inconsistent.");
40
41 pin::computeAllTerms(model, data, q, v);
42 const auto &nle = data.nle;
43
44 int d = 0;
45 for (size_t k = 0; k < constraint_models.size(); ++k) {
46 d += (int)constraint_models[k].size();
47 }
48
49 assert(res.size() == nu + d);
50 MatrixXs work(nv, nu + d);
51
52 work.leftCols(nu) = actMatrix;
53 auto JacT = work.rightCols(d);
54 pin::getConstraintsJacobian(model, data, constraint_models, constraint_datas,
55 JacT.transpose());
56 JacT = -JacT;
57
58 Eigen::ColPivHouseholderQR<MatrixXs> qr(work);
59
60 res = qr.solve(nle);
61}
62
63#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
65 context::Scalar, context::ConstVectorRef, context::ConstVectorRef,
66 context::ConstMatrixRef, context::VectorRef, context::Options>(
68 const Eigen::MatrixBase<context::ConstVectorRef> &,
69 const Eigen::MatrixBase<context::ConstVectorRef> &,
70 const Eigen::MatrixBase<context::ConstMatrixRef> &,
73 const Eigen::MatrixBase<context::VectorRef> &);
74#endif
75} // namespace aligator
::aligator::context::Scalar Scalar
Definition context.hpp:14
pinocchio::ModelTpl< Scalar, Options > PinModel
Definition fwd.hpp:21
static constexpr int Options
Definition context.hpp:10
pinocchio::DataTpl< Scalar, Options > PinData
Definition fwd.hpp:22
Main package namespace.
std::vector< T, Eigen::aligned_allocator< T > > StdVectorEigenAligned
Typedef for vector with Eigen::aligned_allocator allocator type.
Definition fwd.hpp:173
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_)