Constraint of relative pose between two frames on a kinematic chain
Given an input configuration , solving this constraint consists in computing output variables with respect to input variables:
where are the configuration variables of input joint2, are the configuration variables of input joint1 and parent joints, and 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
As function takes values in SE(3), the above expression is equivalent to
that represents the log of the error of input joint2 pose ( ) with respect to its desired value ( ). 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.