Testable Constraint Output (#266)

* Added TestableOutcomeClass.

* Added NonCollidingOutcome.hpp

* Include TestableOutcome in Testable. Fix TestableOutcome to make it work.

* Updated Testable isSatisfied signature and changed *bunch* of signatures in derived classes. [TODO]: Figure out the unused param warnings.

* Changed NonCollidingOutcome name to reflect CollisionFree name change. Included it in CollisionFree.

* Started CollisionFreeOutcome.cpp. Small changed to make it build once it was added as a source file.

* Wrote toString for CollisionFreeOutcome.

* Made CollisionFree fill _outcome on a collision (if not nullptr).

* Fixed test files to make tests build. Will run make format in just a sec!

* Added testing for CollisionFreeOutcome.

* Use move semantics in CollisionFreeOutcome toString method.

* Ran make format.

* Adressed nits from @jslee02. Moving to squish those unused param warnings next.

* Silence unused param warning, ran make format again.

* Used dynamic_cast in CollisionFree when populating outcome fields. Barf if type mismatch.

* Modify CollisionFreeOutcome to take Contact references as inputs as per feedback from @brianhou. Added a method to get the name from a CollisionObject.

* Move include of sstream into .cpp file instead of header.

* Made tests take advantage of programatic acess to data. Required adding a getter for Contact vectors in CollisionFreeOutcome and making getCollisionObjectName public.

* Made getter methods for Contact vectors const because they can be.

* Address nits from @jslee02.

* Two small nits I missed in the last commit.

* Aaaaaand, one more!

* Implemented createOutcome functionality in Testable base class.

* Use auto collisionOutcome as per request of @brianhou.

* Moved dynamic cast for outcome in CollisionFree.

* Added method to clear CollisionFreeOutcome. Use this method in CollisionFree so outcome objects can be reused. Also added stuff to test to make sure outcomes can actually be reused.

* Fixed directory placement of outcome classes. Renamed DummyOutcome ---> DefaultOutcome.

* Added simple functionality to DefaultOutcome.

* Responded to most recent review comments by making every Testable derivative class implement createOutcome (createOutcome in Testable is now a pure virtual function). Also made each implementation of isSatisfied in any Testable derivative class populate the isSatisfied field of DefaultOutcome.

* Run make format.

* [WIP] Refactor castic logic into helper, and use this in CollisionFree. Will stamp this into every other class now.

* [WIP] Replace redundant casting login in different Testables with helper.

* [WIP] Mop up some extra casts that were missed prior.

* Added documentation.

* Make CollisionFree a friend class of CollisionFreeOutcome, use Eigen alocator.

* Modify FrameTestable to "pass through" outcome object to mPoseConstraint.

* Rename DefaultOutcome ---> DefaultTestableOutcome. Final clean up as well.

* Address @mkoval's comments.

* Update nits: comment and renamed dynamic_cast_if_present to dynamic_cast_or_throw.

* Undo the use of the Eigen allocator in CollisionFreeOutcome.

* Update CHANGELOG.md

CHANGELOG.md

@@ -9,6 +9,7 @@
 
   * Added methods for removing groups from NonColliding constraints: [#247](https://github.com/personalrobotics/aikido/pull/247)
   * Renamed NonColliding to CollisionFree: [#256](https://github.com/personalrobotics/aikido/pull/256)
+  * Added TestableOutcome class: [#266](https://github.com/personalrobotics/aikido/pull/266)
 
 * Perception
 

include/aikido/constraint/CartesianProductTestable.hpp

@@ -26,7 +26,12 @@ class CartesianProductTestable : public Testable
   statespace::StateSpacePtr getStateSpace() const override;
 
   bool isSatisfied(
-      const aikido::statespace::StateSpace::State* _state) const override;
+      const aikido::statespace::StateSpace::State* _state,
+      TestableOutcome* outcome = nullptr) const override;
+
+  /// Return an instance of DefaultTestableOutcome, since this class doesn't
+  /// have a more specialized TestableOutcome derivative assigned to it.
+  std::unique_ptr<TestableOutcome> createOutcome() const override;
 
 private:
   std::shared_ptr<statespace::CartesianProduct> mStateSpace;

include/aikido/constraint/CollisionFree.hpp

@@ -9,6 +9,7 @@
 #include <dart/collision/CollisionGroup.hpp>
 #include <dart/collision/CollisionOption.hpp>
 #include "../statespace/dart/MetaSkeletonStateSpace.hpp"
+#include "CollisionFreeOutcome.hpp"
 #include "Testable.hpp"
 
 namespace aikido {
@@ -39,9 +40,17 @@ class CollisionFree : public Testable
   // Documentation inherited.
   statespace::StateSpacePtr getStateSpace() const override;
 
-  // Documentation inherited.
+  /// \copydoc Testable::isSatisfied()
+  /// \note Outcome is expected to be an instance of CollisionFreeOutcome.
+  /// This method will cast outcome to a pointer of this type, and then populate
+  /// the collision information (which bodies are in self/pairwise collision).
   bool isSatisfied(
-      const aikido::statespace::StateSpace::State* _state) const override;
+      const aikido::statespace::StateSpace::State* _state,
+      TestableOutcome* outcome = nullptr) const override;
+
+  /// \copydoc Testable::createOutcome()
+  /// \note Returns an instance of CollisionFreeOutcome.
+  std::unique_ptr<TestableOutcome> createOutcome() const override;
 
   /// Checks collision between group1 and group2.
   /// \param group1 First collision group.

include/aikido/constraint/CollisionFreeOutcome.hpp

@@ -0,0 +1,61 @@
+#ifndef AIKIDO_CONSTRAINT_COLLISIONFREEOUTCOME_HPP_
+#define AIKIDO_CONSTRAINT_COLLISIONFREEOUTCOME_HPP_
+
+#include <vector>
+#include "TestableOutcome.hpp"
+#include "dart/collision/CollisionObject.hpp"
+#include "dart/collision/Contact.hpp"
+#include "dart/dynamics/BodyNode.hpp"
+#include "dart/dynamics/ShapeFrame.hpp"
+#include "dart/dynamics/ShapeNode.hpp"
+
+namespace aikido {
+namespace constraint {
+
+/// TestableOutcome derivative class intended as (optional) input to isSatisfied
+/// method in CollisionFree class.
+class CollisionFreeOutcome : public TestableOutcome
+{
+  friend class CollisionFree;
+
+public:
+  /// Documentation inherited.
+  bool isSatisfied() const override;
+
+  /// Returns a string with each pair of CollisionObject names on a separate
+  /// line. Each pair is also marked as being a normal collision or self
+  /// collision.
+  std::string toString() const override;
+
+  /// Clears this outcome object. Useful in the event that a CollisionFree
+  /// object is passed to the isSatisfied() method of more than one constraint
+  /// object.
+  void clear();
+
+  /// Return a copy of the vector storing the Contact objects from pairwise
+  /// collisions.
+  std::vector<dart::collision::Contact> getPairwiseContacts() const;
+
+  /// Return a copy of the vector storing the Contact objects from self
+  /// collisions.
+  std::vector<dart::collision::Contact> getSelfContacts() const;
+
+  /// Gets the name of a CollisionObject. The name returned is that of the
+  /// corresponding BodyNode (if possible). If not, the name of the ShapeFrame
+  /// is returned instead. This is a helper for toString().
+  /// \param[in] object object pointer to CollisionObject we want the name of.
+  std::string getCollisionObjectName(
+      const dart::collision::CollisionObject* object) const;
+
+protected:
+  /// Holds Contact objects from pairwise collisions.
+  std::vector<dart::collision::Contact> mPairwiseContacts;
+
+  /// Holds Contact objects from self collisions.
+  std::vector<dart::collision::Contact> mSelfContacts;
+};
+
+} // namespace constraint
+} // namespace aikido
+
+#endif // AIKIDO_CONSTRAINT_COLLISIONFREEOUTCOME_HPP_

include/aikido/constraint/DefaultTestableOutcome.hpp

@@ -0,0 +1,35 @@
+#ifndef AIKIDO_CONSTRAINT_DEFAULTTESTABLEOUTCOME_HPP_
+#define AIKIDO_CONSTRAINT_DEFAULTTESTABLEOUTCOME_HPP_
+
+#include "TestableOutcome.hpp"
+
+namespace aikido {
+namespace constraint {
+
+/// Simple default TestableOutcome derivative class. An instance of this class
+/// is returned when createOutcome() is called on an instance of a class that
+/// inherits Testable, but has no corresponding TestableOutcome derivative
+/// implemented.
+class DefaultTestableOutcome : public TestableOutcome
+{
+public:
+  /// Returns whether the isSatisfied method this object was passed to
+  /// returned true or false.
+  bool isSatisfied() const override;
+
+  /// String representation of isSatisfied return value.
+  std::string toString() const override;
+
+  /// Used by the isSatisfied this outcome object is passed to set whether the
+  /// constraint was satisifed or not.
+  /// \param[in] satisfiedFlag whether the constraint was satisfied or not.
+  void setSatisfiedFlag(bool satisfiedFlag);
+
+protected:
+  bool mSatisfiedFlag;
+};
+
+} // namespace constraint
+} // namespace aikido
+
+#endif // AIKIDO_CONSTRAINT_DEFAULTTESTABLEOUTCOME_HPP_

include/aikido/constraint/FrameTestable.hpp

@@ -29,7 +29,17 @@ class FrameTestable : public Testable
   /// \param _state a MetaskeletonState to set the configuration of this
   ///        constraint's metaskeketon. This state's StateSpace should match
   ///        StateSpace returend by getStateSpace().
-  bool isSatisfied(const statespace::StateSpace::State* _state) const override;
+  /// \param outcome Testable outcome derivative class. Passed to the
+  ///        isSatisfied method of mPoseConstraint to allow "pass through" of
+  ///        debugging information.
+  bool isSatisfied(
+      const statespace::StateSpace::State* _state,
+      TestableOutcome* outcome = nullptr) const override;
+
+  /// Return the outcome of mPoseConstraint->createOutcome(). Reason:
+  /// isSatisfied in this class will just pass outcome to the isSatisfied
+  /// method of of mPoseConstraint.
+  std::unique_ptr<TestableOutcome> createOutcome() const override;
 
   // Documentation inhereted
   std::shared_ptr<statespace::StateSpace> getStateSpace() const override;

include/aikido/constraint/Satisfied.hpp

@@ -34,7 +34,13 @@ class Satisfied : public constraint::Differentiable,
   /// Returns \c true.
   ///
   /// \param state a state in \c getStateSpace()
-  bool isSatisfied(const statespace::StateSpace::State* state) const override;
+  bool isSatisfied(
+      const statespace::StateSpace::State* state,
+      TestableOutcome* outcome = nullptr) const override;
+
+  /// Return an instance of DefaultTestableOutcome, since this class doesn't
+  /// have a more specialized TestableOutcome derivative assigned to it.
+  std::unique_ptr<TestableOutcome> createOutcome() const override;
 
   /// Sets \c _out to \c _s.
   ///

include/aikido/constraint/TSR.hpp

@@ -80,7 +80,13 @@ class TSR : public Sampleable,
   std::unique_ptr<SampleGenerator> createSampleGenerator() const override;
 
   // Documentation inherited.
-  bool isSatisfied(const statespace::StateSpace::State* _s) const override;
+  bool isSatisfied(
+      const statespace::StateSpace::State* _s,
+      TestableOutcome* outcome = nullptr) const override;
+
+  /// Return an instance of DefaultTestableOutcome, since this class doesn't
+  /// have a more specialized TestableOutcome derivative assigned to it.
+  std::unique_ptr<TestableOutcome> createOutcome() const override;
 
   /// Throws an invalid_argument exception if this TSR is invalid.
   /// For a TSR to be valid, mBw(i, 0) <= mBw(i, 1).

include/aikido/constraint/Testable.hpp

@@ -3,27 +3,42 @@
 
 #include <memory>
 #include "../statespace/StateSpace.hpp"
+#include "DefaultTestableOutcome.hpp"
 
 namespace aikido {
 namespace constraint {
 
+class TestableOutcome;
+
 /// Constraint which can be tested.
 class Testable
 {
 public:
   virtual ~Testable() = default;
 
   /// Returns true if state satisfies this constraint.
+  /// \param[in] _state given state to test.
+  /// \param[in] outcome pointer to TestableOutcome derivative instance that
+  /// method will populate with useful information. Each derivative class of
+  /// Testable may expect outcome to be a different derivative class of
+  /// TestableOutcome (this casting and population is done under the hood). If
+  /// this argument is missing, it is ignored.
   virtual bool isSatisfied(
-      const statespace::StateSpace::State* _state) const = 0;
+      const statespace::StateSpace::State* _state,
+      TestableOutcome* outcome = nullptr) const = 0;
 
   /// Returns StateSpace in which this constraint operates.
   virtual statespace::StateSpacePtr getStateSpace() const = 0;
+
+  /// Return an instance of a TestableOutcome derivative class that corresponds
+  /// to this constraint class. Ensures that correct outcome object is passed
+  /// to isSatisfied (and casts, etc do not explode).
+  virtual std::unique_ptr<TestableOutcome> createOutcome() const = 0;
 };
 
 using TestablePtr = std::shared_ptr<Testable>;
 
 } // namespace constraint
 } // namespace aikido
 
-#endif // AIKIDO_CONSTRAINT_TESTABLE_HPP_
+#endif // AIKIDO_CONSTRAINT_TESTABLE_HPP_

include/aikido/constraint/TestableIntersection.hpp

@@ -23,7 +23,12 @@ class TestableIntersection : public Testable
 
   // Documentation inherited.
   bool isSatisfied(
-      const aikido::statespace::StateSpace::State* state) const override;
+      const aikido::statespace::StateSpace::State* state,
+      TestableOutcome* outcome = nullptr) const override;
+
+  /// Return an instance of DefaultTestableOutcome, since this class doesn't
+  /// have a more specialized TestableOutcome derivative assigned to it.
+  std::unique_ptr<TestableOutcome> createOutcome() const override;
 
   // Documentation inherited.
   statespace::StateSpacePtr getStateSpace() const override;

include/aikido/constraint/TestableOutcome.hpp

@@ -0,0 +1,36 @@
+#ifndef AIKIDO_CONSTRAINT_TESTABLEOUTCOME_HPP_
+#define AIKIDO_CONSTRAINT_TESTABLEOUTCOME_HPP_
+
+#include <string>
+
+namespace aikido {
+namespace constraint {
+
+/// Base class for constraint outcomes. At a high level, each constraint can
+/// have a corresponding derivative of TestableOutcome that is passed as an
+/// optional parameter to its isSatisfied method. This allow programmatic access
+/// to data on why the constraint was (or was not) satisfied.
+class TestableOutcome
+{
+public:
+  /// Returns true if isSatisfied call this outcome object was passed to
+  /// returned true. False otherwise.
+  virtual bool isSatisfied() const = 0;
+
+  /// String representation of the outcome. Provides useful, programmatic
+  /// access to debug information.
+  virtual std::string toString() const = 0;
+};
+
+/// Helper function. Avoids repeating logic for casting TestableOutcome
+/// pointers down to pointers for a derivative class. Mostly used in the
+/// isSatisfied methods of classes that inherit Testable.
+template <class Child>
+Child* dynamic_cast_or_throw(TestableOutcome* outcome);
+
+} // namespace constraint
+} // namespace aikido
+
+#include "detail/TestableOutcome-impl.hpp"
+
+#endif // AIKIDO_CONSTRAINT_TESTABLEOUTCOME_HPP_

include/aikido/constraint/detail/TestableOutcome-impl.hpp

@@ -0,0 +1,28 @@
+#include <sstream>
+#include <stdexcept>
+#include <typeinfo>
+
+namespace aikido {
+namespace constraint {
+
+//==============================================================================
+template <class Child>
+Child* dynamic_cast_or_throw(TestableOutcome* outcome)
+{
+  if (!outcome)
+    return nullptr;
+
+  auto childPtr = dynamic_cast<Child*>(outcome);
+  if (!childPtr)
+  {
+    std::stringstream message;
+    message << "TestableOutcome pointer is not of type " << typeid(Child).name()
+            << ".";
+    throw std::invalid_argument(message.str());
+  }
+
+  return childPtr;
+}
+
+} // namespace constraint
+} // namespace aikido

include/aikido/constraint/uniform/RnBoxConstraint.hpp

@@ -46,7 +46,13 @@ class RBoxConstraint : public constraint::Differentiable,
   std::vector<constraint::ConstraintType> getConstraintTypes() const override;
 
   // Documentation inherited.
-  bool isSatisfied(const statespace::StateSpace::State* state) const override;
+  bool isSatisfied(
+      const statespace::StateSpace::State* state,
+      TestableOutcome* outcome = nullptr) const override;
+
+  /// Return an instance of DefaultTestableOutcome, since this class doesn't
+  /// have a more specialized TestableOutcome derivative assigned to it.
+  std::unique_ptr<TestableOutcome> createOutcome() const override;
 
   // Documentation inherited.
   bool project(

include/aikido/constraint/uniform/SE2BoxConstraint.hpp

@@ -35,8 +35,14 @@ class SE2BoxConstraint : public constraint::Projectable,
   // Documentation inherited.
   statespace::StateSpacePtr getStateSpace() const override;
 
-  // Documentation inherited.
-  bool isSatisfied(const statespace::StateSpace::State* state) const override;
+  /// Documentation inherited.
+  bool isSatisfied(
+      const statespace::StateSpace::State* state,
+      TestableOutcome* outcome = nullptr) const override;
+
+  /// Return an instance of DefaultTestableOutcome, since this class doesn't
+  /// have a more specialized TestableOutcome derivative assigned to it.
+  std::unique_ptr<TestableOutcome> createOutcome() const override;
 
   // Documentation inherited.
   bool project(

include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp

@@ -175,19 +175,36 @@ std::vector<ConstraintType> RBoxConstraint<N>::getConstraintTypes() const
 //==============================================================================
 template <int N>
 bool RBoxConstraint<N>::isSatisfied(
-    const statespace::StateSpace::State* state) const
+    const statespace::StateSpace::State* state, TestableOutcome* outcome) const
 {
+  auto defaultOutcomeObject
+      = dynamic_cast_or_throw<DefaultTestableOutcome>(outcome);
+
   const auto value = mSpace->getValue(
       static_cast<const typename statespace::R<N>::State*>(state));
 
   for (auto i = 0; i < value.size(); ++i)
   {
     if (value[i] < mLowerLimits[i] || value[i] > mUpperLimits[i])
+    {
+      if (defaultOutcomeObject)
+        defaultOutcomeObject->setSatisfiedFlag(false);
       return false;
+    }
   }
+
+  if (defaultOutcomeObject)
+    defaultOutcomeObject->setSatisfiedFlag(true);
   return true;
 }
 
+//==============================================================================
+template <int N>
+std::unique_ptr<TestableOutcome> RBoxConstraint<N>::createOutcome() const
+{
+  return std::unique_ptr<TestableOutcome>(new DefaultTestableOutcome);
+}
+
 //==============================================================================
 template <int N>
 bool RBoxConstraint<N>::project(