PSO: use KinematicsInverse() to calculate PSO joint position