Singularity in constraint of relative pose

[florent-lamiraux]

I have an explicit relative pose constraint between two objects with right hand side (implicit) 0 0 0 0 3.14159 0. When the configuration of object 2 is computed from the configuration of object 1 by the explicit formulation, the right hand side of the implicit formulation is 0 0 0 0 -3.14159 0. This yields an error of norm 6.28318.

I think this could be fixed by making the computation of f(q) - rhs virtual in constraint::Implicit and calling the virtual method instead of

hpp-constraints/src/solver/hierarchical-iterative.cc

Line 624 in 9b0c493

d.error = d.output - d.rightHandSide;

.

Unfortunately, relative pose constraints are not specialized in the implicit formulation. I see therefore two solutions:

1. specializing implicit relative pose (I think we did that in the past and removed the class later),
2. fix the issue in the explicit formulation only.

[jmirabel]

A theoretically cleaner way to address this is to let the right hand side be a LieGroupElement instead of a mere vector so that the minus that yields the wrong error gets fixed.

I already tried this. In hpp-constraints, there is a class called TransformationSE3 that returns a value in SE3. I am sure it works in certain conditions (I have used it already) but I also remember it does not work everywhere (but I think it is only a matter of implementation).

[florent-lamiraux]

I will have a look. Thank you.

[florent-lamiraux]

I think it is possible to modify explicit_::RelativePose so that the implicit formulation uses RelativeTransformationSE3.
I also think that here

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

Line 103 in 9b0c493

std::vector <bool> mask = std::vector<bool>(6,true),

the mask should always be the default value. I propose to remove the argument to the constructor.