17 #ifndef HPP_CONSTRAINTS_EXPLICIT_CONSTRAINT_SET_HH 18 #define HPP_CONSTRAINTS_EXPLICIT_CONSTRAINT_SET_HH 22 #include <boost/function.hpp> 31 namespace constraints {
139 bool& constraintFound)
const;
160 bool contains (
const ExplicitPtr_t& numericalConstraint)
const;
166 : configSpace_ (space)
167 , inArgs_ (), notOutArgs_ ()
168 , inDers_ (), notOutDers_ ()
169 , outArgs_ (), outDers_ ()
170 , argFunction_ (
Eigen::VectorXi::Constant(space->nq (), -1))
171 , derFunction_ (
Eigen::VectorXi::Constant(space->nv (), -1))
175 , arg_ (space->nq ()), diff_(space->nv ()), diffSmall_()
177 notOutArgs_.addRow(0, space->nq ());
178 notOutDers_.addCol(0, space->nv ());
189 squaredErrorThreshold_ = threshold * threshold;
194 return sqrt (squaredErrorThreshold_);
199 return squaredErrorThreshold_;
236 ColBlockIndices activeParameters ()
const;
239 const ColBlockIndices& activeDerivativeParameters ()
const;
248 return inOutDependencies_;
252 Eigen::MatrixXi inOutDofDependencies ()
const;
284 std::size_t
nq ()
const 286 return configSpace_->nq ();
290 std::size_t
nv ()
const 292 return configSpace_->nv ();
311 return MatrixBlockView(jacobian,
312 outDers_.nbIndices() , outDers_.indices(),
313 notOutDers_.nbIndices(), notOutDers_.indices());
378 bool rightHandSideFromInput (
const ExplicitPtr_t& constraint,
441 std::ostream& print (std::ostream& os)
const;
444 typedef std::vector<bool> Computed_t;
450 void solveExplicitConstraint(
const std::size_t& i,
vectorOut_t arg)
466 void computeJacobian(
const std::size_t& i,
matrixOut_t J)
const;
467 void computeOrder(
const std::size_t& iF, std::size_t& iOrder, Computed_t& computed);
474 RowBlockIndices equalityIndices;
485 RowBlockIndices inArgs_, notOutArgs_;
486 ColBlockIndices inDers_, notOutDers_;
488 RowBlockIndices outArgs_, outDers_;
490 Eigen::MatrixXi inOutDependencies_;
492 std::vector<Data> data_;
493 std::vector<std::size_t> computationOrder_;
499 Eigen::VectorXi argFunction_, derFunction_;
503 mutable vector_t arg_, diff_, diffSmall_;
507 : inArgs_ (), notOutArgs_ ()
508 , inDers_ (), notOutDers_ ()
509 , outArgs_ (), outDers_ ()
510 , squaredErrorThreshold_ (Eigen::NumTraits<value_type>::epsilon())
516 configSpace_ = space;
517 argFunction_ = Eigen::VectorXi::Constant(space->nq (), -1);
518 derFunction_ = Eigen::VectorXi::Constant(space->nv (), -1);
519 arg_.resize(space->nq ());
520 diff_.resize(space->nv ());
522 notOutArgs_.
addRow(0, space->nq ());
523 notOutDers_.
addCol(0, space->nv ());
532 #endif // HPP_CONSTRAINTS_EXPLICIT_CONSTRAINT_SET_HH void addCol(const size_type &col, const size_type size)
Definition: matrix-view.hh:611
pinocchio::vector_t vector_t
Definition: fwd.hh:47
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:48
void errorThreshold(const value_type &threshold)
Set error threshold.
Definition: explicit-constraint-set.hh:187
std::size_t nq() const
The number of variables.
Definition: explicit-constraint-set.hh:284
Definition: active-set-differentiable-function.hh:24
pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t
Definition: fwd.hh:57
pinocchio::LiegroupElement LiegroupElement
Definition: fwd.hh:53
const RowBlockIndices & inArgs() const
Set of input configuration variables.
Definition: explicit-constraint-set.hh:208
const RowBlockIndices & notOutArgs() const
Definition: explicit-constraint-set.hh:222
pinocchio::matrix_t matrix_t
Definition: fwd.hh:44
void addRow(const size_type &row, const size_type size)
Definition: matrix-view.hh:602
LiegroupSpacePtr_t configSpace() const
The Lie group on which constraints are defined.
Definition: explicit-constraint-set.hh:278
size_type errorSize() const
Definition: explicit-constraint-set.hh:118
std::size_t nv() const
The number of derivative variables.
Definition: explicit-constraint-set.hh:290
Eigen::MatrixBlockView< matrix_t, Eigen::Dynamic, Eigen::Dynamic, false, false > MatrixBlockView
Definition: explicit-constraint-set.hh:98
ExplicitConstraintSet(const LiegroupSpacePtr_t &space)
Definition: explicit-constraint-set.hh:165
Definition: matrix-view.hh:111
const Eigen::MatrixXi & inOutDependencies() const
Definition: explicit-constraint-set.hh:246
boost::shared_ptr< Explicit > ExplicitPtr_t
Definition: fwd.hh:179
const Eigen::VectorXi & derFunction() const
Definition: explicit-constraint-set.hh:254
value_type errorThreshold() const
Get error threshold.
Definition: explicit-constraint-set.hh:192
Eigen::ColBlockIndices ColBlockIndices
Definition: explicit-constraint-set.hh:97
Definition: by-substitution.hh:62
const RowBlockIndices & outDers() const
Definition: explicit-constraint-set.hh:272
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:49
Eigen::RowBlockIndices RowBlockIndices
Definition: explicit-constraint-set.hh:96
pinocchio::size_type size_type
Definition: fwd.hh:36
Eigen::Ref< matrix_t > matrixOut_t
Definition: fwd.hh:46
Eigen::NumTraits< value_type > NumTraits
Definition: by-substitution.hh:24
MatrixBlockView jacobianNotOutToOut(matrix_t &jacobian) const
Definition: explicit-constraint-set.hh:309
const ColBlockIndices & notOutDers() const
Definition: explicit-constraint-set.hh:230
pinocchio::value_type value_type
Definition: fwd.hh:37
boost::shared_ptr< Implicit > ImplicitPtr_t
Definition: fwd.hh:163
const ColBlockIndices & inDers() const
Set of input velocity variables.
Definition: explicit-constraint-set.hh:214
const RowBlockIndices & outArgs() const
Definition: explicit-constraint-set.hh:263
Definition: explicit-constraint-set.hh:93
value_type squaredErrorThreshold() const
Get error threshold.
Definition: explicit-constraint-set.hh:197