#include <hpp/constraints/explicit.hh>
Public Member Functions inherited from Protected Attributes | |
| DifferentiableFunctionPtr_t | inputToOutput_ |
| segments_t | inputConf_ |
| segments_t | outputConf_ |
| segments_t | inputVelocity_ |
| segments_t | outputVelocity_ |
Detailed Description
Explicit numerical constraint
An explicit numerical constraint is a constraint such that some configuration variables called output are function of the others called input.
Let
- \((ic_{1}, \cdots, ic_{n_{ic}})\) be the list of indices corresponding to ordered input configuration variables,
- \((oc_{1}, \cdots, oc_{n_{oc}})\) be the list of indices corresponding to ordered output configuration variables,
- \((iv_{1}, \cdots, iv_{n_{iv}})\) be the list of indices corresponding to ordered input degrees of freedom,
- \((ov_{1}, \cdots, ov_{n_{ov}})\) be the list of indices corresponding to ordered output degrees of freedom.
Recall that degrees of freedom refer to velocity vectors.
Let us notice that \(n_{ic} + n_{oc}\) is less than the robot configuration size, and \(n_{iv} + n_{ov}\) is less than the velocity size. Some degrees of freedom may indeed be neither input nor output.
Then the differential function is of the form
\begin{equation*} \left(\begin{array}{c} q_{oc_{1}} \\ \vdots \\ q_{oc_{n_{oc}}} \end{array}\right) - f \left((q_{ic_{1}} \cdots q_{ic_{n_{ic}}})^T\right) \end{equation*}
It is straightforward that an equality constraint with this function can solved explicitely:
\begin{align*} \left(\begin{array}{c} q_{oc_{1}} \\ \vdots \\ q_{oc_{n_{oc}}} \end{array}\right) &- f \left((q_{ic_{1}} \cdots q_{ic_{n_{ic}}})^T\right) = rhs \\ & \mbox {if and only if}\\ \left(\begin{array}{c} q_{oc_{1}} \\ \vdots \\ q_{oc_{n_{oc}}} \end{array}\right) &= f \left((q_{ic_{1}} \cdots q_{ic_{n_{ic}}})^T\right) + rhs \\ \end{align*}
If function \(f\) takes values in a Lie group (SO(2), SO(3)), the above "+" between a Lie group element and a tangent vector has to be undestood as the integration of the constant velocity from the Lie group element:
\begin{equation*} \mathbf{q} + \mathbf{v} = \mathbf{q}.\exp (\mathbf{v}) \end{equation*}
where \(\mathbf{q}\) is a Lie group element and \(\mathbf{v}\) is a tangent vector.
Considered as a Implicit instance, the expression of the Jacobian of the DifferentiableFunction above depends on the output space of function \(f\). The rows corresponding to values in a vector space are expressed as follows.
for any index \(i\) between 0 and the size of velocity vectors, either
- \(\dot{q}_i\) is an input degree of freedom: \(\exists j\) integer, \(1 \leq j \leq n_{iv}\) such that \(i=iv_{j}\),
- \(\dot{q}_i\) is an output degree of freedom: \(\exists j\) integer, \(1\leq j \leq n_{ov}\) such that \(i=ov_{j}\), or
- \(\dot{q}_i\) neither input nor output. In this case, the corresponding column is equal to 0.
\begin{equation*} J = \left(\begin{array}{cccccccccccc} \cdots & ov_1 & \cdots & iv_{1} & \cdots & ov_2 & \cdots & iv_2 & \cdots & ov_{n_{ov}} & \cdots \\ & 1 & & & & 0 & & & & & \\ & 0 & & & & 1 & & & & & \\ & & & -\frac{\partial f}{q_1} & & & & -\frac{\partial f}{q_2} \\ &&&&&\\ & 0 & & & & 0 & & & & 1 \end{array}\right) \end{equation*}
The rows corresponding to values in SO(3) have the following expression.\begin{equation*} J = \left(\begin{array}{cccccccccccc} ov_1 \ ov_2 \ ov_3 & iv_1 \cdots iv_{n_{iv}} \\ J_{log}(R_{f}^T R_{out}) & -J_{log}(R_{f}^T R_{out})R_{out}^T R_{f} \frac{\partial f}{\partial q_{in}} \end{array}\right) \end{equation*}
where - \(R_{out}\) is the rotation matrix corresponding to unit quaternion \((q_{oc1},q_{oc2},q_{oc3},q_{oc4})\),
- \(R_{f}\) is the rotation matrix corresponding to the part of the output value of \(f\) corresponding to SO(3),
- \(J_{log}\) is the Jacobian matrix of function that associates to a rotation matrix \(R\) the vector \(\omega\) such that
\begin{equation*} R = \exp (\left[\omega\right]_{\times}) \end{equation*}
Constructor & Destructor Documentation
◆ Explicit() [1/3]
|
protected |
Constructor
- Parameters
-
robot Robot for which the constraint is defined. function relation between input configuration variables and output configuration variables, inputConf set of integer intervals defining indices \((ic_{1}, \cdots, ic_{n_{ic}})\), outputConf set of integer intervals defining indices \((oc_{1}, \cdots, oc_{n_{oc}})\), inputVelocity set of integer defining indices \((iv_{1}, \cdots, iv_{n_{iv}})\). outputVelocity set of integer defining indices \((ov_{1}, \cdots, ov_{n_{ov}})\).
- Note
- comparison type for this constraint is always equality
- Deprecated:
- Use constructor that takes LiegroupSpacePtr_t instead of DevicePtr_t as input and used robot->configSpace () as argument.
◆ Explicit() [2/3]
|
protected |
Constructor
- Parameters
-
configSpace Configuration space on which the constraint is defined, function relation between input configuration variables and output configuration variables, inputConf set of integer intervals defining indices \((ic_{1}, \cdots, ic_{n_{ic}})\), outputConf set of integer intervals defining indices \((oc_{1}, \cdots, oc_{n_{oc}})\), inputVelocity set of integer defining indices \((iv_{1}, \cdots, iv_{n_{iv}})\). outputVelocity set of integer defining indices \((ov_{1}, \cdots, ov_{n_{ov}})\).
- Note
- comparison type for this constraint is always equality
◆ Explicit() [3/3]
|
protected |
Copy constructor.
Member Function Documentation
◆ copy()
|
virtual |
Copy object and return shared pointer to copy.
Reimplemented from hpp::constraints::Implicit.
Reimplemented in hpp::constraints::LockedJoint, and hpp::constraints::explicit_::RelativePose.
◆ create() [1/2]
|
static |
Create instance and return shared pointer
- Parameters
-
robot Robot for which the constraint is defined. function relation between input configuration variables and output configuration variables, inputConf set of integer intervals defining indices \((ic_{1}, \cdots, ic_{n_{ic}})\), outputConf set of integer intervals defining indices \((oc_{1}, \cdots, oc_{n_{oc}})\), inputVelocity set of integer defining indices \((iv_{1}, \cdots, iv_{n_{iv}})\). outputVelocity set of integer defining indices \((ov_{1}, \cdots, ov_{n_{ov}})\).
- Note
- comparison type for this constraint is always equality
- Deprecated:
- Call method that takes LiegroupSpacePtr_t instead of DevicePtr_t as input and used robot->configSpace () as argument.
◆ create() [2/2]
|
static |
Create instance and return shared pointer
- Parameters
-
configSpace Configuration space on which the constraint is defined, function relation between input configuration variables and output configuration variables, inputConf set of integer intervals defining indices \((ic_{1}, \cdots, ic_{n_{ic}})\), outputConf set of integer intervals defining indices \((oc_{1}, \cdots, oc_{n_{oc}})\), inputVelocity set of integer defining indices \((iv_{1}, \cdots, iv_{n_{iv}})\). outputVelocity set of integer defining indices \((ov_{1}, \cdots, ov_{n_{ov}})\).
- Note
- comparison type for this constraint is always equality
◆ createCopy()
|
static |
Create a copy and return shared pointer.
◆ explicitFunction()
|
inlinevirtual |
Function that maps input to output
- Returns
- function \(f\).
◆ explicitToImplicitRhs()
|
inlinevirtual |
Convert right hand side
- Parameters
-
explicitRhs right hand side of explicit formulation.
- Return values
-
implicitRhs right hand side of implicit formulation,
When implicit formulation is different from explicit formulation,
- See also
- hpp::constraints::explicit_::RelativePose, right hand side are also different. This method converts right hand side from explicit to implicit formulation.
When implicit formulation derives from explicit one, this method copies the first argument to the second one.
Reimplemented in hpp::constraints::explicit_::RelativePose.
◆ implicitToExplicitRhs()
|
inlinevirtual |
Convert right hand side
- Parameters
-
implicitRhs right hand side of implicit formulation,
- Return values
-
explicitRhs right hand side of explicit formulation.
When implicit formulation is different from explicit formulation,
- See also
- hpp::constraints::explicit_::RelativePose, right hand side are also different. This method converts right hand side from implicit to explicit formulation.
When implicit formulation derives from explicit one, this method copies the first argument to the second one.
Reimplemented in hpp::constraints::explicit_::RelativePose.
◆ init()
|
protected |
◆ inputConf()
|
inline |
Get input configuration variables.
◆ inputVelocity()
|
inline |
Get input degrees of freedom.
◆ outputConf()
|
inline |
Get output configuration variables.
◆ outputVelocity()
|
inline |
Get output degrees of freedom.
Member Data Documentation
◆ inputConf_
|
protected |
◆ inputToOutput_
|
protected |
◆ inputVelocity_
|
protected |
◆ outputConf_
|
protected |
◆ outputVelocity_
|
protected |
The documentation for this class was generated from the following file:
- include/hpp/constraints/explicit.hh
