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

## Conversation

Contributor

### hartmanndennis 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.
reviewed
 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))))

#### 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 -?

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

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

#### hartmanndennis Aug 23, 2017 • edited

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]

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

#### hartmanndennis Aug 31, 2017

Author Contributor

Changed it so it doesn't change continuous joints.

### hartmanndennisforce-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.`
``` 3376006 ```

Contributor

### jrgnicho commented Aug 31, 2017 • edited

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

### jrgnicho added the awaits 2nd review label Aug 31, 2017

``` 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.```
``` 14a8ac9 ```
Contributor Author

### hartmanndennis 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.
Member

### v4hn commented Sep 11, 2017

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

### v4hn 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 merged commit `98524d4` into ros-planning:kinetic-devel Sep 21, 2017 1 check passed

#### 1 check passed

continuous-integration/travis-ci/pr The Travis CI build passed
Details
referenced this pull request Sep 26, 2017