Replace custom FK with MoveIt FK

[wyattrees]

Given recent issues that have arisen out of bio_ik's Forward Kinematics, this issue proposes a switch to use FK from MoveIt's RobotState.

Replacing bio_ik's FK with MoveIt FK
Changing bio_ik to use MoveIt for forward kinematics would touch a significant amount of code.

A brief start on the work required here is present in the moveit-fk branch.

Primarily, this would require changing how each solver thread calculates the FK and checks the fitness of a potential solution. The code in question is from the main solver calculation loop in ik_parallel.h:

auto& fk = solvers[i]->model;

fk.applyConfiguration(result);

bool success = solvers[i]->checkSolution(result, fk.getTipFrames());

This code is meant to be generic to different types of solvers. In each solver, model is a member variable that refers to the forward kinematics being used. This has been a specific version of the various kinds of FK that bio_ik offers, even though many are present. Starting with the ik_gradient solver, the fk variable retrieved here should not refer to bio_ik's Frame type whatsoever. This may require implementing the above functionality as a part of a single method implemented by each solver type. To start, the base class IKBase & IKGradientDescent need to implement this. The base class implementation may follow the code above exactly, while the IKGradientDescent implementation may differ significantly.

Changing one solver's FK to use MoveIt would, at minimum, include:

• Changing all references to bio_ik's Frame type to use Eigen::Isometry3d to represent transforms. Use of Frame is most prevalant in methods of IKBase and Problem.
• Here is where the forward kinematics are actually calculated while evaluating a potential solution. Instead of using bio_ik::RobotFK (model), we should use moveit::core::RobotState and it's corresponding methods to compute FK.
• Providing an evaluate method of each Goal type that uses Eigen methods of comparing positions & orientations. The old evaluate may be left as is, but the new FK framework needs a way of querying the fitness of goals based on Eigen types.