Added batch options to ConcreteRobot's planToTSR

[amalnanavati]

• Added batchSize and maxBatches parameters to planToTSR
• Renamed ConfigurationToTSR's maxSamples to maxSamplingTries to avoid confusion

[Describe this pull request. Link to relevant GitHub issues, if any.]

ConcreteRobot's current planToTSR method samples 100 end-effector poses from the TSR, solves their IK to get a configuration, ranks then based on their distance from the current configuration, and plans in reverse-distance order until it finds a valid plan. However, if IK is slow (e.g., DART's GradientDescentSolver on ADA's 6DoF arm), this procedure is also slow. This PR adds parameters for batchSize and maxBatches, where the planner samples batchSize end effector poses, ranks then, plans to them in reverse-distance order, and only if it doesn't find a valid plan does it continue to the next batch (up to maxBatches). batchSize=100 and maxBatches=1 is equivalent to the old system.

[Explain how this pull request was tested.]

I tested the ADA sim demo with this code and with the current master branches of libada and ada_feeding. I also tested it with modified versions of libada and ada_feeding that modify the batchSize and maxBatches parameters (upcoming PRs in those two repositories) and it worked for multiple configurations of batchSize and maxBatches.

---

Before creating a pull request

• [x] Document new methods and classes
• [x] Format code with make format

Before merging a pull request

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

[amalnanavati]

Q for the reviewer:

In the existing code, before maxSamplingTries is used (in InverseKinematicsSampleable.cpp), there is an assertion to ensure it is positive. If I were to add an assertion for the two new parameters, I believe the best spot for it would be in the constructors of ConfigurationToTSR, which currently do nothing. Let me know if you think I should add assertions into those constructors (lines 29 and 52 of the new ConfigurationToTSR.cpp), or leave it as-is (i.e., trusting the user to pass in positive values for those parameters)

[codecov[bot]]

Codecov Report

Merging #607 (b3c9b42) into master (c0a8b7e) will decrease coverage by 0.16%. The diff coverage is 16.32%.

@@            Coverage Diff             @@
##           master     #607      +/-   ##
==========================================
- Coverage   76.38%   76.22%   -0.17%     
==========================================
  Files         200      200              
  Lines        7030     7044      +14     
==========================================
- Hits         5370     5369       -1     
- Misses       1660     1675      +15     

Impacted Files	Coverage Δ
...do/constraint/dart/InverseKinematicsSampleable.hpp	100.00% <ø> (ø)
...igurationToConfiguration_to_ConfigurationToTSR.cpp	6.25% <0.00%> (-0.90%)	:arrow_down:
src/planner/dart/ConfigurationToTSR.cpp	0.00% <0.00%> (ø)
...rc/constraint/dart/InverseKinematicsSampleable.cpp	85.26% <100.00%> (ø)
src/planner/ompl/CRRT.cpp	72.95% <0.00%> (-0.52%)	:arrow_down:

[amalnanavati]

The code compiles, but I have not been able to test that it works with the ADA feeding demo in simulation (blocked by libada#64 and a corresponding ada_feeding PR.) Converting this into a draft until it can be fully tested with libada and ada_feeding.

[amalnanavati]

Verified that this works with the new AIKIDO/libada/ada_feeding code in simulation.

This should be merged together with ada_feeding pull request #7

[egordon]

LGTM at this point, trusting the testing with both main libada / ada_feeding AND the new ada_feeding branch (which will also be merged today).