Inverse Kinematics cleanup and improvements

[traversaro]

We are a bit time-constrained now, but the InverseKinematics class would really benefit from a cleanup. We have several tests checking its behavior, so it should be relatively pain-free to do refactor on it.

Several improvements include:

• [ ] Fix support for quaternion-based optimization (https://github.com/robotology/idyntree/issues/1059).
• [ ] Add support for position inequality constraint/tasks (requested by @pattacini).
• [ ] Refactor all the conversion between "geometric" (i.e. using angular velocities) and "analytical" (i.e. using derivative on rpy & quaternions) jacobians. It would probably make sense to move all that code to somewhere else, for example KinDynComputations or some higher level class. In that case it is important to decide what to do for left-trivialized/right-trivialized representations.
• [ ] Refactor all Constraints/Targets logic in separate classes, so we can have FrameConstraint, COMConstraint, etc, etc. The Constraint interface of OpenSOT could be a good inspiration for this: http://opensot.github.io/Constraint_8h_source.html .
  • [ ] correctly implement the PIMPL idiom (i.e. directly access private data from InverseKinematics)
  • [x] Add 3D Center of Mass task to the class (needed by @S-Dafarra for walking)
  • [x] Add support for computing the position of the task/constraints using all the joints of a model, even if only a subset is actually used for solving the inverse kinematics problem ( https://github.com/robotology/idyntree/issues/320 ).

[traversaro]

cc @francesco-romano

[traversaro]

• [x] Add 3D Center of Mass task to the class (needed by @S-Dafarra for walking)

[traversaro]

• [ ] Add support for computing the position of the task/constraints using all the joints of a model, even if only a subset is actually used for solving the inverse kinematics problem ( https://github.com/robotology/idyntree/issues/320 ).

[francesco-romano]

• [ ] correctly implement the PIMPL idiom (i.e. directly access private data from InverseKinematics).

Actually: clean up the whole PIMPL: the main idea (of June 2016) is the following:

• InverseKinematics contains the public interface
• InverseKinematicsData contains the private data of InverseKinematics
• InverseKinematicsNLP (to be renamed to InverseKinematicsIPOPT, or similar) contains the specific NLP implementation for IPOPT. Ideally, switching this class should allow a different solver to be used

[traversaro]

cc @lrapetti this is the main issue regarding the InverseKinematics. See also all other issues with the IK label: https://github.com/robotology/idyntree/labels/Component%3A%20InverseKinematics .