Find constrained solutions in Screw Theory solver

[PeterBowman]

Currently, the ScrewTheoryLib is unable to solve any IK problem that entails finding at least one solution of a redundant mechanism (i.e. more than 6 DOF, usually). There are several ways to overcome this.

In DrPardosGotor/ScrewTheoryRobotics-KINEMATICS-Illustrated, the 7-DOF KUKA IIWA robotic arm has been solved in an elegant manner: in the process of selecting the most adequate IK subproblem, whenever a situation arises in which too many unknowns would prevent the algorithm from applying a Paden-Kahan or Pardos-Gotor formula, one or more joints values can be assumed known. More precisely, this particular problem applies PK1 on joints q1 and q3 (this choice was mandated by the mere process of simplification) under the assumption that we may force a known orientation for the robot tool during the whole process of solving the IK problem. Ultimately, sixteen solutions are found.

This algorithm could prove useful also for non-redundant problems. Because of its configuration, AMOR (a 7-DOF arm) cannot be solved even if we happened to block one of its joints. In that 6-DOF scenario, it would not be possible to obtain eight solutions, but at least that's better than none.

Ref: Dr. José M. Pardos-Gotor, Screw Theory for Robotics. A practical approach for modern Robot KINEMATICS. An Illustrated Handbook., s. 6.6: "Examples of inverse kinematics: Redundant robot (e.g. KUKA IIWAR820)", p. 116. ISBN 978-1717931818.