PS: Calculates new configuration for linear and circular motion, but keeps the configuration for joint motion