|
static
ExplicitNumericalConstraintPtr_t | create (const DevicePtr_t &robot, const DifferentiableFunctionPtr_t &function, const segments_t &inputConf, const segments_t &inputVelocity, const segments_t &outputConf, const segments_t &outputVelocity, const ComparisonTypes_t &comp=ComparisonTypes_t()) |
| Create instance and return shared pointer. More...
|
|
static
ExplicitNumericalConstraintPtr_t | create (const DevicePtr_t &robot, const DifferentiableFunctionPtr_t &function, const DifferentiableFunctionPtr_t &g, const DifferentiableFunctionPtr_t &ginv, const segments_t &inputConf, const segments_t &inputVelocity, const segments_t &outputConf, const segments_t &outputVelocity, const ComparisonTypes_t &comp=ComparisonTypes_t()) |
| Create instance and return shared pointer. More...
|
|
static
ExplicitNumericalConstraintPtr_t | createCopy (const ExplicitNumericalConstraintPtr_t &other) |
| Create a copy and return shared pointer. More...
|
|
static NumericalConstraintPtr_t | create (const DifferentiableFunctionPtr_t &function) |
| Create a shared pointer to a new instance. More...
|
|
static NumericalConstraintPtr_t | create (const DifferentiableFunctionPtr_t &function, ComparisonTypes_t comp) |
| Create a shared pointer to a new instance. More...
|
|
static NumericalConstraintPtr_t | create (const DifferentiableFunctionPtr_t &function, ComparisonTypes_t comp, vectorIn_t rhs) |
| Create a shared pointer to a new instance. More...
|
|
static NumericalConstraintPtr_t | createCopy (const NumericalConstraintPtr_t &other) |
| Create a copy and return shared pointer. More...
|
|
Explicit numerical constraint.
An explicit numerical constraint is a constraint such that some
configuration variables called \b output are function of the
others called \b input.
Let
\li \form#6 be the list of indices
corresponding to ordered input configuration variables,
\li \form#7 be the list of indices
corresponding to ordered output configuration variables,
\li \form#8 be the list of indices
corresponding to ordered input degrees of freedom,
\li \form#9 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 \form#10 is less than the robot
configuration size, and \form#11 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 NumericalConstraint, 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*}