Add degraded status chainiksolverpos #358
Conversation
The position solver was causing an IK failure if it did not converge after the maximum number of iterations was exceeded. However, when singularity avoidance is active (degraded), the solution may not converge but the IK should be allowed to continue. So a degraded flag was added and is checked when the maximum number of iterations is exceeded. If it is degraded, the error is set to E_DEGRADED to allow it to continue. Otherwise, it is set to E_MAX_ITERATIONS_EXCEEDED which will cause the IK to fail. Added functions to set the input parameters (maxiter, eps) and to retrieve the twist error, number of iterations performed, and the velocity solver status.
…singular If the maximum iterations is reached but the velocity solver returns a singular status, it is now flagged as degraded and allowed to continue. Because it was singular, it could not reduce the error to zero which is why it kept iterating to the limit. This does not constitute a failure of the inverse kinematics.
|
Please follow code styler more strictly. Please add more documentations what the new variables are used for. |
@MatthijsBurgh Where is the code style documented? It seems like over the years that no really style has been enforced here. Are there particular things you're looking for now? |
|
@snrkiwi there is indeed no strict guide. I am aiming for the ros1 style but with 4 space indent |
|
Ok, it'll be good to have a convention. Is there somewhere in the doc's that we can document this, and add a URL to the ROS1 style (along with any modifications)? |
|
We opened an issue a while back, #258 |
|
I am not certain where this stands. Are there specific code style issues that need to be addressed to move this forward? I tried to follow the style that was already being used even though I did not always agree with it. Note that there is already documentation of the new variables in the header file, but I can expand upon that if desired. |
| /** | ||
| * Get delta twist from last call to CartToJnt() | ||
| */ | ||
| void getDeltaTwist(KDL::Twist& _delta_twist) |
There was a problem hiding this comment.
Reason why not to just simple return?
There was a problem hiding this comment.
Do you mean something like this?
Twist& getDeltaTwist()
{
return delta_twist;
}
There was a problem hiding this comment.
Yes, indeed. But then get a const reference.
There was a problem hiding this comment.
Ah, so this?
const Twist& getDeltaTwist()const
{
return delta_twist;
}
Co-authored-by: Matthijs van der Burgh <[email protected]>
Co-authored-by: Matthijs van der Burgh <[email protected]>
Co-authored-by: Matthijs van der Burgh <[email protected]>
Co-authored-by: Matthijs van der Burgh <[email protected]>
Co-authored-by: Matthijs van der Burgh <[email protected]>
Co-authored-by: Matthijs van der Burgh <[email protected]>
Co-authored-by: Matthijs van der Burgh <[email protected]>
Co-authored-by: Matthijs van der Burgh <[email protected]>
Co-authored-by: Matthijs van der Burgh <[email protected]>
Co-authored-by: Matthijs van der Burgh <[email protected]>
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_DEGRADEDand the user can then decide whether it should be allowed to continue. Otherwise, the error is set toE_MAX_ITERATIONS_EXCEEDEDwhich 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).