orocos_kinematics_dynamics
orocos_kinematics_dynamics copied to clipboard
Add degraded status chainiksolverpos
The nr position solver currently produces a non-convergence error, if the Cartesian pose does not converge to the desired value within the specified maximum number of iterations. However, the lack of convergence could be due to the singularity avoidance being active leaving the solver in a "degraded" state. So that it cannot achieve the desired pose.
This commit adds a degraded flag which is checked, when the maximum number of iterations is reached. If it is degraded, then the solver error is set to E_DEGRADED
and the user can then decide whether it should be allowed to continue. Otherwise, the error is set to E_MAX_ITERATIONS_EXCEEDED
which would normally terminate the process.
In addition, functions were added to set/get the input parameters (maxiter
, eps
) and retrieve other data from the solver (twist error, number of iterations, velocity solver status).