17 #ifndef HPP_CONSTRAINTS_EXPLICIT_CONSTRAINT_SET_HH
18 #define HPP_CONSTRAINTS_EXPLICIT_CONSTRAINT_SET_HH
29 namespace constraints {
144 bool& constraintFound)
const;
171 : configSpace_ (space)
172 , inArgs_ (), notOutArgs_ ()
173 , inDers_ (), notOutDers_ ()
174 , outArgs_ (), outDers_ ()
175 , argFunction_ (
Eigen::VectorXi::Constant(space->nq (), -1))
176 , derFunction_ (
Eigen::VectorXi::Constant(space->nv (), -1))
180 , arg_ (space->nq ()), diff_(space->nv ()), diffSmall_()
182 notOutArgs_.addRow(0, space->nq ());
183 notOutDers_.addCol(0, space->nv ());
194 errorThreshold_ = threshold;
199 return errorThreshold_;
204 return errorThreshold_*errorThreshold_;
253 return inOutDependencies_;
289 std::size_t
nq ()
const
291 return configSpace_->nq ();
295 std::size_t
nv ()
const
297 return configSpace_->nv ();
317 outDers_.nbIndices() , outDers_.indices(),
318 notOutDers_.nbIndices(), notOutDers_.indices());
446 std::ostream&
print (std::ostream& os)
const;
449 typedef std::vector<bool> Computed_t;
455 void solveExplicitConstraint(
const std::size_t& i,
vectorOut_t arg)
471 void computeJacobian(
const std::size_t& i,
matrixOut_t J)
const;
472 void computeOrder(
const std::size_t& iF, std::size_t& iOrder, Computed_t& computed);
495 Eigen::MatrixXi inOutDependencies_;
497 std::vector<Data> data_;
498 std::vector<std::size_t> computationOrder_;
504 Eigen::VectorXi argFunction_, derFunction_;
508 mutable vector_t arg_, diff_, diffSmall_;
512 : inArgs_ (), notOutArgs_ ()
513 , inDers_ (), notOutDers_ ()
514 , outArgs_ (), outDers_ ()
521 configSpace_ = space;
522 argFunction_ = Eigen::VectorXi::Constant(space->nq (), -1);
523 derFunction_ = Eigen::VectorXi::Constant(space->nv (), -1);
524 arg_.resize(space->nq ());
525 diff_.resize(space->nv ());
527 notOutArgs_.addRow(0, space->nq ());
528 notOutDers_.addCol(0, space->nv ());
Definition: matrix-view.hh:819
Definition: explicit-constraint-set.hh:92
ColBlockIndices activeParameters() const
Same as inArgs.
const ColBlockIndices & activeDerivativeParameters() const
Same as inDers.
void jacobian(matrixOut_t jacobian, vectorIn_t q) const
bool contains(const ExplicitPtr_t &numericalConstraint) const
Eigen::ColBlockIndices ColBlockIndices
Definition: explicit-constraint-set.hh:95
bool isSatisfied(vectorIn_t arg, vectorOut_t error, value_type errorThreshold=-1) const
const RowBlockIndices & notOutArgs() const
Definition: explicit-constraint-set.hh:227
ExplicitConstraintSet(const LiegroupSpacePtr_t &space)
Definition: explicit-constraint-set.hh:170
size_type errorSize() const
Definition: explicit-constraint-set.hh:112
bool rightHandSideFromInput(const ExplicitPtr_t &constraint, vectorIn_t p)
bool isConstraintSatisfied(const ImplicitPtr_t &constraint, vectorIn_t arg, vectorOut_t error, bool &constraintFound) const
size_type add(const ExplicitPtr_t &constraint)
void rightHandSide(const size_type &i, vectorIn_t rhs)
bool isSatisfied(vectorIn_t arg, value_type errorThreshold=-1) const
Eigen::MatrixBlockView< matrix_t, Eigen::Dynamic, Eigen::Dynamic, false, false > MatrixBlockView
Definition: explicit-constraint-set.hh:96
const ColBlockIndices & inDers() const
Set of input velocity variables.
Definition: explicit-constraint-set.hh:219
void rightHandSide(vectorIn_t rhs)
MatrixBlockView jacobianNotOutToOut(matrix_t &jacobian) const
Definition: explicit-constraint-set.hh:314
void rightHandSideFromInput(const size_type &i, vectorIn_t p)
const ColBlockIndices & notOutDers() const
Definition: explicit-constraint-set.hh:235
Eigen::RowBlockIndices RowBlockIndices
Definition: explicit-constraint-set.hh:94
void errorThreshold(const value_type &threshold)
Set error threshold.
Definition: explicit-constraint-set.hh:192
vector_t rightHandSide() const
std::size_t nq() const
The number of variables.
Definition: explicit-constraint-set.hh:289
const Eigen::VectorXi & derFunction() const
Definition: explicit-constraint-set.hh:259
const Eigen::MatrixXi & inOutDependencies() const
Definition: explicit-constraint-set.hh:251
vector_t rightHandSideFromInput(vectorIn_t p)
Eigen::MatrixXi inOutDofDependencies() const
Same as inOutDependencies except that cols correpond to DoFs.
value_type errorThreshold() const
Get error threshold.
Definition: explicit-constraint-set.hh:197
const RowBlockIndices & inArgs() const
Set of input configuration variables.
Definition: explicit-constraint-set.hh:213
bool getRightHandSide(const ExplicitPtr_t &constraint, vectorOut_t rhs) const
const RowBlockIndices & outArgs() const
Definition: explicit-constraint-set.hh:268
value_type squaredErrorThreshold() const
Get error threshold.
Definition: explicit-constraint-set.hh:202
const RowBlockIndices & outDers() const
Definition: explicit-constraint-set.hh:277
std::size_t nv() const
The number of derivative variables.
Definition: explicit-constraint-set.hh:295
std::ostream & print(std::ostream &os) const
size_type rightHandSideSize() const
bool solve(vectorOut_t arg) const
LiegroupSpacePtr_t configSpace() const
The Lie group on which constraints are defined.
Definition: explicit-constraint-set.hh:283
bool rightHandSide(const ExplicitPtr_t &constraint, vectorIn_t rhs)
Definition: by-substitution.hh:64
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
Eigen::NumTraits< value_type > NumTraits
Definition: by-substitution.hh:24
pinocchio::LiegroupElement LiegroupElement
Definition: fwd.hh:53
shared_ptr< Explicit > ExplicitPtr_t
Definition: fwd.hh:179
pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t
Definition: fwd.hh:57
shared_ptr< Implicit > ImplicitPtr_t
Definition: fwd.hh:163
pinocchio::size_type size_type
Definition: fwd.hh:36
pinocchio::value_type value_type
Definition: fwd.hh:37
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:48
pinocchio::matrix_t matrix_t
Definition: fwd.hh:44
Eigen::Ref< matrix_t > matrixOut_t
Definition: fwd.hh:46
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:49
pinocchio::vector_t vector_t
Definition: fwd.hh:47
Definition: active-set-differentiable-function.hh:24