|
| Explicit (const LiegroupSpacePtr_t &configSpace, const DifferentiableFunctionPtr_t &function, const segments_t &inputConf, const segments_t &outputConf, const segments_t &inputVelocity, const segments_t &outputVelocity, const ComparisonTypes_t &comp, std::vector< bool > mask) |
|
| Explicit (const DifferentiableFunctionPtr_t &implicitFunction, const DifferentiableFunctionPtr_t &function, const segments_t &inputConf, const segments_t &outputConf, const segments_t &inputVelocity, const segments_t &outputVelocity, const ComparisonTypes_t &comp, std::vector< bool > mask) |
| (const LiegroupSpacePtr_t&, constDifferentiableFunctionPtr_t&, const segments_t& inputConf, constsegments_t& outputConf, const segments_t& inputVelocity, constsegments_t&, const ComparisonTypes_t&); - Parameters
-
More...
|
|
| Explicit (const Explicit &other) |
| Copy constructor. More...
|
|
bool | isEqual (const Implicit &other, bool swapAndTest) const |
|
void | init (const ExplicitWkPtr_t &weak) |
|
| Explicit () |
|
| 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 () |
|
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{eqnarray*}
\mathbf{q}_{out} - g \left(\mathbf{q}_{in}\right)
\ \ &\mbox{with}&
\mathbf{q}_{out} = \left(q_{oc_{1}} \cdots q_{oc_{n_{oc}}}\right)^T,
\ \ \ \mathbf{q}_{in} = (q_{ic_{1}} \cdots q_{ic_{n_{ic}}})^T
\end{eqnarray*}
It is straightforward that an equality constraint with this function can solved explicitely:
\begin{align*}
\mathbf{q}_{out} &- g \left(\mathbf{q}_{in}\right) = rhs \\
& \mbox {if and only if}\\
\mathbf{q}_{out} &= g \left(\mathbf{q}_{in}\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 g}{q_1} & & & & -\frac{\partial
g}{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_{g}^T R_{out}) & -J_{log}(R_{g}^T R_{out})R_{out}^T R_{g}
\frac{\partial g}{\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_{g}\) 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*}