Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[IKFastTemplate] Expand solutions to full joint range in searchPositionIK #598

Conversation

@hartmanndennis
Copy link
Contributor

commented Aug 22, 2017

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.

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))))

This comment has been minimized.

Copy link
@jrgnicho

jrgnicho Aug 23, 2017

Contributor

Should this be
(solutions[i][j] + 2 * M_PI < (joint_max_vector_[j] + LIMIT_TOLERANCE) with + LIMIT_TOLERANCE instead of -?

This comment has been minimized.

Copy link
@hartmanndennis

hartmanndennis Aug 23, 2017

Author Contributor

Yes.

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] ||

This comment has been minimized.

Copy link
@jrgnicho

jrgnicho Aug 23, 2017

Contributor

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

This comment has been minimized.

Copy link
@hartmanndennis

hartmanndennis Aug 23, 2017

Author Contributor

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]

This comment has been minimized.

Copy link
@v4hn

v4hn Aug 30, 2017

Member

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].

This comment has been minimized.

Copy link
@hartmanndennis

hartmanndennis Aug 31, 2017

Author Contributor

Changed it so it doesn't change continuous joints.

@hartmanndennis hartmanndennis force-pushed the iirob:pr-ikfast-plugin-expand-solutions-in-joint-limits branch from 176e9c4 to 3b665b1 Aug 23, 2017

Changed searchPositionIK to return the closest solution to the seed i…
…n the full joint range in the no freem paramater case.

@hartmanndennis hartmanndennis force-pushed the iirob:pr-ikfast-plugin-expand-solutions-in-joint-limits branch from 3b665b1 to 3376006 Aug 31, 2017

@jrgnicho

This comment has been minimized.

Copy link
Contributor

commented Aug 31, 2017

It took me a little while to make sense out of this change but I think it should unwind the joint values as expected.

Added new function getSolution with extra parameter ik_seed_state tha…
…t rotates each viable joint in steps of 360degree nearest to the seed state.

searchPositionIK and both getPositionIK functions now use the new getSolution.
@hartmanndennis

This comment has been minimized.

Copy link
Contributor Author

commented Sep 11, 2017

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.
In this version multi-soluition getPositionIK still return only solutions that differ more than rotation(s) by 360° but each solution returned is closest to the seed state in respect to 360° rotations. I think this is a good compromise.
If you aggree with this version I'll squash rebase the PR. else I'll revert it.

@v4hn

This comment has been minimized.

Copy link
Member

commented Sep 11, 2017

The approach looks good to me.
I can't check the code/ verify runtime at the moment though.

@v4hn

This comment has been minimized.

Copy link
Member

commented Sep 21, 2017

Merging.

I'm unsure about whether this should be back-ported to indigo because it changes the behavior quite a bit.
I'll wait for users to speak up if they need it there.

@v4hn v4hn merged commit 98524d4 into ros-planning:kinetic-devel Sep 21, 2017

1 check passed

continuous-integration/travis-ci/pr The Travis CI build passed
Details
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
3 participants
You can’t perform that action at this time.