[doc] Improve documentation of explicit_::RelativePose.

include/hpp/constraints/explicit.hh

@@ -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,

include/hpp/constraints/explicit/relative-pose.hh

@@ -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:

include/hpp/constraints/generic-transformation.hh

@@ -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 :

src/explicit.cc

@@ -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);
       }
     }