Commit 2d612dd7 authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

[doc] Improve documentation of explicit_::RelativePose.

parent adfde796
Pipeline #17653 passed with stage
in 14 minutes and 32 seconds
......@@ -48,7 +48,7 @@ namespace hpp {
Then the differential function is of the form
\f{eqnarray*}{
\mathbf{q}_{out} - f \left(\mathbf{q}_{in}\right)
\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
......@@ -56,9 +56,9 @@ namespace hpp {
It is straightforward that an equality constraint with this function can
solved explicitely:
\f{align*}{
\mathbf{q}_{out} &- f \left(\mathbf{q}_{in}\right) = rhs \\
\mathbf{q}_{out} &- g \left(\mathbf{q}_{in}\right) = rhs \\
& \mbox {if and only if}\\
\mathbf{q}_{out} &= f \left(\mathbf{q}_{in}\right) + rhs \\
\mathbf{q}_{out} &= g \left(\mathbf{q}_{in}\right) + rhs \\
\f}
If function \f$f\f$ takes values in a Lie group (SO(2), SO(3)),
the above "+" between a Lie group element and a tangent vector
......@@ -89,7 +89,7 @@ namespace hpp {
\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} \\
& & & -\frac{\partial g}{q_1} & & & & -\frac{\partial g}{q_2} \\
&&&&&\\
& 0 & & & & 0 & & & & 1
\end{array}\right)
......@@ -98,13 +98,13 @@ namespace hpp {
\f{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}}
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)
\f}
where
\li \f$R_{out}\f$ is the rotation matrix corresponding to unit
quaternion \f$(q_{oc1},q_{oc2},q_{oc3},q_{oc4})\f$,
\li \f$R_{f}\f$ is the rotation matrix corresponding to the part of the
\li \f$R_{g}\f$ is the rotation matrix corresponding to the part of the
output value of \f$f\f$ corresponding to SO(3),
\li \f$J_{log}\f$ is the Jacobian matrix of function that associates
to a rotation matrix \f$R\f$ the vector \f$\omega\f$ such that
......@@ -184,7 +184,7 @@ namespace hpp {
///
/// The default implementation computes
/// \f{equation}
/// f \left((q_{ic_{1}} \cdots q_{ic_{n_{ic}}})^T\right) + rhs
/// g \left((q_{ic_{1}} \cdots q_{ic_{n_{ic}}})^T\right) + rhs
/// \f}
virtual void outputValue(LiegroupElementRef result, vectorIn_t qin,
LiegroupElementConstRef rhs) const;
......@@ -192,15 +192,15 @@ namespace hpp {
/// Compute Jacobian of output value
///
/// \f{eqnarray*}
/// J &=& \frac{\partial}{\partial\mathbf{q}_{in}}\left(f(\mathbf{q}_{in})
/// J &=& \frac{\partial}{\partial\mathbf{q}_{in}}\left(g(\mathbf{q}_{in})
/// + rhs\right).
/// \f}
/// \param qin vector of input variables,
/// \param f_value \f$f(\mathbf{q}_{in})\f$ provided to avoid
/// \param g_value \f$f(\mathbf{q}_{in})\f$ provided to avoid
/// recomputation,
/// \param rhs right hand side (of implicit formulation).
virtual void jacobianOutputValue(vectorIn_t qin, LiegroupElementConstRef
f_value, LiegroupElementConstRef rhs,
g_value, LiegroupElementConstRef rhs,
matrixOut_t jacobian) const;
protected:
/// Constructor
......@@ -234,7 +234,7 @@ namespace hpp {
/// \param implicitFunction differentiable function of the implicit
/// formulation if different from default expression
/// \f{equation}{
/// \mathbf{q}_{out} - f \left(\mathbf{q}_{in}\right),
/// \mathbf{q}_{out} - g \left(\mathbf{q}_{in}\right),
/// \f}
Explicit
(const DifferentiableFunctionPtr_t& implicitFunction,
......
......@@ -24,6 +24,42 @@ namespace hpp {
namespace constraints {
namespace explicit_ {
/// Constraint of relative pose between two frames on a kinematic chain
///
/// Given an input configuration \f$\mathbf{q}\f$, solving this constraint
/// consists in computing output variables with respect to input
/// variables:
/// \f[
/// \mathbf{q}_{out} = g(\mathbf{q}_{in})
///\f]
/// where \f$\mathbf{q}_{out}\f$ are the configuration variables of
/// input joint2, \f${q}_{in}\f$ are the configuration variables
/// of input joint1 and parent joints, and \f$g\f$ is a mapping
/// with values is SE(3). Note that joint2 should be a
/// freeflyer joint.
///
/// \note According to the documentation of class Explicit, the
/// implicit formulation should be
/// \f[
/// f(\mathbf{q}) = \mathbf{q}_{out} - g(\mathbf{q}_{in}).
/// \f]
/// As function \f$g\f$ takes values in SE(3), the above expression is
/// equivalent to
/// \f[
/// f(\mathbf{q}) = \log_{SE(3)}\left(g(\mathbf{q}_{in})^{-1}
/// \mathbf{q}_{out}\right)
/// \f]
/// that represents the log of the error of input joint2 pose
/// (\f$\mathbf{q}_{out}\f$) with respect to its desired value
/// (\f$g(\mathbf{q}_{in}\f$). The problem with this expression is
/// that it is different from the corresponding implicit formulation
/// hpp::constraints::RelativeTransformationR3xSO3 that compares the poses of input
/// joint1 and joint2. For manipulation planning applications where pairs
/// of constraints and complements are replaced by an explicit constraint,
/// this difference of formulation results in inconsistencies such as a
/// configuration satisfying one formulation (the error being below the
/// threshold) but not the other one. To cope with this issue, the default
/// implicit formulation is replaced by the one defined by class
/// hpp::constraints::RelativeTransformationR3xSO3.
class HPP_CONSTRAINTS_DLLAPI RelativePose : public Explicit
{
public:
......
......@@ -72,9 +72,11 @@ namespace hpp {
/// \addtogroup constraints
/// \{
/** GenericTransformation class encapsulates 6 possible differentiable
* functions: Position, Orientation, Transformation and their relative
* versions RelativePosition, RelativeOrientation, RelativeTransformation.
/** GenericTransformation class encapsulates 10 possible differentiable
* functions: Position, Orientation, OrientationSO3, Transformation,
* TransformationR3xSO3 and their relative
* versions RelativeTransformationR3xSO3, RelativePosition,
* RelativeOrientation, RelativeOrientationSO3, RelativeTransformation.
*
* These functions compute the position of frame
* GenericTransformation::frame2InJoint2 in joint
......@@ -83,11 +85,14 @@ namespace hpp {
* frame. For absolute functions, GenericTransformation::joint1 is
* NULL and joint1 frame is the world frame.
*
* The value of the RelativeTransformation function is a 6-dimensional
* The value of the RelativeTransformation function is
* a 6-dimensional
* vector. The 3 first coordinates are the position of the center of the
* second frame expressed in the first frame.
* The 3 last coordinates are the log of the orientation of frame 2 in
* frame 1.
* The values of RelativePosition and RelativeOrientation are respectively
* the 3 first and the 3 last components of the above 6 dimensional vector.
*
* \f{equation*}
* f (\mathbf{q}) = \left(\begin{array}{c}
......@@ -106,6 +111,9 @@ namespace hpp {
* (R_2 J_{2\,\omega} - R_1 J_{1\,\omega})
* \end{array}\right)
* \f}
*
* For RelativeTransformationR3xSO3, OrientationSO3, the 3 last components
* are replaced by a quaternion.
*/
template <int _Options>
class HPP_CONSTRAINTS_DLLAPI GenericTransformation :
......
......@@ -113,7 +113,7 @@ namespace hpp {
}
void Explicit::jacobianOutputValue(vectorIn_t qin, LiegroupElementConstRef
f_value, LiegroupElementConstRef rhs,
g_value, LiegroupElementConstRef rhs,
matrixOut_t jacobian) const
{
assert(rhs.space()->isVectorSpace());
......@@ -121,7 +121,7 @@ namespace hpp {
if (!rhs.vector().isZero())
{
explicitFunction ()->outputSpace ()->dIntegrate_dq
<pinocchio::DerivativeTimesInput> (f_value, rhs.vector(), jacobian);
<pinocchio::DerivativeTimesInput> (g_value, rhs.vector(), jacobian);
}
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment