hpp-constraints 6.0.0
Definition of basic geometric constraints for motion planning
|
#include <hpp/constraints/explicit-constraint-set.hh>
Public Types | |
typedef Eigen::RowBlockIndices | RowBlockIndices |
typedef Eigen::ColBlockIndices | ColBlockIndices |
typedef Eigen::MatrixBlockView< matrix_t, Eigen::Dynamic, Eigen::Dynamic, false, false > | MatrixBlockView |
Public Member Functions | |
MatrixBlockView | jacobianNotOutToOut (matrix_t &jacobian) const |
void | jacobian (matrixOut_t jacobian, vectorIn_t q) const |
std::ostream & | print (std::ostream &os) const |
Resolution | |
bool | solve (vectorOut_t arg) const |
size_type | errorSize () const |
bool | isSatisfied (vectorIn_t arg, value_type errorThreshold=-1) const |
bool | isSatisfied (vectorIn_t arg, vectorOut_t error, value_type errorThreshold=-1) const |
bool | isConstraintSatisfied (const ImplicitPtr_t &constraint, vectorIn_t arg, vectorOut_t error, bool &constraintFound) const |
Construction of the problem | |
size_type | add (const ExplicitPtr_t &constraint) |
bool | contains (const ExplicitPtr_t &numericalConstraint) const |
ExplicitConstraintSet (const LiegroupSpacePtr_t &space) | |
Parameters | |
void | errorThreshold (const value_type &threshold) |
Set error threshold. | |
value_type | errorThreshold () const |
Get error threshold. | |
value_type | squaredErrorThreshold () const |
Get error threshold. | |
Input and outputs | |
const RowBlockIndices & | inArgs () const |
Set ![]() | |
const ColBlockIndices & | inDers () const |
Set of input velocity variables. | |
const RowBlockIndices & | notOutArgs () const |
const ColBlockIndices & | notOutDers () const |
ColBlockIndices | activeParameters () const |
Same as inArgs. | |
const ColBlockIndices & | activeDerivativeParameters () const |
Same as inDers. | |
const Eigen::MatrixXi & | inOutDependencies () const |
Eigen::MatrixXi | inOutDofDependencies () const |
Same as inOutDependencies except that cols correpond to DoFs. | |
const Eigen::VectorXi & | derFunction () const |
const RowBlockIndices & | outArgs () const |
const RowBlockIndices & | outDers () const |
LiegroupSpacePtr_t | configSpace () const |
The Lie group on which constraints are defined. | |
std::size_t | nq () const |
The number of variables. | |
std::size_t | nv () const |
The number of derivative variables. | |
Right hand side accessors | |
vector_t | rightHandSideFromInput (vectorIn_t p) |
bool | rightHandSideFromInput (const ExplicitPtr_t &constraint, vectorIn_t p) |
void | rightHandSideFromInput (const size_type &i, vectorIn_t p) |
void | rightHandSide (vectorIn_t rhs) |
bool | rightHandSide (const ExplicitPtr_t &constraint, vectorIn_t rhs) |
bool | getRightHandSide (const ExplicitPtr_t &constraint, vectorOut_t rhs) const |
void | rightHandSide (const size_type &i, vectorIn_t rhs) |
vector_t | rightHandSide () const |
size_type | rightHandSideSize () const |
Friends | |
class | solver::BySubstitution |
Set of explicit constraints
This class combines compatible explicit constraints as defined in the following paper published in Robotics Science and System 2018: https://hal.archives-ouvertes.fr/hal-01804774/file/paper.pdf, Section II-B Definition 4.
An explicit constraint
For manipulation planning, it is useful to handle a parameterizable right hand side
The right hand side may be set using the various methods ExplicitConstraintSet::rightHandSide and ExplicitConstraintSet::rightHandSideFromInput.
To add explicit constraints, use methods ExplicitConstraintSet::add. If the constraint to add is not compatible with the previous one, this method returns -1.
Method ExplicitConstraintSet::solve solves the explicit constraints.
The combination of compatible explicit constraints is an explicit constraint. As such this class can be considered as an explicit constraint.
We will therefore use the following notation
typedef Eigen::MatrixBlockView<matrix_t, Eigen::Dynamic, Eigen::Dynamic, false, false> hpp::constraints::ExplicitConstraintSet::MatrixBlockView |
|
inline |
Constructor
space | Lie group on which constraints are defined. |
const ColBlockIndices & hpp::constraints::ExplicitConstraintSet::activeDerivativeParameters | ( | ) | const |
Same as inDers.
ColBlockIndices hpp::constraints::ExplicitConstraintSet::activeParameters | ( | ) | const |
Same as inArgs.
size_type hpp::constraints::ExplicitConstraintSet::add | ( | const ExplicitPtr_t & | constraint | ) |
Attempt to add an explicit constraint
constraint | explicit constraint |
|
inline |
The Lie group on which constraints are defined.
bool hpp::constraints::ExplicitConstraintSet::contains | ( | const ExplicitPtr_t & | numericalConstraint | ) | const |
Check whether an explicit numerical constraint has been added
numericalConstraint | explicit numerical constraint |
|
inline |
|
inline |
Size of error vector
|
inline |
Get error threshold.
|
inline |
Set error threshold.
bool hpp::constraints::ExplicitConstraintSet::getRightHandSide | ( | const ExplicitPtr_t & | constraint, |
vectorOut_t | rhs | ||
) | const |
Get the right hand side for a given explicit constraint
constraint | the explicit constraint, |
rhs | right hand side. |
|
inline |
Set
|
inline |
Set of input velocity variables.
|
inline |
Returns a matrix of integer whose:
Eigen::MatrixXi hpp::constraints::ExplicitConstraintSet::inOutDofDependencies | ( | ) | const |
Same as inOutDependencies except that cols correpond to DoFs.
bool hpp::constraints::ExplicitConstraintSet::isConstraintSatisfied | ( | const ImplicitPtr_t & | constraint, |
vectorIn_t | arg, | ||
vectorOut_t | error, | ||
bool & | constraintFound | ||
) | const |
Whether a constraint is satisfied for an input vector
constraint,the | constraint in the solver, |
arg | the input vector |
error | the error of the constraint. |
constraintFound | whether the constraint belongs to the solver, |
bool hpp::constraints::ExplicitConstraintSet::isSatisfied | ( | vectorIn_t | arg, |
value_type | errorThreshold = -1 |
||
) | const |
Whether input vector satisfies the constraints of the solver
arg | input vector |
errorThreshold | threshold to compare against the norms of the constraint errors. Default value is accessible via methods errorThreshold. |
bool hpp::constraints::ExplicitConstraintSet::isSatisfied | ( | vectorIn_t | arg, |
vectorOut_t | error, | ||
value_type | errorThreshold = -1 |
||
) | const |
Whether input vector satisfies the constraints of the solver
arg | input vector |
error | the constraint errors |
errorThreshold | threshold to compare against the norms of the constraint errors. Default value is accessible via methods errorThreshold. |
void hpp::constraints::ExplicitConstraintSet::jacobian | ( | matrixOut_t | jacobian, |
vectorIn_t | q | ||
) | const |
Compute the Jacobian of the explicit constraint resolution
q | input configuration |
jacobian | square Jacobian matrix of same size as velocity i.e. given by nv method. |
The result is the Jacobian of the explicit constraint set considered as a projector that maps to any
|
inline |
Return Jacobian matrix of output variable wrt not output variables
jacobian | Jacobian matrix of the mapping from non output variables to output variables. The columns of this matrix corresponding to variables ![]() ![]()
![]() |
|
inline |
Set
Configuration variables that are not output variables
|
inline |
Set
Velocity variables that are not output variables
|
inline |
The number of variables.
|
inline |
The number of derivative variables.
|
inline |
Set
|
inline |
Set of output velocity variables
std::ostream & hpp::constraints::ExplicitConstraintSet::print | ( | std::ostream & | os | ) | const |
vector_t hpp::constraints::ExplicitConstraintSet::rightHandSide | ( | ) | const |
Get the right hand sides
bool hpp::constraints::ExplicitConstraintSet::rightHandSide | ( | const ExplicitPtr_t & | constraint, |
vectorIn_t | rhs | ||
) |
Set the right hand side for a given explicit constraint
constraint | the explicit constraint, |
rhs | right hand side. |
void hpp::constraints::ExplicitConstraintSet::rightHandSide | ( | const size_type & | i, |
vectorIn_t | rhs | ||
) |
Set the right hand side for a given explicit constraint
i | order of the explicit constraint, |
rhs | right hand side. |
void hpp::constraints::ExplicitConstraintSet::rightHandSide | ( | vectorIn_t | rhs | ) |
Set the right hand sides of the explicit constraints.
rhs | the right hand side. |
The components of rhs are dispatched to the right hand sides of the explicit constraints in the order they are added.
bool hpp::constraints::ExplicitConstraintSet::rightHandSideFromInput | ( | const ExplicitPtr_t & | constraint, |
vectorIn_t | p | ||
) |
Compute right hand side of constraint using input configuration.
constraint | explicit constraint, |
p | vector in ![]() |
Let
where in general
in such a way that all
void hpp::constraints::ExplicitConstraintSet::rightHandSideFromInput | ( | const size_type & | i, |
vectorIn_t | p | ||
) |
Compute right hand side of constraint using input configuration.
i | index of the explicit constraint, |
p | vector in ![]() |
Let
where in general
in such a way that all
vector_t hpp::constraints::ExplicitConstraintSet::rightHandSideFromInput | ( | vectorIn_t | p | ) |
Compute right hand side of constraints using input configuration.
p | vector in ![]() |
For each explicit constraint
where in general
in such a way that all
size_type hpp::constraints::ExplicitConstraintSet::rightHandSideSize | ( | ) | const |
bool hpp::constraints::ExplicitConstraintSet::solve | ( | vectorOut_t | arg | ) | const |
Solve the explicit constraints
arg | input configuration, |
arg | output configuration satisfying the explicit constraints: ![]() |
|
inline |
Get error threshold.
|
friend |