hpp-constraints
5.1.0
Definition of basic geometric constraints for motion planning
|
Go to the documentation of this file.
29 #ifndef HPP_CONSTRAINTS_EXPLICIT_CONSTRAINT_SET_HH
30 #define HPP_CONSTRAINTS_EXPLICIT_CONSTRAINT_SET_HH
39 namespace constraints {
170 bool contains(
const ExplicitPtr_t& numericalConstraint)
const;
176 : configSpace_(space),
183 argFunction_(
Eigen::VectorXi::Constant(space->nq(), -1)),
184 derFunction_(
Eigen::VectorXi::Constant(space->nv(), -1)),
192 notOutArgs_.addRow(0, space->nq());
193 notOutDers_.addCol(0, space->nv());
203 errorThreshold_ = threshold;
209 return errorThreshold_ * errorThreshold_;
245 return inOutDependencies_;
249 Eigen::MatrixXi inOutDofDependencies()
const;
251 const Eigen::VectorXi&
derFunction()
const {
return derFunction_; }
269 std::size_t
nq()
const {
return configSpace_->nq(); }
272 std::size_t
nv()
const {
return configSpace_->nv(); }
289 return MatrixBlockView(jacobian, outDers_.nbIndices(), outDers_.indices(),
290 notOutDers_.nbIndices(), notOutDers_.indices());
417 std::ostream& print(std::ostream& os)
const;
420 typedef std::vector<bool> Computed_t;
426 void solveExplicitConstraint(
const std::size_t& i,
vectorOut_t arg)
const;
441 void computeJacobian(
const std::size_t& i,
matrixOut_t J)
const;
442 void computeOrder(
const std::size_t& iF, std::size_t& iOrder,
443 Computed_t& computed);
466 Eigen::MatrixXi inOutDependencies_;
468 std::vector<Data> data_;
469 std::vector<std::size_t> computationOrder_;
475 Eigen::VectorXi argFunction_, derFunction_;
479 mutable vector_t arg_, diff_, diffSmall_;
482 ExplicitConstraintSet()
493 configSpace_ = space;
494 argFunction_ = Eigen::VectorXi::Constant(space->nq(), -1);
495 derFunction_ = Eigen::VectorXi::Constant(space->nv(), -1);
496 arg_.resize(space->nq());
497 diff_.resize(space->nv());
499 notOutArgs_.addRow(0, space->nq());
500 notOutDers_.addCol(0, space->nv());
509 #endif // HPP_CONSTRAINTS_EXPLICIT_CONSTRAINT_SET_HH
const RowBlockIndices & outArgs() const
Definition: explicit-constraint-set.hh:257
pinocchio::vector_t vector_t
Definition: fwd.hh:59
value_type errorThreshold() const
Get error threshold.
Definition: explicit-constraint-set.hh:206
Eigen::MatrixBlockView< matrix_t, Eigen::Dynamic, Eigen::Dynamic, false, false > MatrixBlockView
Definition: explicit-constraint-set.hh:105
std::size_t nq() const
The number of variables.
Definition: explicit-constraint-set.hh:269
Definition: explicit-constraint-set.hh:99
const ColBlockIndices & notOutDers() const
Definition: explicit-constraint-set.hh:231
void errorThreshold(const value_type &threshold)
Set error threshold.
Definition: explicit-constraint-set.hh:202
pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t
Definition: fwd.hh:69
LiegroupSpacePtr_t configSpace() const
The Lie group on which constraints are defined.
Definition: explicit-constraint-set.hh:266
pinocchio::LiegroupElement LiegroupElement
Definition: fwd.hh:65
MatrixBlockView jacobianNotOutToOut(matrix_t &jacobian) const
Definition: explicit-constraint-set.hh:288
ExplicitConstraintSet(const LiegroupSpacePtr_t &space)
Definition: explicit-constraint-set.hh:175
pinocchio::matrix_t matrix_t
Definition: fwd.hh:56
Definition: matrix-view.hh:124
const Eigen::MatrixXi & inOutDependencies() const
Definition: explicit-constraint-set.hh:244
Eigen::ColBlockIndices ColBlockIndices
Definition: explicit-constraint-set.hh:102
const Eigen::VectorXi & derFunction() const
Definition: explicit-constraint-set.hh:251
std::size_t nv() const
The number of derivative variables.
Definition: explicit-constraint-set.hh:272
Eigen::NumTraits< value_type > NumTraits
Definition: by-substitution.hh:36
Eigen::Ref< matrix_t > matrixOut_t
Definition: fwd.hh:58
Definition: by-substitution.hh:72
Definition: active-set-differentiable-function.hh:36
pinocchio::size_type size_type
Definition: fwd.hh:47
value_type squaredErrorThreshold() const
Get error threshold.
Definition: explicit-constraint-set.hh:208
const RowBlockIndices & outDers() const
Definition: explicit-constraint-set.hh:263
size_type errorSize() const
Definition: explicit-constraint-set.hh:121
shared_ptr< Implicit > ImplicitPtr_t
Definition: fwd.hh:173
pinocchio::value_type value_type
Definition: fwd.hh:48
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:61
shared_ptr< Explicit > ExplicitPtr_t
Definition: fwd.hh:183
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:60
Eigen::RowBlockIndices RowBlockIndices
Definition: explicit-constraint-set.hh:101
const RowBlockIndices & notOutArgs() const
Definition: explicit-constraint-set.hh:226
const RowBlockIndices & inArgs() const
Set of input configuration variables.
Definition: explicit-constraint-set.hh:218
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:88
const ColBlockIndices & inDers() const
Set of input velocity variables.
Definition: explicit-constraint-set.hh:221