Add manipulability check for twist / wrench command controllers #326
Description
The twist controller with damped singular value decomposition may deviate from planned path. A manipulability check should prevent motion in this case.
In addition, to allow for safe use of KUKA's Cartesian impedance control, a singularity check is required.