hpp-constraints
4.9.0
Definition of basic geometric constraints for motion planning
|
Go to the documentation of this file.
17 #ifndef HPP_CONSTRAINTS_EXPLICIT_CONSTRAINT_SET_HH
18 #define HPP_CONSTRAINTS_EXPLICIT_CONSTRAINT_SET_HH
22 #include <boost/function.hpp>
31 namespace constraints {
131 bool& constraintFound)
const;
152 bool contains (
const ExplicitPtr_t& numericalConstraint)
const;
158 : configSpace_ (space)
159 , inArgs_ (), notOutArgs_ ()
160 , inDers_ (), notOutDers_ ()
161 , outArgs_ (), outDers_ ()
162 , argFunction_ (
Eigen::VectorXi::Constant(space->nq (), -1))
163 , derFunction_ (
Eigen::VectorXi::Constant(space->nv (), -1))
166 , arg_ (space->nq ()), diff_(space->nv ()), diffSmall_()
168 notOutArgs_.addRow(0, space->nq ());
169 notOutDers_.addCol(0, space->nv ());
180 squaredErrorThreshold_ = threshold * threshold;
185 return sqrt (squaredErrorThreshold_);
190 return squaredErrorThreshold_;
255 return inOutDependencies_;
259 Eigen::MatrixXi inOutDofDependencies ()
const;
288 return configSpace_->nq ();
295 return configSpace_->nv ();
305 std::size_t
nq ()
const
307 return configSpace_->nq ();
311 std::size_t
nv ()
const
313 return configSpace_->nv ();
324 outDers_.nbIndices() , outDers_.indices(),
325 notOutDers_.nbIndices(), notOutDers_.indices());
343 outDers_.nbIndices() , outDers_.indices(),
344 notOutDers_.nbIndices(), notOutDers_.indices());
409 bool rightHandSideFromInput (
const ExplicitPtr_t& constraint,
472 std::ostream& print (std::ostream& os)
const;
475 typedef std::vector<bool> Computed_t;
481 void solveExplicitConstraint(
const std::size_t& i,
vectorOut_t arg)
497 void computeJacobian(
const std::size_t& i,
matrixOut_t J)
const;
498 void computeOrder(
const std::size_t& iF, std::size_t& iOrder, Computed_t& computed);
522 Eigen::MatrixXi inOutDependencies_;
524 std::vector<Data> data_;
525 std::vector<std::size_t> computationOrder_;
531 Eigen::VectorXi argFunction_, derFunction_;
534 mutable vector_t arg_, diff_, diffSmall_;
540 #endif // HPP_CONSTRAINTS_EXPLICIT_CONSTRAINT_SET_HH
const RowBlockIndices & outArgs() const
Definition: explicit-constraint-set.hh:270
const RowBlockIndices & freeArgs() const HPP_CONSTRAINTS_DEPRECATED
Definition: explicit-constraint-set.hh:213
pinocchio::vector_t vector_t
Definition: fwd.hh:45
value_type errorThreshold() const
Get error threshold.
Definition: explicit-constraint-set.hh:183
const ColBlockIndices & freeDers() const HPP_CONSTRAINTS_DEPRECATED
Definition: explicit-constraint-set.hh:221
Eigen::MatrixBlockView< matrix_t, Eigen::Dynamic, Eigen::Dynamic, false, false > MatrixBlockView
Definition: explicit-constraint-set.hh:98
std::size_t nq() const
The number of variables.
Definition: explicit-constraint-set.hh:305
Definition: explicit-constraint-set.hh:93
const ColBlockIndices & notOutDers() const
Definition: explicit-constraint-set.hh:237
void errorThreshold(const value_type &threshold)
Set error threshold.
Definition: explicit-constraint-set.hh:178
pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t
Definition: fwd.hh:54
LiegroupSpacePtr_t configSpace() const
The Lie group on which constraints are defined.
Definition: explicit-constraint-set.hh:299
pinocchio::LiegroupElement LiegroupElement
Definition: fwd.hh:51
MatrixBlockView jacobianNotOutToOut(matrix_t &jacobian) const
Definition: explicit-constraint-set.hh:340
ExplicitConstraintSet(const LiegroupSpacePtr_t &space)
Definition: explicit-constraint-set.hh:157
pinocchio::matrix_t matrix_t
Definition: fwd.hh:42
Definition: matrix-view.hh:111
const Eigen::MatrixXi & inOutDependencies() const
Definition: explicit-constraint-set.hh:253
Eigen::ColBlockIndices ColBlockIndices
Definition: explicit-constraint-set.hh:97
const Eigen::VectorXi & derFunction() const
Definition: explicit-constraint-set.hh:261
std::size_t nv() const
The number of derivative variables.
Definition: explicit-constraint-set.hh:311
Eigen::NumTraits< value_type > NumTraits
Definition: by-substitution.hh:24
Eigen::Ref< matrix_t > matrixOut_t
Definition: fwd.hh:44
std::size_t argSize() const HPP_CONSTRAINTS_DEPRECATED
Definition: explicit-constraint-set.hh:286
Definition: active-set-differentiable-function.hh:24
boost::shared_ptr< Explicit > ExplicitPtr_t
Definition: fwd.hh:173
pinocchio::size_type size_type
Definition: fwd.hh:35
value_type squaredErrorThreshold() const
Get error threshold.
Definition: explicit-constraint-set.hh:188
const RowBlockIndices & outDers() const
Definition: explicit-constraint-set.hh:279
pinocchio::value_type value_type
Definition: fwd.hh:36
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:47
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:46
Eigen::RowBlockIndices RowBlockIndices
Definition: explicit-constraint-set.hh:96
const RowBlockIndices & notOutArgs() const
Definition: explicit-constraint-set.hh:229
boost::shared_ptr< Implicit > ImplicitPtr_t
Definition: fwd.hh:157
const RowBlockIndices & inArgs() const
Set of input configuration variables.
Definition: explicit-constraint-set.hh:199
MatrixBlockView viewJacobian(matrix_t &jacobian) const HPP_CONSTRAINTS_DEPRECATED
Definition: explicit-constraint-set.hh:320
std::size_t derSize() const HPP_CONSTRAINTS_DEPRECATED
Definition: explicit-constraint-set.hh:293
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
DifferentiableFunctionSet DifferentiableFunctionStack HPP_CONSTRAINTS_DEPRECATED
Definition: fwd.hh:99
const ColBlockIndices & inDers() const
Set of input velocity variables.
Definition: explicit-constraint-set.hh:205