Trajectory inverse kinematics by nonlinear, nongaussian tracking

We study trajectory inverse kinematics: to find a feasible trajectory in angle space that produces a given trajectory in workspace. We explicitly represent the multivalued inverse mapping by the modes of a conditional density of angles given workspace coordinates, estimated by a particle filter. We find all the modes using a mean-shift algorithm and then disambiguate the angle trajectory by minimising over the set of modes a global constraint that penalises discontinuous jumps in angle space or invalid inverses. We demonstrate the method with a PUMA 560 robot arm.