Apply IK solution only when the solver succeeded

[jslee02]

Problem

IK solvers apply solution even when the solver failed to solve.

Solution

Apply solution only on success.

Testing

Added a simple unit test that checks if IK solver changes joint values when the solver failed.

---

Before creating a pull request

• Document new methods and classes
• Format new code files using clang-format

Before merging a pull request

• Set version target by selecting a milestone on the right side
• Summarize this change in CHANGELOG.md
• Add unit test(s) for this change

[mxgrey]

Applying the solution during a failure was intentional, because iterative solvers will often do a decent job of getting the end effector close to the target even if it failed to find a solution, and that's often desirable.

My personal view is that if the user wants the solution to be applied conditionally, they can set _applySolution to false and then choose after the fact whether they want the result to be applied based on whether or not it's a valid solution.

Maybe an alternative to make the behavior more clear would be to change the name of the input argument to _applyResult?

[jslee02]

Thanks for the clarification. I agree with you about iterative solver, but I'm a bit worried that the current IK implementation accepts any types of nonlinear optimizers. Non-iterative solver or analytic IK solver could return totally incorrect solution if the solution doesn't exist.

My personal view is that if the user wants the solution to be applied conditionally, they can set _applySolution to false and then choose after the fact whether they want the result to be applied based on whether or not it's a valid solution.

Yeah, this is possible, but I'd prefer the opposite. I mean the default behavior to be applying solution on success, and the user should apply solution by calling InverseKinematics::setPositions() if they want to use the incomplete solution from an iterative solver. This is because I think the safe and conservative way should be the default.

I'd like to propose three possible solutions:

(a) No change but renaming _applySolution
(b) Change the default value of _applySolution to false so that the user can pay attention to the behavior when set the parameter.
(c) Change the APIs to be more explicit something like

bool solve(Eigen::VectorXd& position);
bool solveAndApply(Eigen::VectorXd& position, bool allowIncompleteResult = false);
bool solveAndApply(bool allowIncompleteResult = false);

In any case, I think we should document more clear about the behavior.

[mxgrey]

I definitely understand where you're coming from, and I agree that we could make it better with some changes. One concern I have is that I'm sure I've written code that depends on the current behavior of solve(~), and I wouldn't be surprised if others have as well.

I guess we could deprecate the solve(bool) and solve(Eigen::VectorXd&, bool) functions and introduce solveAndApply(~) with the signatures you're suggesting. Then we could add a bool findSolution(Eigen::VectorXd& position) function that finds the solution without applying it.

[jslee02]

It sounds good to me! Let me update this PR accordingly.

[jslee02]

Done!