In an effort for fewer singularities to arise during Cartesian planning & Servo, bio_ik's kinematics plugin may be used to prioritize manipulability during such movements. To integrate this with MoveIt2, this requires:

• Introducing cost functions to KinematicsBase, RobotState, and CartesianInterpolator, so they may be consumed by bio_ik
• Making IK modular for Servo by using kinematics plugins, similar to CartesianInterpolator. Currently, only the inverse Jacobian method is supported