[IKFastTemplate] Expand solutions to full joint range in searchPositionIK #598
Conversation
| solutions[i][j] -= 2 * M_PI; | ||
| } | ||
| while (distance < -M_PI && (!joint_has_limits_vector_[j] || | ||
| (solutions[i][j] + 2 * M_PI < (joint_max_vector_[j] - LIMIT_TOLERANCE)))) |
There was a problem hiding this comment.
Should this be
(solutions[i][j] + 2 * M_PI < (joint_max_vector_[j] + LIMIT_TOLERANCE) with + LIMIT_TOLERANCE instead of -?
| for (std::size_t j = 0; j < num_joints_; ++j) | ||
| { | ||
| double distance = solutions[i][j] - ik_seed_state[j]; | ||
| while (distance > M_PI && (!joint_has_limits_vector_[j] || |
There was a problem hiding this comment.
It looks like you could check the joint_has_limits_vector_ variable at the very beginning of the second for loop instead of each while loop
There was a problem hiding this comment.
I don't think so. If a joint doesn't have joint limits I still want to rotate the solution.
Edit: Actually I'm not sure if the solution should then be in [-PI,PI]
There was a problem hiding this comment.
I'm not sure I interpret your edit right, but yes, if a joint is continuous, the output solution here should always be in [-pi;pi].
There was a problem hiding this comment.
Changed it so it doesn't change continuous joints.
hartmanndennis
force-pushed
the
pr-ikfast-plugin-expand-solutions-in-joint-limits
branch
from
176e9c4 to
3b665b1
Compare
…n the full joint range in the no freem paramater case.
hartmanndennis
force-pushed
the
pr-ikfast-plugin-expand-solutions-in-joint-limits
branch
from
3b665b1 to
3376006
Compare
|
It took me a little while to make sense out of this change but I think it should unwind the joint values as expected. |
…t rotates each viable joint in steps of 360degree nearest to the seed state. searchPositionIK and both getPositionIK functions now use the new getSolution.
|
I added a commit that adds this behavior for all cases by adding a new version of getSolution with an extra parameter ik_seed_state and calling this function in the relevant parts. |
|
The approach looks good to me. |
|
Merging. I'm unsure about whether this should be back-ported to indigo because it changes the behavior quite a bit. |
* Use __has_includes preprocessor directive for deprecated headers * Fix parameter template types * Proper initialization of smart pointers, rclcpp::Duration
Implementation of #571 for the no free parameter case of searchPositionIK.
This implementation works without solution explosion and still returns the closest solution to the seed.
It should scale linearly with increased joint limits.
Implementation for the other case and other functions requires more code changes.