Add parameterization type to orientation constraints

[JeroenDM]

As explained and discussed in #89

I think it could be useful to add an extra field to the message for orientation constraints that specifies how the orientation should be parameterized when testing the constraints. Currently, the tolerances are applied to the XYZ Euler angles (intrinsic rotations), but they could be just as well be applied to any three number parameterization.

While working on #2092, I seem to notice that exponential coordinates (rotation angle * rotation axis) performed better for projection-based sampling.

[...], but for planners that don't like Euler angles, it is useful to have this alternative. For example, TrajOpt does not use Euler angles.

I'm almost finished with a corresponding PR that implements this new parameterization. (Edit: the PR) This would look like this:

  if (parameterization_ == Parameterization::XYZ_EULER_ANGLES)
  {
    // original code
    xyz = diff.linear().eulerAngles(0, 1, 2);  // 0,1,2 corresponds to XYZ, the convention used in sampling constraints
    xyz(0) = std::min(fabs(xyz(0)), boost::math::constants::pi<double>() - fabs(xyz(0)));
    xyz(1) = std::min(fabs(xyz(1)), boost::math::constants::pi<double>() - fabs(xyz(1)));
    xyz(2) = std::min(fabs(xyz(2)), boost::math::constants::pi<double>() - fabs(xyz(2)));
  }
  else if (parameterization_ == Parameterization::ROTATION_VECTOR)
  {
   // new code
    Eigen::AngleAxisd aa(diff.linear());
    xyz = aa.axis() * aa.angle();
    xyz(0) = fabs(xyz(0));
    xyz(1) = fabs(xyz(1));
    xyz(2) = fabs(xyz(2));
  }

in moveit_core/kinematics_constraints.

[JeroenDM]

@mamoll @v4hn what do you think?

This will bring us a step closer to planning with orientation constraints with the new OMPL planner.

[AndyZe]

Approved assuming you make the small punctuation change

[JeroenDM]

I applied the latest review suggestions. Thank you!

@felixvd I would think squashing is fine for such a small change, but if you want I can clean up the commit history.

[felixvd]

Yes, I'll squash merge after this last comment.

[felixvd]

I'm good with the content, but shouldn't this new block be moved above the absolute_axis_tolerance entries? This line also assumes they are below.

[JeroenDM]

Oh, good catch, I thought I changed it to above... I will fix it. But about the order:

My reasoning was that if you do not care about the parameterization you don't have to read it. I imagined the file was organized with the important (required) features at the top and the optional less common features at the bottom.

But on the other hand, it is maybe better to first mention how the tolerance values are used before allowing you to specify them.

[JeroenDM]

@felixvd I changed "below" to "above". I assume users start reading at the top of the file, and they can stop reading if they don't care about which parameterization is used. (For small tolerances, or tolerance on a single axis, this does not matter.)

Is this ok?

[felixvd]

Some nits, but I'm fine with this otherwise. Will merge after last comments and maybe changes from Jeroen.

[JeroenDM]

@felixvd I went through your latest suggestions.