hpp-constraints
4.13.0
Definition of basic geometric constraints for motion planning
|
#include <hpp/constraints/implicit.hh>
Static Public Member Functions | |
static ImplicitPtr_t | create (const DifferentiableFunctionPtr_t &func, ComparisonTypes_t comp, std::vector< bool > mask=std::vector< bool >()) |
static ImplicitPtr_t | createCopy (const ImplicitPtr_t &other) |
Create a copy and return shared pointer. More... | |
Protected Member Functions | |
Implicit (const DifferentiableFunctionPtr_t &function, ComparisonTypes_t comp, std::vector< bool > mask) | |
Implicit (const Implicit &other) | |
Copy constructor. More... | |
virtual bool | isEqual (const Implicit &other, bool swapAndTest) const |
void | init (const ImplicitWkPtr_t &weak) |
Implicit () | |
Friends | |
class | ImplicitConstraintSet |
This class represents a parameterizable numerical constraint that compares the output of a function \(h\) to a right hand side Lie group element.
A configuration \(\mathbf{q}\) is said to satisfy the constraint for a given right hand side if and only if the error \(e\) as computed below is smaller in norm than a given threshold.
Let
\[ \Delta = h (\mathbf{q}) - rhs \in \mathbf{R}^{n_v}, \]
for each component \(i\in\{0,\cdots,n_v-1\}\),
A mask is a vector of Boolean values of size \(n_v\). Values set to false means that the corresponding component of the error defined above is not taken into account to determine whether the constraint is satisfied. The active rows of the constraint may be accessed via method activeRows.
Lines with \(\mathbf{Equality}\) comparator in the above definition of the error need a parameter, while lines with comparators \(\mathbf{Inferior}\), \(\mathbf{Superior}\), or \(\mathbf{EqualToZero}\) do not. As a consequence, the right hand side of the constraint is defined by a vector \(\lambda\) of parameters of size the number of \(\mathbf{Equality}\) occurences in vector \(\mathbf{c}\). The right hand side is then defined as in the following example:
\[ rhs = \exp\left(\begin{array}{c}\lambda_1 \\ 0 \\ 0 \\ \lambda_2 \\ \vdots \end{array}\right) \ \ \ \ \mathbf{c} = \left(\begin{array}{c}\mathbf{Equality} \\ \mathbf{EqualToZero} \\ \mathbf{Inferior} \\ \mathbf{Equality} \\ \vdots \end{array}\right) \]
To retrieve the size of vector \(\lambda\), call method Implicit::parameterSize (). To set and get the right hand side value, use method Implicit::rightHandSide.
The right hand side of the constraint may depend on time, for instance if the constraint is associated to a trajectory following task. In this case, the right hand side is a function from \(\mathbf{R}\) to \(\mathbf{L}\).
To set or get a time varying right hand side, use methods Implicit::rightHandSideFunction, Implicit::rightHandSideFromConfig. To get the value of the right hand side for a given time, use method Implicit::rightHandSideAt.
|
inlinevirtual |
|
protected |
Constructor
function | the differentiable function |
comp | vector of comparison \(\mathbf{c}\). |
mask | mask defining which components of the error are taken into account to determine whether the constraint is satisfied. sizes of comp and of mask should be equal to size of tangent space to function output space \(\mathbf{L}\). |
|
protected |
Copy constructor.
|
inlineprotected |
|
inline |
bool hpp::constraints::Implicit::checkRightHandSide | ( | LiegroupElementConstRef | rhs | ) | const |
Check right hand side with regard to comparison types
rhs | right hand side, |
This method checks that elements of the right hand side log corresponding to comparison types different from Equality are equal to 0.
const ComparisonTypes_t& hpp::constraints::Implicit::comparisonType | ( | ) | const |
Return the ComparisonType.
void hpp::constraints::Implicit::comparisonType | ( | const ComparisonTypes_t & | comp | ) |
Set the comparison type.
|
virtual |
Copy object and return shared pointer to copy.
Reimplemented in hpp::constraints::Explicit, hpp::constraints::explicit_::RelativePose, hpp::constraints::LockedJoint, and hpp::constraints::explicit_::ConvexShapeContact.
|
static |
Create a shared pointer to a new instance.
|
static |
Create a copy and return shared pointer.
|
inline |
Get indices of constraint coordinates that are equality.
|
inline |
Return a reference to function \(h\).
|
inline |
Return a reference to function \(h\).
|
inlineprotected |
|
protectedvirtual |
Test equality with other instance
other | object to copy |
swapAndTest | whether we should also check other == this |
Reimplemented in hpp::constraints::Explicit, and hpp::constraints::LockedJoint.
bool hpp::constraints::Implicit::operator== | ( | const Implicit & | other | ) | const |
Operator equality.
size_type hpp::constraints::Implicit::parameterSize | ( | ) | const |
Get size of parameter defining the right hand side of the constraint
See class documentation for details.
void hpp::constraints::Implicit::rightHandSide | ( | vectorIn_t | rhs | ) |
Set the right hand side of the equation.
rhs | the right hand side. |
vectorIn_t hpp::constraints::Implicit::rightHandSide | ( | ) | const |
Return the right hand side of the equation.
vectorOut_t hpp::constraints::Implicit::rightHandSide | ( | ) |
Return a modifiable reference to right hand side of equation.
vectorIn_t hpp::constraints::Implicit::rightHandSideAt | ( | const value_type & | s | ) |
Evaluate and set right hand side at given time
s | time |
void hpp::constraints::Implicit::rightHandSideFromConfig | ( | ConfigurationIn_t | config, |
LiegroupElementRef | rhs | ||
) |
Computes the right hand side from a configuration
in such a way that the configuration satisfies the numerical constraints
config | the input configuration. |
rhs | right hand side as a Lie group element. rhs should be initialized with the right LiegroupSpace (ie the output space of the function). |
void hpp::constraints::Implicit::rightHandSideFromConfig | ( | ConfigurationIn_t | config | ) |
Set the right hand side from a configuration
in such a way that the configuration satisfies the numerical constraints
config | the input configuration. |
void hpp::constraints::Implicit::rightHandSideFunction | ( | const DifferentiableFunctionPtr_t & | rhsF | ) |
Set time-varying right hand side
rhsF | Mapping from \(\mathbf{R}\) to output space \(\mathbf{L}\) of \(h\) defining the variation along time of the right hand side. |
|
inline |
Get time-varying right hand side
size_type hpp::constraints::Implicit::rightHandSideSize | ( | ) | const |
Get size of right hand side of the constraint
This is the dimension (nq) of the output space of the function
void hpp::constraints::Implicit::setInactiveRowsToZero | ( | vectorOut_t | error | ) | const |
Set inactive components of error to 0
error | error vector computed by substracting a right hand side to the output to the value of the function. |
|
friend |