Create ScrewTheoryLib

[PeterBowman]

This PR adds a Screw Theory inverse kinematics solver and related utilities with a broader goal in mind (https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/119).

The core of this implementation relies on a set of solved geometric problems, among which we find the three well-established Paden-Kahan subproblems as well as a complementary collection of four additional cases as described in José Manuel Pardos-Gotor. Screw Theory for Robotics - A practical approach for Modern Robot KINEMATICS - An Illustrated Handbook. Amazon Fulfillment, 9 2018, ISBN 978-1717931818.

KDL has been chosen as the framework that provides all necessary abstractions for such geometric primitives as HT frames, vectors, joint arrays, etc. KDL wrappers for FK/IK solvers are provided as well.

New entities in the roboticslab:: namespace:

• PoeExpression: product of exponentiales (POE) formula. Helper methods are provided to seamlessly convert to and from a KDL::Chain.

• MatrixExponential: each term of a POE formula (ref).

• ScrewTheoryIkSubproblem: interface for all available (seven so far) IK subproblems.

• ScrewTheoryIkProblem: provides a solve() method to compute IK for a target pose.

• ScrewTheoryIkProblemBuilder: decouples solver initialization from runtime operations. This class searches for a valid sequence of local geometric subproblems that can solve a particular global IK problem. If found, an instance of ScrewTheoryIkProblem is created.

Non-exhaustive tests have proven that the Screw Theory algorithm as implemented here performs 5-10 times faster than a numeric Newton-Raphson solver provided by KDL. All available solutions for TEO's right arm are found in ~0.05 milliseconds.

Sample usage:

PoeExpression poe; // already defined and populated
ScrewTheoryIkProblemBuilder builder(poe);
ScrewTheoryIkProblem * ikProblem = builder.build();

KDL::Frame H;
ScrewTheoryIkProblem::Solutions solutions;

if (ikProblem->solve(H, solutions)
{
    // iterate over all available solutions
}

TODOs not covered here:

• out-of-reach poses not thoroughly tested
• simple API that could be improved upon
• configuration selector, i.e. pick the best solution available (cf. AsibotConfiguration)

[coveralls]

Coverage increased (+10.9%) to 56.854% when pulling d0b3dc060e445aae8c61cfbf955f4addd9452a24 on screw-theory into 6ed8110a8a5879d9d119c2312256e154af73f880 on develop.

[coveralls]

Coverage increased (+10.9%) to 56.854% when pulling d0b3dc060e445aae8c61cfbf955f4addd9452a24 on screw-theory into 6ed8110a8a5879d9d119c2312256e154af73f880 on develop.

[coveralls]

Coverage increased (+10.9%) to 56.854% when pulling d0b3dc060e445aae8c61cfbf955f4addd9452a24 on screw-theory into 6ed8110a8a5879d9d119c2312256e154af73f880 on develop.

[coveralls]

Coverage increased (+17.4%) to 63.374% when pulling 3391ee485efeb60fd6f71168a880bbdcc2b90efc on screw-theory into 6ed8110a8a5879d9d119c2312256e154af73f880 on develop.

[PeterBowman]

Follow-up: https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/e8ed37ccf0b866016b0683673ac9bbce3131cec3 (some renaming + ISBN).