aligator  0.15.0
A versatile and efficient C++ library for real-time constrained trajectory optimization.
Loading...
Searching...
No Matches
constraint-set-product.hpp
Go to the documentation of this file.
1
3#pragma once
4
7
8#include <boost/core/span.hpp>
9
10namespace aligator {
11template <typename Derived>
12auto blockMatrixGetRow(const Eigen::MatrixBase<Derived> &matrix,
13 boost::span<const Eigen::Index> rowBlockSizes,
14 std::size_t rowIdx) {
15 Eigen::Index startIdx = 0;
16 for (std::size_t kk = 0; kk < rowIdx; kk++) {
17 startIdx += rowBlockSizes[kk];
18 }
19 return matrix.const_cast_derived().middleRows(startIdx,
20 rowBlockSizes[rowIdx]);
21}
22
23template <typename Derived>
24auto blockVectorGetRow(const Eigen::MatrixBase<Derived> &matrix,
25 boost::span<const Eigen::Index> blockSizes,
26 std::size_t blockIdx) {
27 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
28 Eigen::Index startIdx = 0;
29 for (std::size_t kk = 0; kk < blockIdx; kk++) {
30 startIdx += blockSizes[kk];
31 }
32 return matrix.const_cast_derived().segment(startIdx, blockSizes[blockIdx]);
33}
34
40template <typename Scalar>
44 using ActiveType = typename Base::ActiveType;
45
46 ConstraintSetProductTpl(const std::vector<xyz::polymorphic<Base>> components,
47 const std::vector<Eigen::Index> &blockSizes)
48 : m_components(components)
49 , m_blockSizes(blockSizes) {
50 if (components.size() != blockSizes.size()) {
51 ALIGATOR_RUNTIME_ERROR("Number of components and corresponding "
52 "block sizes should be the same.");
53 }
54 }
55
60
61 Scalar evaluate(const ConstVectorRef &zproj) const override {
62 Scalar res = 0.;
63 for (std::size_t i = 0; i < m_components.size(); i++) {
64 auto zblock = blockVectorGetRow(zproj, m_blockSizes, i);
65 res += m_components[i]->evaluate(zblock);
66 }
67 return res;
68 }
69
70 void projection(const ConstVectorRef &z, VectorRef zout) const override {
71 for (std::size_t i = 0; i < m_components.size(); i++) {
72 ConstVectorRef inblock = blockVectorGetRow(z, m_blockSizes, i);
73 VectorRef outblock = blockVectorGetRow(zout, m_blockSizes, i);
74 m_components[i]->projection(inblock, outblock);
75 }
76 }
77
78 void normalConeProjection(const ConstVectorRef &z,
79 VectorRef zout) const override {
80 for (std::size_t i = 0; i < m_components.size(); i++) {
81 ConstVectorRef inblock = blockVectorGetRow(z, m_blockSizes, i);
82 VectorRef outblock = blockVectorGetRow(zout, m_blockSizes, i);
83 m_components[i]->normalConeProjection(inblock, outblock);
84 }
85 }
86
87 void applyProjectionJacobian(const ConstVectorRef &z,
88 MatrixRef Jout) const override {
89 for (std::size_t i = 0; i < m_components.size(); i++) {
90 ConstVectorRef inblock = blockVectorGetRow(z, m_blockSizes, i);
91 MatrixRef outblock = blockMatrixGetRow(Jout, m_blockSizes, i);
92 m_components[i]->applyProjectionJacobian(inblock, outblock);
93 }
94 }
95
96 void applyNormalConeProjectionJacobian(const ConstVectorRef &z,
97 MatrixRef Jout) const override {
98 for (std::size_t i = 0; i < m_components.size(); i++) {
99 ConstVectorRef inblock = blockVectorGetRow(z, m_blockSizes, i);
100 MatrixRef outblock = blockMatrixGetRow(Jout, m_blockSizes, i);
101 m_components[i]->applyNormalConeProjectionJacobian(inblock, outblock);
102 }
103 }
104
105 void computeActiveSet(const ConstVectorRef &z,
106 Eigen::Ref<ActiveType> out) const override {
107 for (std::size_t i = 0; i < m_components.size(); i++) {
108 ConstVectorRef inblock = blockVectorGetRow(z, m_blockSizes, i);
109 decltype(out) outblock = blockVectorGetRow(out, m_blockSizes, i);
110 m_components[i]->computeActiveSet(inblock, outblock);
111 }
112 }
113
114 const std::vector<xyz::polymorphic<Base>> &components() const {
115 return m_components;
116 }
117 const std::vector<Eigen::Index> &blockSizes() const { return m_blockSizes; }
118
119private:
120 std::vector<xyz::polymorphic<Base>> m_components;
121 std::vector<Eigen::Index> m_blockSizes;
122};
123
124} // namespace aligator
Main package namespace.
auto blockMatrixGetRow(const Eigen::MatrixBase< Derived > &matrix, boost::span< const Eigen::Index > rowBlockSizes, std::size_t rowIdx)
auto blockVectorGetRow(const Eigen::MatrixBase< Derived > &matrix, boost::span< const Eigen::Index > blockSizes, std::size_t blockIdx)
ConstraintSetProductTpl(const ConstraintSetProductTpl &)=default
Scalar evaluate(const ConstVectorRef &zproj) const override
void normalConeProjection(const ConstVectorRef &z, VectorRef zout) const override
Compute projection of z onto the normal cone to the set. The default implementation is just .
ConstraintSetProductTpl & operator=(const ConstraintSetProductTpl &)=default
ConstraintSetProductTpl & operator=(ConstraintSetProductTpl &&)=default
void projection(const ConstVectorRef &z, VectorRef zout) const override
Compute projection of variable z onto the constraint set.
const std::vector< xyz::polymorphic< Base > > & components() const
ConstraintSetProductTpl(ConstraintSetProductTpl &&)=default
void applyNormalConeProjectionJacobian(const ConstVectorRef &z, MatrixRef Jout) const override
Apply the jacobian of the projection on the normal cone.
const std::vector< Eigen::Index > & blockSizes() const
ConstraintSetProductTpl(const std::vector< xyz::polymorphic< Base > > components, const std::vector< Eigen::Index > &blockSizes)
void computeActiveSet(const ConstVectorRef &z, Eigen::Ref< ActiveType > out) const override
void applyProjectionJacobian(const ConstVectorRef &z, MatrixRef Jout) const override
Apply a jacobian of the projection/proximal operator to a matrix.
Eigen::Matrix< bool, Eigen::Dynamic, 1 > ActiveType