[WIP] return Colliding Bodies' Names if user given state is in collision #235

aditya-vk · 2017-10-09T03:26:58Z

This PR forces the planning procedure to terminate if the user provided start state is in collision and prints the body nodes that are in collision.

WIP: Needs tests.

Before creating a pull request

Document new methods and classes
Format code with make 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

codecov · 2017-10-09T20:03:21Z

Codecov Report

Merging #235 into master will not change coverage.
The diff coverage is 66.66%.

@@          Coverage Diff           @@
##           master    #235   +/-   ##
======================================
  Coverage    70.8%   70.8%           
======================================
  Files         172     172           
  Lines        5059    5059           
  Branches      802     802           
======================================
  Hits         3582    3582           
  Misses        993     993           
  Partials      484     484

Impacted Files	Coverage Δ
include/aikido/constraint/Testable.hpp	`100% <ø> (ø)`	⬆️
...lude/aikido/constraint/uniform/RnBoxConstraint.hpp	`100% <ø> (ø)`	⬆️
src/constraint/uniform/SE2BoxConstraint.cpp	`73.07% <ø> (ø)`	⬆️
src/constraint/Satisfied.cpp	`94.73% <ø> (ø)`	⬆️
...constraint/uniform/detail/RnBoxConstraint-impl.hpp	`69.07% <ø> (ø)`	⬆️
include/aikido/constraint/TestableIntersection.hpp	`100% <ø> (ø)`	⬆️
include/aikido/constraint/Satisfied.hpp	`100% <ø> (ø)`	⬆️
include/aikido/constraint/TSR.hpp	`100% <ø> (ø)`	⬆️
include/aikido/constraint/FrameTestable.hpp	`100% <ø> (ø)`	⬆️
...ude/aikido/constraint/CartesianProductTestable.hpp	`100% <ø> (ø)`	⬆️
... and 14 more

mkoval

This is a pretty gross way of returning the information. Instead, I suggest adding an optional output parameter to the base isSatisfied() interface that is populated with debugging information if provided.

This is a little tricky to implement because each implementation of Testable would populate different fields. Here is my best suggestion:

Add a base class for Testable diagnostic information that is printable, to enable parity with the functionality that you have here:
```
class TestableOutcome {
public:
  bool is_satisfied() const = 0;
  virtual std::string to_string() const = 0;
};
```

Modify isSatisfied() to have an optional TestableOutcome output parameter:

bool isSatisfied(const aikido::statespace::StateSpace::State* _state,
                 TestableOutcome* _outcome = nullptr);

Implement a concrete subclass of TestableOutcome for each Testable constraint, e.g.:

class NonCollidingOutcome : public virtual TestableOutcome {
  // implementation of is_satisfied() and to_string()
}

Add a pure virtual overload of isSatisfied() to the Testable base class to make it possible to call (2) without knowing the concrete type of TestableOutcome that must be passed in as the output parameter:
```
bool isSatisfied(const aikido::statespace::StateSpace::State* _state,
                 std::unique_ptr<TestableOutcome>* _outcome = nullptr);
```

Given this architecture, this is how you would print the debugging information that you have today:

NonCollidingOutcome outcome;
noncolliding_testable.isSatisfied(state, &outcome);
std::cout << outcome.to_string() << std::endl;

If you don't know the type of Testable at compile time, you could use (4) to do this:

std::unique_ptr<TestableOutcome> outcome;
testable.isSatisfied(state, &outcome);
std::cout << outcome->to_string() << std::endl;

The key advantage of this API is that it provides programmatic access to the information that is currently printed to stdout. This is critical to implement good backtracking strategies for multi-step planning algorithms like MAGI.

mkoval · 2017-10-10T14:39:13Z

include/aikido/constraint/Testable.hpp

@@ -14,8 +14,10 @@ class Testable
  virtual ~Testable() = default;

  /// Returns true if state satisfies this constraint.
+  /// \param _givenState: true if state is user-provided


What does this mean? Isn't _state always user provided? 😕

Nit: We don't use : (semicolon) for a parameter docstring.

I guess _state is set to false when the checking state is provided by a planner, and, in that case, we don't want to terminate the planning due (could reject the sample though).

I think we don't need _givenState parameter for enforcing the planner to terminate. It's actually done by the caller (see this). _givenState is just using to determine whether a concrete Testable prints.

I agree with @mkoval that changing the way of receiving debug information.

aditya-vk · 2017-10-10T15:22:10Z

Great suggestions! Loved the point about the programmatic access.
Yeah, this is quite messy but was a quick hack-around to unblock some of the planning issues we were facing with pantry-loading demo (for instance, planner returns an invalid start state due to fingers colliding with each other when the hand closes around the glass but we had no access to this information previously). Created a PR so this issue can be tracked and completed appropriately soon. I should've probably left it as WIP.

jslee02

There are many "unused parameter" warnings due to _givenState. Currently, Travis CI doesn't test the working branch so it couldn't catch this. Please double check if all the warnings and errors (if any) on a local computer.

jslee02 · 2017-10-12T01:02:18Z

include/aikido/planner/ompl/detail/Planner-impl.hpp

@@ -24,6 +24,11 @@ trajectory::InterpolatedPtr planOMPL(
    double _maxPlanTime,
    double _maxDistanceBtwValidityChecks)
 {
+
