From 3fcb61f5474765751e7540884b5027e10489b1fa Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Sat, 25 Nov 2017 22:00:47 -0500 Subject: [PATCH 01/41] Added TestableOutcomeClass. --- .../constraint/outcome/TestableOutcome.hpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) create mode 100644 include/aikido/constraint/outcome/TestableOutcome.hpp diff --git a/include/aikido/constraint/outcome/TestableOutcome.hpp b/include/aikido/constraint/outcome/TestableOutcome.hpp new file mode 100644 index 0000000000..83ad4fc81c --- /dev/null +++ b/include/aikido/constraint/outcome/TestableOutcome.hpp @@ -0,0 +1,16 @@ +#ifndef AIKIDO_CONSTRAINT_TESTABLEOUTCOME_HPP_ +#define AIKIDO_CONSTRAINT_TESTABLEOUTCOME_HPP_ + +namespace aikido { +namespace constraint { + +class TestableOutcome { +public: + bool is_satisfied() const = 0; + virtual std::string to_string() const = 0; +}; + +} // namespace constraint +} // namespace aikido + +#endif // ifndef AIKIDO_CONSTRAINT_TESTABLEOUTCOME_HPP_ From 46ed3d8ddc454ea539a2cb0cfc35d3e8463cf53a Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Sat, 25 Nov 2017 22:11:11 -0500 Subject: [PATCH 02/41] Added NonCollidingOutcome.hpp --- .../constraint/outcome/NonCollidingOutcome.hpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) create mode 100644 include/aikido/constraint/outcome/NonCollidingOutcome.hpp diff --git a/include/aikido/constraint/outcome/NonCollidingOutcome.hpp b/include/aikido/constraint/outcome/NonCollidingOutcome.hpp new file mode 100644 index 0000000000..aba04c4ff9 --- /dev/null +++ b/include/aikido/constraint/outcome/NonCollidingOutcome.hpp @@ -0,0 +1,18 @@ +#ifndef AIKIDO_CONSTRAINT_NONCOLLIDING_HPP_ +#define AIKIDO_CONSTRAINT_NONCOLLIDING_HPP_ + +#include "TestableOutcome.hpp" + +namespace aikido { +namespace constraint { + +class NonCollidingOutcome : public TestableOutcome { +public: + bool is_satisfied() const; + std::string to_string() const; +}; + +} // namespace constraint +} // namespace aikido + +#endif // ifndef AIKIDO_CONSTRAINT_NONCOLLIDING_HPP_ From 4dfa90066a64e4db2f6ef622f34db2bf9ea62213 Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Sat, 25 Nov 2017 22:23:57 -0500 Subject: [PATCH 03/41] Include TestableOutcome in Testable. Fix TestableOutcome to make it work. --- include/aikido/constraint/Testable.hpp | 1 + include/aikido/constraint/outcome/TestableOutcome.hpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/include/aikido/constraint/Testable.hpp b/include/aikido/constraint/Testable.hpp index 2c04f8017a..e97f1f06ad 100644 --- a/include/aikido/constraint/Testable.hpp +++ b/include/aikido/constraint/Testable.hpp @@ -3,6 +3,7 @@ #include #include "../statespace/StateSpace.hpp" +#include "outcome/TestableOutcome.hpp" namespace aikido { namespace constraint { diff --git a/include/aikido/constraint/outcome/TestableOutcome.hpp b/include/aikido/constraint/outcome/TestableOutcome.hpp index 83ad4fc81c..4fac0ce3b1 100644 --- a/include/aikido/constraint/outcome/TestableOutcome.hpp +++ b/include/aikido/constraint/outcome/TestableOutcome.hpp @@ -6,7 +6,7 @@ namespace constraint { class TestableOutcome { public: - bool is_satisfied() const = 0; + virtual bool is_satisfied() const = 0; virtual std::string to_string() const = 0; }; From 9368a145145939ff963340a65fc4bab99775b3e5 Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Sat, 25 Nov 2017 22:56:18 -0500 Subject: [PATCH 04/41] Updated Testable isSatisfied signature and changed *bunch* of signatures in derived classes. [TODO]: Figure out the unused param warnings. --- include/aikido/constraint/CartesianProductTestable.hpp | 3 ++- include/aikido/constraint/CollisionFree.hpp | 3 ++- include/aikido/constraint/FrameTestable.hpp | 4 +++- include/aikido/constraint/Satisfied.hpp | 4 +++- include/aikido/constraint/TSR.hpp | 4 +++- include/aikido/constraint/Testable.hpp | 3 ++- include/aikido/constraint/TestableIntersection.hpp | 3 ++- include/aikido/constraint/uniform/RnBoxConstraint.hpp | 4 +++- include/aikido/constraint/uniform/SE2BoxConstraint.hpp | 4 +++- .../constraint/uniform/detail/RnBoxConstraint-impl.hpp | 3 ++- src/constraint/CartesianProductTestable.cpp | 3 ++- src/constraint/CollisionFree.cpp | 3 ++- src/constraint/FrameTestable.cpp | 3 ++- src/constraint/Satisfied.cpp | 3 ++- src/constraint/TSR.cpp | 4 +++- src/constraint/TestableIntersection.cpp | 3 ++- src/constraint/uniform/SE2BoxConstraint.cpp | 5 +++-- 17 files changed, 41 insertions(+), 18 deletions(-) diff --git a/include/aikido/constraint/CartesianProductTestable.hpp b/include/aikido/constraint/CartesianProductTestable.hpp index b8159c9028..2ad888b83c 100644 --- a/include/aikido/constraint/CartesianProductTestable.hpp +++ b/include/aikido/constraint/CartesianProductTestable.hpp @@ -26,7 +26,8 @@ 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; private: std::shared_ptr mStateSpace; diff --git a/include/aikido/constraint/CollisionFree.hpp b/include/aikido/constraint/CollisionFree.hpp index 67805dbbc9..801f18be17 100644 --- a/include/aikido/constraint/CollisionFree.hpp +++ b/include/aikido/constraint/CollisionFree.hpp @@ -41,7 +41,8 @@ class CollisionFree : 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; /// Checks collision between group1 and group2. /// \param group1 First collision group. diff --git a/include/aikido/constraint/FrameTestable.hpp b/include/aikido/constraint/FrameTestable.hpp index c0c5992165..5b2a0c0afa 100644 --- a/include/aikido/constraint/FrameTestable.hpp +++ b/include/aikido/constraint/FrameTestable.hpp @@ -29,7 +29,9 @@ 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; + bool isSatisfied( + const statespace::StateSpace::State* _state, + TestableOutcome* _outcome = nullptr) const override; // Documentation inhereted std::shared_ptr getStateSpace() const override; diff --git a/include/aikido/constraint/Satisfied.hpp b/include/aikido/constraint/Satisfied.hpp index 2ec3705f52..cc06fdc656 100644 --- a/include/aikido/constraint/Satisfied.hpp +++ b/include/aikido/constraint/Satisfied.hpp @@ -34,7 +34,9 @@ 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; /// Sets \c _out to \c _s. /// diff --git a/include/aikido/constraint/TSR.hpp b/include/aikido/constraint/TSR.hpp index 67f06e8521..2b7eda2d20 100644 --- a/include/aikido/constraint/TSR.hpp +++ b/include/aikido/constraint/TSR.hpp @@ -80,7 +80,9 @@ class TSR : public Sampleable, std::unique_ptr 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; /// Throws an invalid_argument exception if this TSR is invalid. /// For a TSR to be valid, mBw(i, 0) <= mBw(i, 1). diff --git a/include/aikido/constraint/Testable.hpp b/include/aikido/constraint/Testable.hpp index e97f1f06ad..31af371e9d 100644 --- a/include/aikido/constraint/Testable.hpp +++ b/include/aikido/constraint/Testable.hpp @@ -16,7 +16,8 @@ class Testable /// Returns true if state satisfies this constraint. 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; diff --git a/include/aikido/constraint/TestableIntersection.hpp b/include/aikido/constraint/TestableIntersection.hpp index bf91d0f670..f704163364 100644 --- a/include/aikido/constraint/TestableIntersection.hpp +++ b/include/aikido/constraint/TestableIntersection.hpp @@ -23,7 +23,8 @@ 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; // Documentation inherited. statespace::StateSpacePtr getStateSpace() const override; diff --git a/include/aikido/constraint/uniform/RnBoxConstraint.hpp b/include/aikido/constraint/uniform/RnBoxConstraint.hpp index 426cd6b53a..bc72af20a7 100644 --- a/include/aikido/constraint/uniform/RnBoxConstraint.hpp +++ b/include/aikido/constraint/uniform/RnBoxConstraint.hpp @@ -46,7 +46,9 @@ class RBoxConstraint : public constraint::Differentiable, std::vector 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; // Documentation inherited. bool project( diff --git a/include/aikido/constraint/uniform/SE2BoxConstraint.hpp b/include/aikido/constraint/uniform/SE2BoxConstraint.hpp index 283d2ac62c..e97cb92ac6 100644 --- a/include/aikido/constraint/uniform/SE2BoxConstraint.hpp +++ b/include/aikido/constraint/uniform/SE2BoxConstraint.hpp @@ -36,7 +36,9 @@ class SE2BoxConstraint : public constraint::Projectable, statespace::StateSpacePtr getStateSpace() 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; // Documentation inherited. bool project( diff --git a/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp b/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp index e4f7d07b1f..69dd2fb25d 100644 --- a/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp +++ b/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp @@ -175,7 +175,8 @@ std::vector RBoxConstraint::getConstraintTypes() const //============================================================================== template bool RBoxConstraint::isSatisfied( - const statespace::StateSpace::State* state) const + const statespace::StateSpace::State* state, + TestableOutcome* _outcome) const { const auto value = mSpace->getValue( static_cast::State*>(state)); diff --git a/src/constraint/CartesianProductTestable.cpp b/src/constraint/CartesianProductTestable.cpp index 427eb9c2bd..536b1ecbd6 100644 --- a/src/constraint/CartesianProductTestable.cpp +++ b/src/constraint/CartesianProductTestable.cpp @@ -53,7 +53,8 @@ statespace::StateSpacePtr CartesianProductTestable::getStateSpace() const //============================================================================== bool CartesianProductTestable::isSatisfied( - const aikido::statespace::StateSpace::State* _state) const + const aikido::statespace::StateSpace::State* _state, + TestableOutcome* _outcome) const { const auto state = static_cast(_state); diff --git a/src/constraint/CollisionFree.cpp b/src/constraint/CollisionFree.cpp index 060596a698..64e4a525d9 100644 --- a/src/constraint/CollisionFree.cpp +++ b/src/constraint/CollisionFree.cpp @@ -27,7 +27,8 @@ statespace::StateSpacePtr CollisionFree::getStateSpace() const //============================================================================== bool CollisionFree::isSatisfied( - const aikido::statespace::StateSpace::State* _state) const + const aikido::statespace::StateSpace::State* _state, + TestableOutcome* _outcome) const { auto skelStatePtr = static_cast(_state); diff --git a/src/constraint/FrameTestable.cpp b/src/constraint/FrameTestable.cpp index b0177ca4af..c195007317 100644 --- a/src/constraint/FrameTestable.cpp +++ b/src/constraint/FrameTestable.cpp @@ -34,7 +34,8 @@ FrameTestable::FrameTestable( //============================================================================== bool FrameTestable::isSatisfied( - const statespace::StateSpace::State* _state) const + const statespace::StateSpace::State* _state, + TestableOutcome* _outcome) const { // Set the state auto state diff --git a/src/constraint/Satisfied.cpp b/src/constraint/Satisfied.cpp index 29a6eb3e88..b8c810e2b6 100644 --- a/src/constraint/Satisfied.cpp +++ b/src/constraint/Satisfied.cpp @@ -31,7 +31,8 @@ std::vector Satisfied::getConstraintTypes() const //============================================================================== bool Satisfied::isSatisfied( - const statespace::StateSpace::State* /*state*/) const + const statespace::StateSpace::State* /*state*/, + TestableOutcome* /*_outcome*/) const { return true; } diff --git a/src/constraint/TSR.cpp b/src/constraint/TSR.cpp index 373124c6cc..e64d4f33cd 100644 --- a/src/constraint/TSR.cpp +++ b/src/constraint/TSR.cpp @@ -210,7 +210,9 @@ std::unique_ptr TSR::createSampleGenerator() const } //============================================================================== -bool TSR::isSatisfied(const statespace::StateSpace::State* _s) const +bool TSR::isSatisfied( + const statespace::StateSpace::State* _s, + TestableOutcome* _outcome) const { Eigen::VectorXd dist; getValue(_s, dist); diff --git a/src/constraint/TestableIntersection.cpp b/src/constraint/TestableIntersection.cpp index ccf0c9efb6..1c00961f9e 100644 --- a/src/constraint/TestableIntersection.cpp +++ b/src/constraint/TestableIntersection.cpp @@ -20,7 +20,8 @@ TestableIntersection::TestableIntersection( //============================================================================== bool TestableIntersection::isSatisfied( - const aikido::statespace::StateSpace::State* _state) const + const aikido::statespace::StateSpace::State* _state, + TestableOutcome* _outcome) const { for (auto c : mConstraints) { diff --git a/src/constraint/uniform/SE2BoxConstraint.cpp b/src/constraint/uniform/SE2BoxConstraint.cpp index 423ac5e577..d4e5fc2fba 100644 --- a/src/constraint/uniform/SE2BoxConstraint.cpp +++ b/src/constraint/uniform/SE2BoxConstraint.cpp @@ -137,7 +137,8 @@ statespace::StateSpacePtr SE2BoxConstraint::getStateSpace() const //============================================================================== bool SE2BoxConstraint::isSatisfied( - const statespace::StateSpace::State* state) const + const statespace::StateSpace::State* state, + TestableOutcome* _outcome) const { Eigen::VectorXd tangent; mSpace->logMap(static_cast(state), tangent); @@ -207,4 +208,4 @@ Eigen::Vector2d SE2BoxConstraint::getUpperLimits() const } } // namespace statespace -} // namespace aikido \ No newline at end of file +} // namespace aikido From 235e0daf4f64fc73653110616e0824e5e41e2c38 Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Sat, 25 Nov 2017 23:05:01 -0500 Subject: [PATCH 05/41] Changed NonCollidingOutcome name to reflect CollisionFree name change. Included it in CollisionFree. --- include/aikido/constraint/CollisionFree.hpp | 1 + .../outcome/CollisionFreeOutcome.hpp | 18 ++++++++++++++++++ .../constraint/outcome/NonCollidingOutcome.hpp | 18 ------------------ 3 files changed, 19 insertions(+), 18 deletions(-) create mode 100644 include/aikido/constraint/outcome/CollisionFreeOutcome.hpp delete mode 100644 include/aikido/constraint/outcome/NonCollidingOutcome.hpp diff --git a/include/aikido/constraint/CollisionFree.hpp b/include/aikido/constraint/CollisionFree.hpp index 801f18be17..6d12ad1d1f 100644 --- a/include/aikido/constraint/CollisionFree.hpp +++ b/include/aikido/constraint/CollisionFree.hpp @@ -10,6 +10,7 @@ #include #include "../statespace/dart/MetaSkeletonStateSpace.hpp" #include "Testable.hpp" +#include "outcome/CollisionFreeOutcome.hpp" namespace aikido { namespace constraint { diff --git a/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp b/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp new file mode 100644 index 0000000000..8e417ac1e1 --- /dev/null +++ b/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp @@ -0,0 +1,18 @@ +#ifndef AIKIDO_CONSTRAINT_COLLISIONFREEOUTCOME_HPP_ +#define AIKIDO_CONSTRAINT_COLLISIONFREEOUTCOME_HPP_ + +#include "TestableOutcome.hpp" + +namespace aikido { +namespace constraint { + +class CollisionFreeOutcome : public TestableOutcome { +public: + bool is_satisfied() const; + std::string to_string() const; +}; + +} // namespace constraint +} // namespace aikido + +#endif // ifndef AIKIDO_CONSTRAINT_COLLISIONFREEOUTCOME_HPP_ diff --git a/include/aikido/constraint/outcome/NonCollidingOutcome.hpp b/include/aikido/constraint/outcome/NonCollidingOutcome.hpp deleted file mode 100644 index aba04c4ff9..0000000000 --- a/include/aikido/constraint/outcome/NonCollidingOutcome.hpp +++ /dev/null @@ -1,18 +0,0 @@ -#ifndef AIKIDO_CONSTRAINT_NONCOLLIDING_HPP_ -#define AIKIDO_CONSTRAINT_NONCOLLIDING_HPP_ - -#include "TestableOutcome.hpp" - -namespace aikido { -namespace constraint { - -class NonCollidingOutcome : public TestableOutcome { -public: - bool is_satisfied() const; - std::string to_string() const; -}; - -} // namespace constraint -} // namespace aikido - -#endif // ifndef AIKIDO_CONSTRAINT_NONCOLLIDING_HPP_ From 98ef1611c5be99746c560acfd8abd68386efb83e Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Sat, 25 Nov 2017 23:40:29 -0500 Subject: [PATCH 06/41] Started CollisionFreeOutcome.cpp. Small changed to make it build once it was added as a source file. --- .../outcome/CollisionFreeOutcome.hpp | 11 +++++- .../constraint/outcome/TestableOutcome.hpp | 6 ++- src/constraint/CMakeLists.txt | 1 + .../outcome/CollisionFreeOutcome.cpp | 39 +++++++++++++++++++ 4 files changed, 53 insertions(+), 4 deletions(-) create mode 100644 src/constraint/outcome/CollisionFreeOutcome.cpp diff --git a/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp b/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp index 8e417ac1e1..3fba22078d 100644 --- a/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp +++ b/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp @@ -1,6 +1,7 @@ #ifndef AIKIDO_CONSTRAINT_COLLISIONFREEOUTCOME_HPP_ #define AIKIDO_CONSTRAINT_COLLISIONFREEOUTCOME_HPP_ +#include #include "TestableOutcome.hpp" namespace aikido { @@ -8,8 +9,14 @@ namespace constraint { class CollisionFreeOutcome : public TestableOutcome { public: - bool is_satisfied() const; - std::string to_string() const; + bool isSatisfied() const; + std::string toString() const; + void markCollisionBodyNode(const std::string& bodyNodeName); + void markSelfCollisionBodyNode(const std::string& bodyNodeName); + +private: + std::vector mCollisionBodyNodes; + std::vector mSelfCollisionBodyNodes; }; } // namespace constraint diff --git a/include/aikido/constraint/outcome/TestableOutcome.hpp b/include/aikido/constraint/outcome/TestableOutcome.hpp index 4fac0ce3b1..b18a7b6987 100644 --- a/include/aikido/constraint/outcome/TestableOutcome.hpp +++ b/include/aikido/constraint/outcome/TestableOutcome.hpp @@ -1,13 +1,15 @@ #ifndef AIKIDO_CONSTRAINT_TESTABLEOUTCOME_HPP_ #define AIKIDO_CONSTRAINT_TESTABLEOUTCOME_HPP_ +#include + namespace aikido { namespace constraint { class TestableOutcome { public: - virtual bool is_satisfied() const = 0; - virtual std::string to_string() const = 0; + virtual bool isSatisfied() const = 0; + virtual std::string toString() const = 0; }; } // namespace constraint diff --git a/src/constraint/CMakeLists.txt b/src/constraint/CMakeLists.txt index 0ea7c2129d..cdc3555830 100644 --- a/src/constraint/CMakeLists.txt +++ b/src/constraint/CMakeLists.txt @@ -1,4 +1,5 @@ set(sources + outcome/CollisionFreeOutcome.cpp uniform/RnBoxConstraint.cpp uniform/RnConstantSampler.cpp uniform/SO2UniformSampler.cpp diff --git a/src/constraint/outcome/CollisionFreeOutcome.cpp b/src/constraint/outcome/CollisionFreeOutcome.cpp new file mode 100644 index 0000000000..fc5a2da077 --- /dev/null +++ b/src/constraint/outcome/CollisionFreeOutcome.cpp @@ -0,0 +1,39 @@ +#include + +namespace aikido { +namespace constraint { + +//============================================================================== +bool CollisionFreeOutcome::isSatisfied() const +{ + if (mCollisionBodyNodes.size() > 0 || mSelfCollisionBodyNodes.size() > 0) + { + return false; + } + + return true; +} + +//============================================================================== +std::string CollisionFreeOutcome::toString() const +{ + // TODO + return "TODO"; +} + +//============================================================================== +void CollisionFreeOutcome::markCollisionBodyNode( + const std::string& bodyNodeName) +{ + mCollisionBodyNodes.push_back(bodyNodeName); +} + +//============================================================================== +void CollisionFreeOutcome::markSelfCollisionBodyNode( + const std::string& bodyNodeName) +{ + mSelfCollisionBodyNodes.push_back(bodyNodeName); +} + +} // namespace constraint +} // namespace aikido From 9c36baec65ff6f3fdbbd15085f424151d6ee154b Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Sat, 25 Nov 2017 23:52:59 -0500 Subject: [PATCH 07/41] Wrote toString for CollisionFreeOutcome. --- .../constraint/outcome/CollisionFreeOutcome.hpp | 1 + src/constraint/outcome/CollisionFreeOutcome.cpp | 16 ++++++++++++++-- 2 files changed, 15 insertions(+), 2 deletions(-) diff --git a/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp b/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp index 3fba22078d..d4344769bd 100644 --- a/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp +++ b/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp @@ -1,6 +1,7 @@ #ifndef AIKIDO_CONSTRAINT_COLLISIONFREEOUTCOME_HPP_ #define AIKIDO_CONSTRAINT_COLLISIONFREEOUTCOME_HPP_ +#include #include #include "TestableOutcome.hpp" diff --git a/src/constraint/outcome/CollisionFreeOutcome.cpp b/src/constraint/outcome/CollisionFreeOutcome.cpp index fc5a2da077..dbc493f6d8 100644 --- a/src/constraint/outcome/CollisionFreeOutcome.cpp +++ b/src/constraint/outcome/CollisionFreeOutcome.cpp @@ -17,8 +17,20 @@ bool CollisionFreeOutcome::isSatisfied() const //============================================================================== std::string CollisionFreeOutcome::toString() const { - // TODO - return "TODO"; + std::stringstream ss; + ss << "ALL COLLISIONS: " << std::endl; + + for (auto collBodyNodeName : mCollisionBodyNodes) + { + ss << "[COLLISION]: " << collBodyNodeName << std::endl; + } + + for (auto selfCollBodyNodeName: mSelfCollisionBodyNodes) + { + ss << "[SELF COLLISION]: " << selfCollBodyNodeName << std::endl; + } + + return ss.str(); } //============================================================================== From 62af07bf26f781cc52e3e37e83adff62c50fa2a2 Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Sun, 26 Nov 2017 00:08:30 -0500 Subject: [PATCH 08/41] Made CollisionFree fill _outcome on a collision (if not nullptr). --- src/constraint/CollisionFree.cpp | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/src/constraint/CollisionFree.cpp b/src/constraint/CollisionFree.cpp index 64e4a525d9..754e15fe07 100644 --- a/src/constraint/CollisionFree.cpp +++ b/src/constraint/CollisionFree.cpp @@ -43,8 +43,20 @@ bool CollisionFree::isSatisfied( groups.second.get(), mCollisionOptions, &collisionResult); + if (collision) + { + if (_outcome != nullptr) + { + auto collisionOutcome = static_cast(_outcome); + auto collidingBodies = collisionResult.getCollidingBodyNodes(); + for (const auto& elem : collidingBodies) + { + collisionOutcome->markCollisionBodyNode(elem->getName()); + } + } return false; + } } for (auto group : mGroupsToSelfCheck) @@ -52,7 +64,18 @@ bool CollisionFree::isSatisfied( collision = mCollisionDetector->collide( group.get(), mCollisionOptions, &collisionResult); if (collision) + { + if (_outcome != nullptr) + { + auto collisionOutcome = static_cast(_outcome); + auto collidingBodies = collisionResult.getCollidingBodyNodes(); + for (const auto& elem : collidingBodies) + { + collisionOutcome->markSelfCollisionBodyNode(elem->getName()); + } + } return false; + } } return true; } From 081e23e550e4b25fd5aec373a463f26b29152bdb Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Mon, 27 Nov 2017 15:15:24 -0500 Subject: [PATCH 09/41] Fixed test files to make tests build. Will run make format in just a sec! --- tests/constraint/MockConstraints.hpp | 6 ++++-- tests/constraint/test_FrameTestable.cpp | 3 ++- tests/planner/ompl/OMPLTestHelpers.hpp | 6 ++++-- 3 files changed, 10 insertions(+), 5 deletions(-) diff --git a/tests/constraint/MockConstraints.hpp b/tests/constraint/MockConstraints.hpp index a070ead859..4e60c8a40f 100644 --- a/tests/constraint/MockConstraints.hpp +++ b/tests/constraint/MockConstraints.hpp @@ -14,7 +14,8 @@ class PassingConstraint : public aikido::constraint::Testable } bool isSatisfied( - const aikido::statespace::StateSpace::State* /*state*/) const override + const aikido::statespace::StateSpace::State* /*state*/, + aikido::constraint::TestableOutcome* = nullptr /*_outcome*/) const override { return true; } @@ -38,7 +39,8 @@ class FailingConstraint : public aikido::constraint::Testable } bool isSatisfied( - const aikido::statespace::StateSpace::State* /*state*/) const override + const aikido::statespace::StateSpace::State* /*state*/, + aikido::constraint::TestableOutcome* = nullptr /*_outcome*/) const override { return false; } diff --git a/tests/constraint/test_FrameTestable.cpp b/tests/constraint/test_FrameTestable.cpp index 38d5379659..7ca30a54aa 100644 --- a/tests/constraint/test_FrameTestable.cpp +++ b/tests/constraint/test_FrameTestable.cpp @@ -29,7 +29,8 @@ class EndEffectorTestable : public aikido::constraint::Testable } bool isSatisfied( - const aikido::statespace::StateSpace::State* _state) const override + const aikido::statespace::StateSpace::State* _state, + aikido::constraint::TestableOutcome* _outcome = nullptr) const override { auto st = static_cast(_state); auto val = st->getIsometry(); diff --git a/tests/planner/ompl/OMPLTestHelpers.hpp b/tests/planner/ompl/OMPLTestHelpers.hpp index 5356453cf6..7584dc9c39 100644 --- a/tests/planner/ompl/OMPLTestHelpers.hpp +++ b/tests/planner/ompl/OMPLTestHelpers.hpp @@ -154,7 +154,8 @@ class MockProjectionConstraint : public aikido::constraint::Projectable, } bool isSatisfied( - const aikido::statespace::StateSpace::State* _s) const override + const aikido::statespace::StateSpace::State* _s, + aikido::constraint::TestableOutcome* _outcome = nullptr) const override { auto state = static_cast(_s); auto val = mStateSpace->getSubStateHandle(state, 0).getValue(); @@ -190,7 +191,8 @@ class MockTranslationalRobotConstraint : public aikido::constraint::Testable // Documentation inherited bool isSatisfied( - const aikido::statespace::StateSpace::State* _state) const override + const aikido::statespace::StateSpace::State* _state, + aikido::constraint::TestableOutcome* _outcome = nullptr) const override { auto cst = static_cast(_state); auto subState = mStateSpace->getSubStateHandle(cst, 0); From d28417c7f7f12eb0faa02c69a750ebeb76b2a6e8 Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Mon, 27 Nov 2017 15:47:20 -0500 Subject: [PATCH 10/41] Added testing for CollisionFreeOutcome. --- tests/constraint/test_CollisionFree.cpp | 32 +++++++++++++++++++++---- 1 file changed, 27 insertions(+), 5 deletions(-) diff --git a/tests/constraint/test_CollisionFree.cpp b/tests/constraint/test_CollisionFree.cpp index 3b3a97abf6..ff6079fc2b 100644 --- a/tests/constraint/test_CollisionFree.cpp +++ b/tests/constraint/test_CollisionFree.cpp @@ -9,6 +9,7 @@ using aikido::statespace::dart::MetaSkeletonStateSpace; using aikido::statespace::dart::MetaSkeletonStateSpacePtr; using aikido::constraint::CollisionFree; +using aikido::constraint::CollisionFreeOutcome; using aikido::statespace::SO2; using aikido::statespace::SE3; @@ -64,6 +65,11 @@ class CollisionFreeTest : public ::testing::Test mStateSpace = std::make_shared(group); mManipulator->enableSelfCollisionCheck(); mManipulator->enableAdjacentBodyCheck(); + + // See what CollisionFreeOutcome's toString returns when no collision + // has occurred. Compared to in tests as a sanity check. + CollisionFreeOutcome outcome; + mCollisionFreeOutcomeString = outcome.toString(); } public: @@ -78,6 +84,9 @@ class CollisionFreeTest : public ::testing::Test // statespace setup MetaSkeletonStateSpacePtr mStateSpace; + + // constraint outcome setup + std::string mCollisionFreeOutcomeString; }; TEST_F(CollisionFreeTest, ConstructorThrowsOnNullStateSpace) @@ -104,7 +113,9 @@ TEST_F(CollisionFreeTest, EmptyCollisionGroup_IsSatisfiedReturnsTrue) auto state = mStateSpace->getScopedStateFromMetaSkeleton(); mStateSpace->setState(state); - EXPECT_TRUE(constraint.isSatisfied(state)); + CollisionFreeOutcome outcome; + EXPECT_TRUE(constraint.isSatisfied(state, &outcome)); + EXPECT_TRUE(outcome.isSatisfied()); } TEST_F(CollisionFreeTest, AddPairwiseCheckPasses_IsSatisfied) @@ -119,7 +130,9 @@ TEST_F(CollisionFreeTest, AddPairwiseCheckPasses_IsSatisfied) mStateSpace->convertPositionsToState(position, state); mStateSpace->setState(state); - EXPECT_TRUE(constraint.isSatisfied(state)); + CollisionFreeOutcome outcome; + EXPECT_TRUE(constraint.isSatisfied(state, &outcome)); + EXPECT_TRUE(outcome.isSatisfied()); } TEST_F(CollisionFreeTest, AddPairwiseCheckFails_IsSatisfied) @@ -129,7 +142,10 @@ TEST_F(CollisionFreeTest, AddPairwiseCheckFails_IsSatisfied) auto state = mStateSpace->getScopedStateFromMetaSkeleton(); constraint.addPairwiseCheck(mCollisionGroup1, mCollisionGroup3); - EXPECT_FALSE(constraint.isSatisfied(state)); + CollisionFreeOutcome outcome; + EXPECT_FALSE(constraint.isSatisfied(state, &outcome)); + EXPECT_FALSE(outcome.isSatisfied()); + EXPECT_NE(mCollisionFreeOutcomeString, outcome.toString()); } TEST_F(CollisionFreeTest, AddAndRemovePairwiseCheckFails_IsSatisfied) @@ -157,7 +173,10 @@ TEST_F(CollisionFreeTest, AddSelfCheckPasses_IsSatisfied) auto state = mStateSpace->getScopedStateFromMetaSkeleton(); constraint.addSelfCheck(mCollisionGroup1); - EXPECT_TRUE(constraint.isSatisfied(state)); + + CollisionFreeOutcome outcome; + EXPECT_TRUE(constraint.isSatisfied(state, &outcome)); + EXPECT_TRUE(outcome.isSatisfied()); } TEST_F(CollisionFreeTest, AddSelfCheckFails_IsSatisfied) @@ -167,7 +186,10 @@ TEST_F(CollisionFreeTest, AddSelfCheckFails_IsSatisfied) auto state = mStateSpace->getScopedStateFromMetaSkeleton(); constraint.addSelfCheck(mCollisionGroup3); - EXPECT_FALSE(constraint.isSatisfied(state)); + CollisionFreeOutcome outcome; + EXPECT_FALSE(constraint.isSatisfied(state, &outcome)); + EXPECT_FALSE(outcome.isSatisfied()); + EXPECT_NE(mCollisionFreeOutcomeString, outcome.toString()); } TEST_F(CollisionFreeTest, AddAndRemoveSelfCheckFails_IsSatisfied) From faffb5289e929f739b63bf4192db57aadefe9980 Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Mon, 27 Nov 2017 15:48:57 -0500 Subject: [PATCH 11/41] Use move semantics in CollisionFreeOutcome toString method. --- src/constraint/outcome/CollisionFreeOutcome.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/constraint/outcome/CollisionFreeOutcome.cpp b/src/constraint/outcome/CollisionFreeOutcome.cpp index dbc493f6d8..cac8f90433 100644 --- a/src/constraint/outcome/CollisionFreeOutcome.cpp +++ b/src/constraint/outcome/CollisionFreeOutcome.cpp @@ -30,7 +30,7 @@ std::string CollisionFreeOutcome::toString() const ss << "[SELF COLLISION]: " << selfCollBodyNodeName << std::endl; } - return ss.str(); + return std::move(ss.str()); } //============================================================================== From 3ecb343961e1a6b2f46d3847233b3d8e1f937056 Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Mon, 27 Nov 2017 15:54:20 -0500 Subject: [PATCH 12/41] Ran make format. --- include/aikido/constraint/outcome/CollisionFreeOutcome.hpp | 3 ++- include/aikido/constraint/outcome/TestableOutcome.hpp | 3 ++- .../constraint/uniform/detail/RnBoxConstraint-impl.hpp | 3 +-- src/constraint/TSR.cpp | 3 +-- src/constraint/outcome/CollisionFreeOutcome.cpp | 4 ++-- src/constraint/uniform/SE2BoxConstraint.cpp | 3 +-- tests/constraint/MockConstraints.hpp | 6 ++++-- 7 files changed, 13 insertions(+), 12 deletions(-) diff --git a/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp b/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp index d4344769bd..91cfeb4923 100644 --- a/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp +++ b/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp @@ -8,7 +8,8 @@ namespace aikido { namespace constraint { -class CollisionFreeOutcome : public TestableOutcome { +class CollisionFreeOutcome : public TestableOutcome +{ public: bool isSatisfied() const; std::string toString() const; diff --git a/include/aikido/constraint/outcome/TestableOutcome.hpp b/include/aikido/constraint/outcome/TestableOutcome.hpp index b18a7b6987..278c7342ec 100644 --- a/include/aikido/constraint/outcome/TestableOutcome.hpp +++ b/include/aikido/constraint/outcome/TestableOutcome.hpp @@ -6,7 +6,8 @@ namespace aikido { namespace constraint { -class TestableOutcome { +class TestableOutcome +{ public: virtual bool isSatisfied() const = 0; virtual std::string toString() const = 0; diff --git a/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp b/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp index 69dd2fb25d..05cf3a0fd7 100644 --- a/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp +++ b/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp @@ -175,8 +175,7 @@ std::vector RBoxConstraint::getConstraintTypes() const //============================================================================== template bool RBoxConstraint::isSatisfied( - const statespace::StateSpace::State* state, - TestableOutcome* _outcome) const + const statespace::StateSpace::State* state, TestableOutcome* _outcome) const { const auto value = mSpace->getValue( static_cast::State*>(state)); diff --git a/src/constraint/TSR.cpp b/src/constraint/TSR.cpp index e64d4f33cd..d7c03a3a79 100644 --- a/src/constraint/TSR.cpp +++ b/src/constraint/TSR.cpp @@ -211,8 +211,7 @@ std::unique_ptr TSR::createSampleGenerator() const //============================================================================== bool TSR::isSatisfied( - const statespace::StateSpace::State* _s, - TestableOutcome* _outcome) const + const statespace::StateSpace::State* _s, TestableOutcome* _outcome) const { Eigen::VectorXd dist; getValue(_s, dist); diff --git a/src/constraint/outcome/CollisionFreeOutcome.cpp b/src/constraint/outcome/CollisionFreeOutcome.cpp index cac8f90433..f55712c957 100644 --- a/src/constraint/outcome/CollisionFreeOutcome.cpp +++ b/src/constraint/outcome/CollisionFreeOutcome.cpp @@ -25,12 +25,12 @@ std::string CollisionFreeOutcome::toString() const ss << "[COLLISION]: " << collBodyNodeName << std::endl; } - for (auto selfCollBodyNodeName: mSelfCollisionBodyNodes) + for (auto selfCollBodyNodeName : mSelfCollisionBodyNodes) { ss << "[SELF COLLISION]: " << selfCollBodyNodeName << std::endl; } - return std::move(ss.str()); + return std::move(ss.str()); } //============================================================================== diff --git a/src/constraint/uniform/SE2BoxConstraint.cpp b/src/constraint/uniform/SE2BoxConstraint.cpp index d4e5fc2fba..2f9e154ff4 100644 --- a/src/constraint/uniform/SE2BoxConstraint.cpp +++ b/src/constraint/uniform/SE2BoxConstraint.cpp @@ -137,8 +137,7 @@ statespace::StateSpacePtr SE2BoxConstraint::getStateSpace() const //============================================================================== bool SE2BoxConstraint::isSatisfied( - const statespace::StateSpace::State* state, - TestableOutcome* _outcome) const + const statespace::StateSpace::State* state, TestableOutcome* _outcome) const { Eigen::VectorXd tangent; mSpace->logMap(static_cast(state), tangent); diff --git a/tests/constraint/MockConstraints.hpp b/tests/constraint/MockConstraints.hpp index 4e60c8a40f..aac5d105d8 100644 --- a/tests/constraint/MockConstraints.hpp +++ b/tests/constraint/MockConstraints.hpp @@ -15,7 +15,8 @@ class PassingConstraint : public aikido::constraint::Testable bool isSatisfied( const aikido::statespace::StateSpace::State* /*state*/, - aikido::constraint::TestableOutcome* = nullptr /*_outcome*/) const override + aikido::constraint::TestableOutcome* = nullptr /*_outcome*/) + const override { return true; } @@ -40,7 +41,8 @@ class FailingConstraint : public aikido::constraint::Testable bool isSatisfied( const aikido::statespace::StateSpace::State* /*state*/, - aikido::constraint::TestableOutcome* = nullptr /*_outcome*/) const override + aikido::constraint::TestableOutcome* = nullptr /*_outcome*/) + const override { return false; } From c281a030d620affa9f3eb09d0a5d4e15543a8f06 Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Mon, 27 Nov 2017 20:21:16 -0500 Subject: [PATCH 13/41] Adressed nits from @jslee02. Moving to squish those unused param warnings next. --- include/aikido/constraint/Testable.hpp | 3 ++- .../outcome/CollisionFreeOutcome.hpp | 20 +++++++++++++++++-- .../constraint/outcome/TestableOutcome.hpp | 5 +++++ src/constraint/CollisionFree.cpp | 8 ++++---- .../outcome/CollisionFreeOutcome.cpp | 13 ++++-------- 5 files changed, 33 insertions(+), 16 deletions(-) diff --git a/include/aikido/constraint/Testable.hpp b/include/aikido/constraint/Testable.hpp index 31af371e9d..8e59ad78df 100644 --- a/include/aikido/constraint/Testable.hpp +++ b/include/aikido/constraint/Testable.hpp @@ -3,11 +3,12 @@ #include #include "../statespace/StateSpace.hpp" -#include "outcome/TestableOutcome.hpp" namespace aikido { namespace constraint { +class TestableOutcome; + /// Constraint which can be tested. class Testable { diff --git a/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp b/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp index 91cfeb4923..5d016517f0 100644 --- a/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp +++ b/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp @@ -11,13 +11,29 @@ namespace constraint { class CollisionFreeOutcome : public TestableOutcome { public: - bool isSatisfied() const; - std::string toString() const; + /// Documentation inherited. + bool isSatisfied() const override; + + /// Returns a string with the name of each colliding BodyNode on a separate + /// line. Each BodyNode is also marked as being a normal collision or self + /// collision. + std::string toString() const override; + + /// Mark a colliding BodyNode in a normal collision. Used by CollisionFree + /// to modify this object. + /// \param bodyNodeName name of body node collided with. void markCollisionBodyNode(const std::string& bodyNodeName); + + /// Mark a colliding BodyNode in a self collision. Used by CollisionFree to + /// modify this object. + /// \param bodyNodeName name of body node collided with during self collision. void markSelfCollisionBodyNode(const std::string& bodyNodeName); private: + /// Holds names of colliding BodyNodes. std::vector mCollisionBodyNodes; + + /// Holds names of self-colliding BodyNodes. std::vector mSelfCollisionBodyNodes; }; diff --git a/include/aikido/constraint/outcome/TestableOutcome.hpp b/include/aikido/constraint/outcome/TestableOutcome.hpp index 278c7342ec..925878b57c 100644 --- a/include/aikido/constraint/outcome/TestableOutcome.hpp +++ b/include/aikido/constraint/outcome/TestableOutcome.hpp @@ -9,7 +9,12 @@ namespace constraint { 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; }; diff --git a/src/constraint/CollisionFree.cpp b/src/constraint/CollisionFree.cpp index 754e15fe07..065617f5c5 100644 --- a/src/constraint/CollisionFree.cpp +++ b/src/constraint/CollisionFree.cpp @@ -48,8 +48,8 @@ bool CollisionFree::isSatisfied( { if (_outcome != nullptr) { - auto collisionOutcome = static_cast(_outcome); - auto collidingBodies = collisionResult.getCollidingBodyNodes(); + const auto& collisionOutcome = static_cast(_outcome); + const auto& collidingBodies = collisionResult.getCollidingBodyNodes(); for (const auto& elem : collidingBodies) { collisionOutcome->markCollisionBodyNode(elem->getName()); @@ -67,8 +67,8 @@ bool CollisionFree::isSatisfied( { if (_outcome != nullptr) { - auto collisionOutcome = static_cast(_outcome); - auto collidingBodies = collisionResult.getCollidingBodyNodes(); + const auto& collisionOutcome = static_cast(_outcome); + const auto& collidingBodies = collisionResult.getCollidingBodyNodes(); for (const auto& elem : collidingBodies) { collisionOutcome->markSelfCollisionBodyNode(elem->getName()); diff --git a/src/constraint/outcome/CollisionFreeOutcome.cpp b/src/constraint/outcome/CollisionFreeOutcome.cpp index f55712c957..9f5103c6f3 100644 --- a/src/constraint/outcome/CollisionFreeOutcome.cpp +++ b/src/constraint/outcome/CollisionFreeOutcome.cpp @@ -6,12 +6,7 @@ namespace constraint { //============================================================================== bool CollisionFreeOutcome::isSatisfied() const { - if (mCollisionBodyNodes.size() > 0 || mSelfCollisionBodyNodes.size() > 0) - { - return false; - } - - return true; + return mCollisionBodyNodes.empty() && mSelfCollisionBodyNodes.empty(); } //============================================================================== @@ -20,17 +15,17 @@ std::string CollisionFreeOutcome::toString() const std::stringstream ss; ss << "ALL COLLISIONS: " << std::endl; - for (auto collBodyNodeName : mCollisionBodyNodes) + for (const auto& collBodyNodeName : mCollisionBodyNodes) { ss << "[COLLISION]: " << collBodyNodeName << std::endl; } - for (auto selfCollBodyNodeName : mSelfCollisionBodyNodes) + for (const auto& selfCollBodyNodeName : mSelfCollisionBodyNodes) { ss << "[SELF COLLISION]: " << selfCollBodyNodeName << std::endl; } - return std::move(ss.str()); + return ss.str(); } //============================================================================== From 924041b1f5a8c970c0660c5c2a8603af87a08943 Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Mon, 27 Nov 2017 20:45:21 -0500 Subject: [PATCH 14/41] Silence unused param warning, ran make format again. --- .../constraint/uniform/detail/RnBoxConstraint-impl.hpp | 3 ++- src/constraint/CartesianProductTestable.cpp | 2 +- src/constraint/CollisionFree.cpp | 6 ++++-- src/constraint/FrameTestable.cpp | 2 +- src/constraint/TSR.cpp | 3 ++- src/constraint/TestableIntersection.cpp | 2 +- src/constraint/uniform/SE2BoxConstraint.cpp | 3 ++- tests/constraint/test_FrameTestable.cpp | 3 ++- tests/planner/ompl/OMPLTestHelpers.hpp | 4 ++-- 9 files changed, 17 insertions(+), 11 deletions(-) diff --git a/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp b/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp index 05cf3a0fd7..dcf7bcb021 100644 --- a/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp +++ b/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp @@ -175,7 +175,8 @@ std::vector RBoxConstraint::getConstraintTypes() const //============================================================================== template bool RBoxConstraint::isSatisfied( - const statespace::StateSpace::State* state, TestableOutcome* _outcome) const + const statespace::StateSpace::State* state, + TestableOutcome* /*_outcome*/) const { const auto value = mSpace->getValue( static_cast::State*>(state)); diff --git a/src/constraint/CartesianProductTestable.cpp b/src/constraint/CartesianProductTestable.cpp index 536b1ecbd6..60c23b3401 100644 --- a/src/constraint/CartesianProductTestable.cpp +++ b/src/constraint/CartesianProductTestable.cpp @@ -54,7 +54,7 @@ statespace::StateSpacePtr CartesianProductTestable::getStateSpace() const //============================================================================== bool CartesianProductTestable::isSatisfied( const aikido::statespace::StateSpace::State* _state, - TestableOutcome* _outcome) const + TestableOutcome* /*_outcome*/) const { const auto state = static_cast(_state); diff --git a/src/constraint/CollisionFree.cpp b/src/constraint/CollisionFree.cpp index 065617f5c5..3a91a37b8d 100644 --- a/src/constraint/CollisionFree.cpp +++ b/src/constraint/CollisionFree.cpp @@ -48,7 +48,8 @@ bool CollisionFree::isSatisfied( { if (_outcome != nullptr) { - const auto& collisionOutcome = static_cast(_outcome); + const auto& collisionOutcome + = static_cast(_outcome); const auto& collidingBodies = collisionResult.getCollidingBodyNodes(); for (const auto& elem : collidingBodies) { @@ -67,7 +68,8 @@ bool CollisionFree::isSatisfied( { if (_outcome != nullptr) { - const auto& collisionOutcome = static_cast(_outcome); + const auto& collisionOutcome + = static_cast(_outcome); const auto& collidingBodies = collisionResult.getCollidingBodyNodes(); for (const auto& elem : collidingBodies) { diff --git a/src/constraint/FrameTestable.cpp b/src/constraint/FrameTestable.cpp index c195007317..99bfe6e48b 100644 --- a/src/constraint/FrameTestable.cpp +++ b/src/constraint/FrameTestable.cpp @@ -35,7 +35,7 @@ FrameTestable::FrameTestable( //============================================================================== bool FrameTestable::isSatisfied( const statespace::StateSpace::State* _state, - TestableOutcome* _outcome) const + TestableOutcome* /*_outcome*/) const { // Set the state auto state diff --git a/src/constraint/TSR.cpp b/src/constraint/TSR.cpp index d7c03a3a79..7533b7c62b 100644 --- a/src/constraint/TSR.cpp +++ b/src/constraint/TSR.cpp @@ -211,7 +211,8 @@ std::unique_ptr TSR::createSampleGenerator() const //============================================================================== bool TSR::isSatisfied( - const statespace::StateSpace::State* _s, TestableOutcome* _outcome) const + const statespace::StateSpace::State* _s, + TestableOutcome* /*_outcome*/) const { Eigen::VectorXd dist; getValue(_s, dist); diff --git a/src/constraint/TestableIntersection.cpp b/src/constraint/TestableIntersection.cpp index 1c00961f9e..0398eeffbc 100644 --- a/src/constraint/TestableIntersection.cpp +++ b/src/constraint/TestableIntersection.cpp @@ -21,7 +21,7 @@ TestableIntersection::TestableIntersection( //============================================================================== bool TestableIntersection::isSatisfied( const aikido::statespace::StateSpace::State* _state, - TestableOutcome* _outcome) const + TestableOutcome* /*_outcome*/) const { for (auto c : mConstraints) { diff --git a/src/constraint/uniform/SE2BoxConstraint.cpp b/src/constraint/uniform/SE2BoxConstraint.cpp index 2f9e154ff4..9ec9f1a200 100644 --- a/src/constraint/uniform/SE2BoxConstraint.cpp +++ b/src/constraint/uniform/SE2BoxConstraint.cpp @@ -137,7 +137,8 @@ statespace::StateSpacePtr SE2BoxConstraint::getStateSpace() const //============================================================================== bool SE2BoxConstraint::isSatisfied( - const statespace::StateSpace::State* state, TestableOutcome* _outcome) const + const statespace::StateSpace::State* state, + TestableOutcome* /*_outcome*/) const { Eigen::VectorXd tangent; mSpace->logMap(static_cast(state), tangent); diff --git a/tests/constraint/test_FrameTestable.cpp b/tests/constraint/test_FrameTestable.cpp index 7ca30a54aa..03a0ec9313 100644 --- a/tests/constraint/test_FrameTestable.cpp +++ b/tests/constraint/test_FrameTestable.cpp @@ -30,7 +30,8 @@ class EndEffectorTestable : public aikido::constraint::Testable bool isSatisfied( const aikido::statespace::StateSpace::State* _state, - aikido::constraint::TestableOutcome* _outcome = nullptr) const override + aikido::constraint::TestableOutcome* = nullptr /*_outcome*/) + const override { auto st = static_cast(_state); auto val = st->getIsometry(); diff --git a/tests/planner/ompl/OMPLTestHelpers.hpp b/tests/planner/ompl/OMPLTestHelpers.hpp index 7584dc9c39..4a335a22b3 100644 --- a/tests/planner/ompl/OMPLTestHelpers.hpp +++ b/tests/planner/ompl/OMPLTestHelpers.hpp @@ -155,7 +155,7 @@ class MockProjectionConstraint : public aikido::constraint::Projectable, bool isSatisfied( const aikido::statespace::StateSpace::State* _s, - aikido::constraint::TestableOutcome* _outcome = nullptr) const override + aikido::constraint::TestableOutcome* = nullptr /*_outcome*/) const override { auto state = static_cast(_s); auto val = mStateSpace->getSubStateHandle(state, 0).getValue(); @@ -192,7 +192,7 @@ class MockTranslationalRobotConstraint : public aikido::constraint::Testable // Documentation inherited bool isSatisfied( const aikido::statespace::StateSpace::State* _state, - aikido::constraint::TestableOutcome* _outcome = nullptr) const override + aikido::constraint::TestableOutcome* = nullptr /*_outcome*/) const override { auto cst = static_cast(_state); auto subState = mStateSpace->getSubStateHandle(cst, 0); From a4a0309455dd81a50ca1767c55fc99be497075d3 Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Mon, 27 Nov 2017 21:04:47 -0500 Subject: [PATCH 15/41] Used dynamic_cast in CollisionFree when populating outcome fields. Barf if type mismatch. --- src/constraint/CollisionFree.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/constraint/CollisionFree.cpp b/src/constraint/CollisionFree.cpp index 3a91a37b8d..f7c2097120 100644 --- a/src/constraint/CollisionFree.cpp +++ b/src/constraint/CollisionFree.cpp @@ -49,7 +49,10 @@ bool CollisionFree::isSatisfied( if (_outcome != nullptr) { const auto& collisionOutcome - = static_cast(_outcome); + = dynamic_cast(_outcome); + if (collisionOutcome == nullptr) + throw std::invalid_argument( + "TestableOutcome pointer is not of type CollisionFreeOutcome."); const auto& collidingBodies = collisionResult.getCollidingBodyNodes(); for (const auto& elem : collidingBodies) { @@ -69,7 +72,10 @@ bool CollisionFree::isSatisfied( if (_outcome != nullptr) { const auto& collisionOutcome - = static_cast(_outcome); + = dynamic_cast(_outcome); + if (collisionOutcome == nullptr) + throw std::invalid_argument( + "TestableOutcome pointer is not of type CollisionFreeOutcome."); const auto& collidingBodies = collisionResult.getCollidingBodyNodes(); for (const auto& elem : collidingBodies) { From 2af94536d1d74ca5841ad15aa01ae072212c9774 Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Mon, 27 Nov 2017 22:49:16 -0500 Subject: [PATCH 16/41] Modify CollisionFreeOutcome to take Contact references as inputs as per feedback from @brianhou. Added a method to get the name from a CollisionObject. --- .../outcome/CollisionFreeOutcome.hpp | 44 ++++++++++++------- src/constraint/CollisionFree.cpp | 12 ++--- .../outcome/CollisionFreeOutcome.cpp | 43 +++++++++++++----- 3 files changed, 66 insertions(+), 33 deletions(-) diff --git a/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp b/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp index 5d016517f0..d65b741ef0 100644 --- a/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp +++ b/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp @@ -4,6 +4,11 @@ #include #include #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 { @@ -14,27 +19,34 @@ class CollisionFreeOutcome : public TestableOutcome /// Documentation inherited. bool isSatisfied() const override; - /// Returns a string with the name of each colliding BodyNode on a separate - /// line. Each BodyNode is also marked as being a normal collision or self + /// 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; - /// Mark a colliding BodyNode in a normal collision. Used by CollisionFree - /// to modify this object. - /// \param bodyNodeName name of body node collided with. - void markCollisionBodyNode(const std::string& bodyNodeName); + /// Store a Contact object from a pairwise collision. Used by CollisionFree + // to modify this object. + /// \param pairwiseContact Contact object to store. + void markPairwiseContact(const dart::collision::Contact& pairwiseContact); - /// Mark a colliding BodyNode in a self collision. Used by CollisionFree to + /// Store a Contact object from a self collision. Used by CollisionFree to /// modify this object. - /// \param bodyNodeName name of body node collided with during self collision. - void markSelfCollisionBodyNode(const std::string& bodyNodeName); - -private: - /// Holds names of colliding BodyNodes. - std::vector mCollisionBodyNodes; - - /// Holds names of self-colliding BodyNodes. - std::vector mSelfCollisionBodyNodes; + /// \param selfContact Contact object to store. + void markSelfContact(const dart::collision::Contact& selfContact); + +protected: + /// Holds Contact objects from pairwise collisions. + std::vector mPairwiseContacts; + + /// Holds Contact objects from self collisions. + std::vector mSelfContacts; + + /// 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 object object pointer to CollisionObject we want the name of. + std::string getCollisionObjectName( + dart::collision::CollisionObject* object) const; }; } // namespace constraint diff --git a/src/constraint/CollisionFree.cpp b/src/constraint/CollisionFree.cpp index f7c2097120..bb056a0ccc 100644 --- a/src/constraint/CollisionFree.cpp +++ b/src/constraint/CollisionFree.cpp @@ -53,10 +53,10 @@ bool CollisionFree::isSatisfied( if (collisionOutcome == nullptr) throw std::invalid_argument( "TestableOutcome pointer is not of type CollisionFreeOutcome."); - const auto& collidingBodies = collisionResult.getCollidingBodyNodes(); - for (const auto& elem : collidingBodies) + const auto& contacts = collisionResult.getContacts(); + for (const auto& elem : contacts) { - collisionOutcome->markCollisionBodyNode(elem->getName()); + collisionOutcome->markPairwiseContact(elem); } } return false; @@ -76,10 +76,10 @@ bool CollisionFree::isSatisfied( if (collisionOutcome == nullptr) throw std::invalid_argument( "TestableOutcome pointer is not of type CollisionFreeOutcome."); - const auto& collidingBodies = collisionResult.getCollidingBodyNodes(); - for (const auto& elem : collidingBodies) + const auto& contacts = collisionResult.getContacts(); + for (const auto& elem : contacts) { - collisionOutcome->markSelfCollisionBodyNode(elem->getName()); + collisionOutcome->markSelfContact(elem); } } return false; diff --git a/src/constraint/outcome/CollisionFreeOutcome.cpp b/src/constraint/outcome/CollisionFreeOutcome.cpp index 9f5103c6f3..240cb9b583 100644 --- a/src/constraint/outcome/CollisionFreeOutcome.cpp +++ b/src/constraint/outcome/CollisionFreeOutcome.cpp @@ -6,7 +6,7 @@ namespace constraint { //============================================================================== bool CollisionFreeOutcome::isSatisfied() const { - return mCollisionBodyNodes.empty() && mSelfCollisionBodyNodes.empty(); + return mPairwiseContacts.empty() && mSelfContacts.empty(); } //============================================================================== @@ -15,31 +15,52 @@ std::string CollisionFreeOutcome::toString() const std::stringstream ss; ss << "ALL COLLISIONS: " << std::endl; - for (const auto& collBodyNodeName : mCollisionBodyNodes) + for (const auto& pairwiseContact : mPairwiseContacts) { - ss << "[COLLISION]: " << collBodyNodeName << std::endl; + ss << "[COLLISION]: " + << getCollisionObjectName(pairwiseContact.collisionObject1) << " and " + << getCollisionObjectName(pairwiseContact.collisionObject2) << std::endl; } - for (const auto& selfCollBodyNodeName : mSelfCollisionBodyNodes) + for (const auto& selfContact : mSelfContacts) { - ss << "[SELF COLLISION]: " << selfCollBodyNodeName << std::endl; + ss << "[SELF COLLISION]: " + << getCollisionObjectName(selfContact.collisionObject1) << " and " + << getCollisionObjectName(selfContact.collisionObject2) << std::endl; } return ss.str(); } //============================================================================== -void CollisionFreeOutcome::markCollisionBodyNode( - const std::string& bodyNodeName) +void CollisionFreeOutcome::markPairwiseContact( + const dart::collision::Contact& pairwiseContact) { - mCollisionBodyNodes.push_back(bodyNodeName); + mPairwiseContacts.push_back(pairwiseContact); } //============================================================================== -void CollisionFreeOutcome::markSelfCollisionBodyNode( - const std::string& bodyNodeName) +void CollisionFreeOutcome::markSelfContact( + const dart::collision::Contact& selfContact) { - mSelfCollisionBodyNodes.push_back(bodyNodeName); + mSelfContacts.push_back(selfContact); +} + +//============================================================================== +std::string CollisionFreeOutcome::getCollisionObjectName( + dart::collision::CollisionObject* object) const +{ + const dart::dynamics::ShapeFrame* frame = object->getShapeFrame(); + + if (frame->isShapeNode()) + { + const dart::dynamics::ShapeNode* node = frame->asShapeNode(); + return node->getBodyNodePtr()->getName(); + } + else + { + return frame->getName(); + } } } // namespace constraint From 4ec21f878d86b7b2a91e8296337c13b190ebe119 Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Tue, 28 Nov 2017 15:08:23 -0500 Subject: [PATCH 17/41] Move include of sstream into .cpp file instead of header. --- include/aikido/constraint/outcome/CollisionFreeOutcome.hpp | 1 - src/constraint/outcome/CollisionFreeOutcome.cpp | 1 + 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp b/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp index d65b741ef0..cb1d90e7b4 100644 --- a/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp +++ b/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp @@ -1,7 +1,6 @@ #ifndef AIKIDO_CONSTRAINT_COLLISIONFREEOUTCOME_HPP_ #define AIKIDO_CONSTRAINT_COLLISIONFREEOUTCOME_HPP_ -#include #include #include "TestableOutcome.hpp" #include "dart/collision/CollisionObject.hpp" diff --git a/src/constraint/outcome/CollisionFreeOutcome.cpp b/src/constraint/outcome/CollisionFreeOutcome.cpp index 240cb9b583..e2505d594a 100644 --- a/src/constraint/outcome/CollisionFreeOutcome.cpp +++ b/src/constraint/outcome/CollisionFreeOutcome.cpp @@ -1,4 +1,5 @@ #include +#include namespace aikido { namespace constraint { From 4e2e20f807ea87e17a12950cf8bf2c33743784ba Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Tue, 28 Nov 2017 15:44:55 -0500 Subject: [PATCH 18/41] Made tests take advantage of programatic acess to data. Required adding a getter for Contact vectors in CollisionFreeOutcome and making getCollisionObjectName public. --- .../outcome/CollisionFreeOutcome.hpp | 18 +++++++++---- .../outcome/CollisionFreeOutcome.cpp | 15 ++++++++++- tests/constraint/test_CollisionFree.cpp | 26 +++++++++++++++++-- 3 files changed, 51 insertions(+), 8 deletions(-) diff --git a/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp b/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp index cb1d90e7b4..e4f22ee1e6 100644 --- a/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp +++ b/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp @@ -33,12 +33,13 @@ class CollisionFreeOutcome : public TestableOutcome /// \param selfContact Contact object to store. void markSelfContact(const dart::collision::Contact& selfContact); -protected: - /// Holds Contact objects from pairwise collisions. - std::vector mPairwiseContacts; + /// Return a copy of the vector storing the Contact objects from pairwise + /// collisions. + std::vector getPairwiseContacts(); - /// Holds Contact objects from self collisions. - std::vector mSelfContacts; + /// Return a copy of the vector storing the Contact objects from self + /// collisions. + std::vector getSelfContacts(); /// Gets the name of a CollisionObject. The name returned is that of the /// corresponding BodyNode (if possible). If not, the name of the ShapeFrame @@ -46,6 +47,13 @@ class CollisionFreeOutcome : public TestableOutcome /// \param object object pointer to CollisionObject we want the name of. std::string getCollisionObjectName( dart::collision::CollisionObject* object) const; + +protected: + /// Holds Contact objects from pairwise collisions. + std::vector mPairwiseContacts; + + /// Holds Contact objects from self collisions. + std::vector mSelfContacts; }; } // namespace constraint diff --git a/src/constraint/outcome/CollisionFreeOutcome.cpp b/src/constraint/outcome/CollisionFreeOutcome.cpp index e2505d594a..14f80eb234 100644 --- a/src/constraint/outcome/CollisionFreeOutcome.cpp +++ b/src/constraint/outcome/CollisionFreeOutcome.cpp @@ -1,5 +1,5 @@ -#include #include +#include namespace aikido { namespace constraint { @@ -47,6 +47,19 @@ void CollisionFreeOutcome::markSelfContact( mSelfContacts.push_back(selfContact); } +//============================================================================== +std::vector +CollisionFreeOutcome::getPairwiseContacts() +{ + return mPairwiseContacts; +} + +//============================================================================== +std::vector CollisionFreeOutcome::getSelfContacts() +{ + return mSelfContacts; +} + //============================================================================== std::string CollisionFreeOutcome::getCollisionObjectName( dart::collision::CollisionObject* object) const diff --git a/tests/constraint/test_CollisionFree.cpp b/tests/constraint/test_CollisionFree.cpp index ff6079fc2b..0e14c45ad8 100644 --- a/tests/constraint/test_CollisionFree.cpp +++ b/tests/constraint/test_CollisionFree.cpp @@ -145,7 +145,19 @@ TEST_F(CollisionFreeTest, AddPairwiseCheckFails_IsSatisfied) CollisionFreeOutcome outcome; EXPECT_FALSE(constraint.isSatisfied(state, &outcome)); EXPECT_FALSE(outcome.isSatisfied()); - EXPECT_NE(mCollisionFreeOutcomeString, outcome.toString()); + EXPECT_EQ(0, outcome.getSelfContacts().size()); + + std::vector pairwiseContacts + = outcome.getPairwiseContacts(); + EXPECT_EQ(1, pairwiseContacts.size()); + EXPECT_EQ( + "BodyNode", + outcome.getCollisionObjectName( + pairwiseContacts.front().collisionObject1)); + EXPECT_EQ( + "BodyNode", + outcome.getCollisionObjectName( + pairwiseContacts.front().collisionObject2)); } TEST_F(CollisionFreeTest, AddAndRemovePairwiseCheckFails_IsSatisfied) @@ -189,7 +201,17 @@ TEST_F(CollisionFreeTest, AddSelfCheckFails_IsSatisfied) CollisionFreeOutcome outcome; EXPECT_FALSE(constraint.isSatisfied(state, &outcome)); EXPECT_FALSE(outcome.isSatisfied()); - EXPECT_NE(mCollisionFreeOutcomeString, outcome.toString()); + EXPECT_EQ(0, outcome.getPairwiseContacts().size()); + + std::vector selfContacts + = outcome.getSelfContacts(); + EXPECT_EQ(1, selfContacts.size()); + EXPECT_EQ( + "BodyNode", + outcome.getCollisionObjectName(selfContacts.front().collisionObject1)); + EXPECT_EQ( + "BodyNode", + outcome.getCollisionObjectName(selfContacts.front().collisionObject2)); } TEST_F(CollisionFreeTest, AddAndRemoveSelfCheckFails_IsSatisfied) From b8e00107956cfa20fad5bf24cfbcc1fc0718920b Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Tue, 28 Nov 2017 15:50:53 -0500 Subject: [PATCH 19/41] Made getter methods for Contact vectors const because they can be. --- include/aikido/constraint/outcome/CollisionFreeOutcome.hpp | 4 ++-- src/constraint/outcome/CollisionFreeOutcome.cpp | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp b/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp index e4f22ee1e6..5b6315f20e 100644 --- a/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp +++ b/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp @@ -35,11 +35,11 @@ class CollisionFreeOutcome : public TestableOutcome /// Return a copy of the vector storing the Contact objects from pairwise /// collisions. - std::vector getPairwiseContacts(); + std::vector getPairwiseContacts() const; /// Return a copy of the vector storing the Contact objects from self /// collisions. - std::vector getSelfContacts(); + std::vector 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 diff --git a/src/constraint/outcome/CollisionFreeOutcome.cpp b/src/constraint/outcome/CollisionFreeOutcome.cpp index 14f80eb234..dec3a5c6ca 100644 --- a/src/constraint/outcome/CollisionFreeOutcome.cpp +++ b/src/constraint/outcome/CollisionFreeOutcome.cpp @@ -49,13 +49,14 @@ void CollisionFreeOutcome::markSelfContact( //============================================================================== std::vector -CollisionFreeOutcome::getPairwiseContacts() +CollisionFreeOutcome::getPairwiseContacts() const { return mPairwiseContacts; } //============================================================================== std::vector CollisionFreeOutcome::getSelfContacts() + const { return mSelfContacts; } From 816e86edef794ee50614fc825fe9c3e3ed4d3aac Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Thu, 30 Nov 2017 01:57:19 -0500 Subject: [PATCH 20/41] Address nits from @jslee02. --- .../aikido/constraint/CartesianProductTestable.hpp | 2 +- include/aikido/constraint/CollisionFree.hpp | 2 +- include/aikido/constraint/FrameTestable.hpp | 2 +- include/aikido/constraint/Satisfied.hpp | 2 +- include/aikido/constraint/TSR.hpp | 2 +- include/aikido/constraint/Testable.hpp | 2 +- include/aikido/constraint/TestableIntersection.hpp | 2 +- .../constraint/outcome/CollisionFreeOutcome.hpp | 14 ++++++++------ .../aikido/constraint/outcome/TestableOutcome.hpp | 6 +++++- .../aikido/constraint/uniform/RnBoxConstraint.hpp | 2 +- .../aikido/constraint/uniform/SE2BoxConstraint.hpp | 2 +- .../uniform/detail/RnBoxConstraint-impl.hpp | 2 +- src/constraint/CartesianProductTestable.cpp | 2 +- src/constraint/CollisionFree.cpp | 14 +++++++------- src/constraint/FrameTestable.cpp | 2 +- src/constraint/Satisfied.cpp | 2 +- src/constraint/TSR.cpp | 3 +-- src/constraint/TestableIntersection.cpp | 2 +- src/constraint/outcome/CollisionFreeOutcome.cpp | 2 +- tests/constraint/MockConstraints.hpp | 6 ++---- tests/constraint/test_FrameTestable.cpp | 3 +-- tests/planner/ompl/OMPLTestHelpers.hpp | 4 ++-- 22 files changed, 41 insertions(+), 39 deletions(-) diff --git a/include/aikido/constraint/CartesianProductTestable.hpp b/include/aikido/constraint/CartesianProductTestable.hpp index 2ad888b83c..886d572c24 100644 --- a/include/aikido/constraint/CartesianProductTestable.hpp +++ b/include/aikido/constraint/CartesianProductTestable.hpp @@ -27,7 +27,7 @@ class CartesianProductTestable : public Testable bool isSatisfied( const aikido::statespace::StateSpace::State* _state, - TestableOutcome* _outcome = nullptr) const override; + TestableOutcome* outcome = nullptr) const override; private: std::shared_ptr mStateSpace; diff --git a/include/aikido/constraint/CollisionFree.hpp b/include/aikido/constraint/CollisionFree.hpp index 6d12ad1d1f..1a5c924d4d 100644 --- a/include/aikido/constraint/CollisionFree.hpp +++ b/include/aikido/constraint/CollisionFree.hpp @@ -43,7 +43,7 @@ class CollisionFree : public Testable // Documentation inherited. bool isSatisfied( const aikido::statespace::StateSpace::State* _state, - TestableOutcome* _outcome = nullptr) const override; + TestableOutcome* outcome = nullptr) const override; /// Checks collision between group1 and group2. /// \param group1 First collision group. diff --git a/include/aikido/constraint/FrameTestable.hpp b/include/aikido/constraint/FrameTestable.hpp index 5b2a0c0afa..16703b313d 100644 --- a/include/aikido/constraint/FrameTestable.hpp +++ b/include/aikido/constraint/FrameTestable.hpp @@ -31,7 +31,7 @@ class FrameTestable : public Testable /// StateSpace returend by getStateSpace(). bool isSatisfied( const statespace::StateSpace::State* _state, - TestableOutcome* _outcome = nullptr) const override; + TestableOutcome* outcome = nullptr) const override; // Documentation inhereted std::shared_ptr getStateSpace() const override; diff --git a/include/aikido/constraint/Satisfied.hpp b/include/aikido/constraint/Satisfied.hpp index cc06fdc656..592329869a 100644 --- a/include/aikido/constraint/Satisfied.hpp +++ b/include/aikido/constraint/Satisfied.hpp @@ -36,7 +36,7 @@ class Satisfied : public constraint::Differentiable, /// \param state a state in \c getStateSpace() bool isSatisfied( const statespace::StateSpace::State* state, - TestableOutcome* _outcome = nullptr) const override; + TestableOutcome* outcome = nullptr) const override; /// Sets \c _out to \c _s. /// diff --git a/include/aikido/constraint/TSR.hpp b/include/aikido/constraint/TSR.hpp index 2b7eda2d20..6ed6943f2d 100644 --- a/include/aikido/constraint/TSR.hpp +++ b/include/aikido/constraint/TSR.hpp @@ -82,7 +82,7 @@ class TSR : public Sampleable, // Documentation inherited. bool isSatisfied( const statespace::StateSpace::State* _s, - TestableOutcome* _outcome = nullptr) const override; + TestableOutcome* outcome = nullptr) const override; /// Throws an invalid_argument exception if this TSR is invalid. /// For a TSR to be valid, mBw(i, 0) <= mBw(i, 1). diff --git a/include/aikido/constraint/Testable.hpp b/include/aikido/constraint/Testable.hpp index 8e59ad78df..56ef07237c 100644 --- a/include/aikido/constraint/Testable.hpp +++ b/include/aikido/constraint/Testable.hpp @@ -18,7 +18,7 @@ class Testable /// Returns true if state satisfies this constraint. virtual bool isSatisfied( const statespace::StateSpace::State* _state, - TestableOutcome* _outcome = nullptr) const = 0; + TestableOutcome* outcome = nullptr) const = 0; /// Returns StateSpace in which this constraint operates. virtual statespace::StateSpacePtr getStateSpace() const = 0; diff --git a/include/aikido/constraint/TestableIntersection.hpp b/include/aikido/constraint/TestableIntersection.hpp index f704163364..306f1d6991 100644 --- a/include/aikido/constraint/TestableIntersection.hpp +++ b/include/aikido/constraint/TestableIntersection.hpp @@ -24,7 +24,7 @@ class TestableIntersection : public Testable // Documentation inherited. bool isSatisfied( const aikido::statespace::StateSpace::State* state, - TestableOutcome* _outcome = nullptr) const override; + TestableOutcome* outcome = nullptr) const override; // Documentation inherited. statespace::StateSpacePtr getStateSpace() const override; diff --git a/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp b/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp index 5b6315f20e..597fdf80d3 100644 --- a/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp +++ b/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp @@ -12,6 +12,8 @@ namespace aikido { namespace constraint { +/// TestableOutcome derivative class intended as (optional) input to isSatisfied +/// method in CollisionFree class. class CollisionFreeOutcome : public TestableOutcome { public: @@ -24,13 +26,13 @@ class CollisionFreeOutcome : public TestableOutcome std::string toString() const override; /// Store a Contact object from a pairwise collision. Used by CollisionFree - // to modify this object. - /// \param pairwiseContact Contact object to store. + /// to modify this object. + /// \param[in] pairwiseContact Contact object to store. void markPairwiseContact(const dart::collision::Contact& pairwiseContact); /// Store a Contact object from a self collision. Used by CollisionFree to /// modify this object. - /// \param selfContact Contact object to store. + /// \param[in] selfContact Contact object to store. void markSelfContact(const dart::collision::Contact& selfContact); /// Return a copy of the vector storing the Contact objects from pairwise @@ -44,9 +46,9 @@ class CollisionFreeOutcome : public TestableOutcome /// 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 object object pointer to CollisionObject we want the name of. + /// \param[in] object object pointer to CollisionObject we want the name of. std::string getCollisionObjectName( - dart::collision::CollisionObject* object) const; + const dart::collision::CollisionObject* object) const; protected: /// Holds Contact objects from pairwise collisions. @@ -59,4 +61,4 @@ class CollisionFreeOutcome : public TestableOutcome } // namespace constraint } // namespace aikido -#endif // ifndef AIKIDO_CONSTRAINT_COLLISIONFREEOUTCOME_HPP_ +#endif // AIKIDO_CONSTRAINT_COLLISIONFREEOUTCOME_HPP_ diff --git a/include/aikido/constraint/outcome/TestableOutcome.hpp b/include/aikido/constraint/outcome/TestableOutcome.hpp index 925878b57c..8e2e58927e 100644 --- a/include/aikido/constraint/outcome/TestableOutcome.hpp +++ b/include/aikido/constraint/outcome/TestableOutcome.hpp @@ -6,6 +6,10 @@ 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: @@ -21,4 +25,4 @@ class TestableOutcome } // namespace constraint } // namespace aikido -#endif // ifndef AIKIDO_CONSTRAINT_TESTABLEOUTCOME_HPP_ +#endif // AIKIDO_CONSTRAINT_TESTABLEOUTCOME_HPP_ diff --git a/include/aikido/constraint/uniform/RnBoxConstraint.hpp b/include/aikido/constraint/uniform/RnBoxConstraint.hpp index bc72af20a7..63bf7b7601 100644 --- a/include/aikido/constraint/uniform/RnBoxConstraint.hpp +++ b/include/aikido/constraint/uniform/RnBoxConstraint.hpp @@ -48,7 +48,7 @@ class RBoxConstraint : public constraint::Differentiable, // Documentation inherited. bool isSatisfied( const statespace::StateSpace::State* state, - TestableOutcome* _outcome = nullptr) const override; + TestableOutcome* outcome = nullptr) const override; // Documentation inherited. bool project( diff --git a/include/aikido/constraint/uniform/SE2BoxConstraint.hpp b/include/aikido/constraint/uniform/SE2BoxConstraint.hpp index e97cb92ac6..d5382b997e 100644 --- a/include/aikido/constraint/uniform/SE2BoxConstraint.hpp +++ b/include/aikido/constraint/uniform/SE2BoxConstraint.hpp @@ -38,7 +38,7 @@ class SE2BoxConstraint : public constraint::Projectable, // Documentation inherited. bool isSatisfied( const statespace::StateSpace::State* state, - TestableOutcome* _outcome = nullptr) const override; + TestableOutcome* outcome = nullptr) const override; // Documentation inherited. bool project( diff --git a/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp b/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp index dcf7bcb021..748513d4a4 100644 --- a/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp +++ b/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp @@ -176,7 +176,7 @@ std::vector RBoxConstraint::getConstraintTypes() const template bool RBoxConstraint::isSatisfied( const statespace::StateSpace::State* state, - TestableOutcome* /*_outcome*/) const + TestableOutcome* /*outcome*/) const { const auto value = mSpace->getValue( static_cast::State*>(state)); diff --git a/src/constraint/CartesianProductTestable.cpp b/src/constraint/CartesianProductTestable.cpp index 60c23b3401..fbf6e5030a 100644 --- a/src/constraint/CartesianProductTestable.cpp +++ b/src/constraint/CartesianProductTestable.cpp @@ -54,7 +54,7 @@ statespace::StateSpacePtr CartesianProductTestable::getStateSpace() const //============================================================================== bool CartesianProductTestable::isSatisfied( const aikido::statespace::StateSpace::State* _state, - TestableOutcome* /*_outcome*/) const + TestableOutcome* /*outcome*/) const { const auto state = static_cast(_state); diff --git a/src/constraint/CollisionFree.cpp b/src/constraint/CollisionFree.cpp index bb056a0ccc..97ef2ad8cd 100644 --- a/src/constraint/CollisionFree.cpp +++ b/src/constraint/CollisionFree.cpp @@ -28,7 +28,7 @@ statespace::StateSpacePtr CollisionFree::getStateSpace() const //============================================================================== bool CollisionFree::isSatisfied( const aikido::statespace::StateSpace::State* _state, - TestableOutcome* _outcome) const + TestableOutcome* outcome) const { auto skelStatePtr = static_cast(_state); @@ -36,7 +36,7 @@ bool CollisionFree::isSatisfied( bool collision = false; dart::collision::CollisionResult collisionResult; - for (auto groups : mGroupsToPairwiseCheck) + for (const auto& groups : mGroupsToPairwiseCheck) { collision = mCollisionDetector->collide( groups.first.get(), @@ -46,10 +46,10 @@ bool CollisionFree::isSatisfied( if (collision) { - if (_outcome != nullptr) + if (outcome != nullptr) { const auto& collisionOutcome - = dynamic_cast(_outcome); + = dynamic_cast(outcome); if (collisionOutcome == nullptr) throw std::invalid_argument( "TestableOutcome pointer is not of type CollisionFreeOutcome."); @@ -63,16 +63,16 @@ bool CollisionFree::isSatisfied( } } - for (auto group : mGroupsToSelfCheck) + for (const auto& group : mGroupsToSelfCheck) { collision = mCollisionDetector->collide( group.get(), mCollisionOptions, &collisionResult); if (collision) { - if (_outcome != nullptr) + if (outcome != nullptr) { const auto& collisionOutcome - = dynamic_cast(_outcome); + = dynamic_cast(outcome); if (collisionOutcome == nullptr) throw std::invalid_argument( "TestableOutcome pointer is not of type CollisionFreeOutcome."); diff --git a/src/constraint/FrameTestable.cpp b/src/constraint/FrameTestable.cpp index 99bfe6e48b..d8d8ee77a8 100644 --- a/src/constraint/FrameTestable.cpp +++ b/src/constraint/FrameTestable.cpp @@ -35,7 +35,7 @@ FrameTestable::FrameTestable( //============================================================================== bool FrameTestable::isSatisfied( const statespace::StateSpace::State* _state, - TestableOutcome* /*_outcome*/) const + TestableOutcome* /*outcome*/) const { // Set the state auto state diff --git a/src/constraint/Satisfied.cpp b/src/constraint/Satisfied.cpp index b8c810e2b6..b0ec12e817 100644 --- a/src/constraint/Satisfied.cpp +++ b/src/constraint/Satisfied.cpp @@ -32,7 +32,7 @@ std::vector Satisfied::getConstraintTypes() const //============================================================================== bool Satisfied::isSatisfied( const statespace::StateSpace::State* /*state*/, - TestableOutcome* /*_outcome*/) const + TestableOutcome* /*outcome*/) const { return true; } diff --git a/src/constraint/TSR.cpp b/src/constraint/TSR.cpp index 7533b7c62b..3369792867 100644 --- a/src/constraint/TSR.cpp +++ b/src/constraint/TSR.cpp @@ -211,8 +211,7 @@ std::unique_ptr TSR::createSampleGenerator() const //============================================================================== bool TSR::isSatisfied( - const statespace::StateSpace::State* _s, - TestableOutcome* /*_outcome*/) const + const statespace::StateSpace::State* _s, TestableOutcome* /*outcome*/) const { Eigen::VectorXd dist; getValue(_s, dist); diff --git a/src/constraint/TestableIntersection.cpp b/src/constraint/TestableIntersection.cpp index 0398eeffbc..790d7ae3d4 100644 --- a/src/constraint/TestableIntersection.cpp +++ b/src/constraint/TestableIntersection.cpp @@ -21,7 +21,7 @@ TestableIntersection::TestableIntersection( //============================================================================== bool TestableIntersection::isSatisfied( const aikido::statespace::StateSpace::State* _state, - TestableOutcome* /*_outcome*/) const + TestableOutcome* /*outcome*/) const { for (auto c : mConstraints) { diff --git a/src/constraint/outcome/CollisionFreeOutcome.cpp b/src/constraint/outcome/CollisionFreeOutcome.cpp index dec3a5c6ca..3b2a94db5b 100644 --- a/src/constraint/outcome/CollisionFreeOutcome.cpp +++ b/src/constraint/outcome/CollisionFreeOutcome.cpp @@ -63,7 +63,7 @@ std::vector CollisionFreeOutcome::getSelfContacts() //============================================================================== std::string CollisionFreeOutcome::getCollisionObjectName( - dart::collision::CollisionObject* object) const + const dart::collision::CollisionObject* object) const { const dart::dynamics::ShapeFrame* frame = object->getShapeFrame(); diff --git a/tests/constraint/MockConstraints.hpp b/tests/constraint/MockConstraints.hpp index aac5d105d8..280594278b 100644 --- a/tests/constraint/MockConstraints.hpp +++ b/tests/constraint/MockConstraints.hpp @@ -15,8 +15,7 @@ class PassingConstraint : public aikido::constraint::Testable bool isSatisfied( const aikido::statespace::StateSpace::State* /*state*/, - aikido::constraint::TestableOutcome* = nullptr /*_outcome*/) - const override + aikido::constraint::TestableOutcome* /*outcome*/ = nullptr) const override { return true; } @@ -41,8 +40,7 @@ class FailingConstraint : public aikido::constraint::Testable bool isSatisfied( const aikido::statespace::StateSpace::State* /*state*/, - aikido::constraint::TestableOutcome* = nullptr /*_outcome*/) - const override + aikido::constraint::TestableOutcome* /*outcome*/ = nullptr) const override { return false; } diff --git a/tests/constraint/test_FrameTestable.cpp b/tests/constraint/test_FrameTestable.cpp index 03a0ec9313..144a348a8d 100644 --- a/tests/constraint/test_FrameTestable.cpp +++ b/tests/constraint/test_FrameTestable.cpp @@ -30,8 +30,7 @@ class EndEffectorTestable : public aikido::constraint::Testable bool isSatisfied( const aikido::statespace::StateSpace::State* _state, - aikido::constraint::TestableOutcome* = nullptr /*_outcome*/) - const override + aikido::constraint::TestableOutcome* /*outcome*/ = nullptr) const override { auto st = static_cast(_state); auto val = st->getIsometry(); diff --git a/tests/planner/ompl/OMPLTestHelpers.hpp b/tests/planner/ompl/OMPLTestHelpers.hpp index 4a335a22b3..3f52b7d9b7 100644 --- a/tests/planner/ompl/OMPLTestHelpers.hpp +++ b/tests/planner/ompl/OMPLTestHelpers.hpp @@ -155,7 +155,7 @@ class MockProjectionConstraint : public aikido::constraint::Projectable, bool isSatisfied( const aikido::statespace::StateSpace::State* _s, - aikido::constraint::TestableOutcome* = nullptr /*_outcome*/) const override + aikido::constraint::TestableOutcome* /*outcome*/ = nullptr) const override { auto state = static_cast(_s); auto val = mStateSpace->getSubStateHandle(state, 0).getValue(); @@ -192,7 +192,7 @@ class MockTranslationalRobotConstraint : public aikido::constraint::Testable // Documentation inherited bool isSatisfied( const aikido::statespace::StateSpace::State* _state, - aikido::constraint::TestableOutcome* = nullptr /*_outcome*/) const override + aikido::constraint::TestableOutcome* = nullptr /*outcome*/) const override { auto cst = static_cast(_state); auto subState = mStateSpace->getSubStateHandle(cst, 0); From 4174efc865ad496ca7230d11416b490d3c5fb7b4 Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Thu, 30 Nov 2017 02:01:51 -0500 Subject: [PATCH 21/41] Two small nits I missed in the last commit. --- src/constraint/outcome/CollisionFreeOutcome.cpp | 2 +- src/constraint/uniform/SE2BoxConstraint.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/constraint/outcome/CollisionFreeOutcome.cpp b/src/constraint/outcome/CollisionFreeOutcome.cpp index 3b2a94db5b..ddfc44c608 100644 --- a/src/constraint/outcome/CollisionFreeOutcome.cpp +++ b/src/constraint/outcome/CollisionFreeOutcome.cpp @@ -1,5 +1,5 @@ -#include #include +#include namespace aikido { namespace constraint { diff --git a/src/constraint/uniform/SE2BoxConstraint.cpp b/src/constraint/uniform/SE2BoxConstraint.cpp index 9ec9f1a200..d906d015ff 100644 --- a/src/constraint/uniform/SE2BoxConstraint.cpp +++ b/src/constraint/uniform/SE2BoxConstraint.cpp @@ -138,7 +138,7 @@ statespace::StateSpacePtr SE2BoxConstraint::getStateSpace() const //============================================================================== bool SE2BoxConstraint::isSatisfied( const statespace::StateSpace::State* state, - TestableOutcome* /*_outcome*/) const + TestableOutcome* /*outcome*/) const { Eigen::VectorXd tangent; mSpace->logMap(static_cast(state), tangent); From 511555a0e8677a5d70dba18502ad4962aa535a09 Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Thu, 30 Nov 2017 02:04:20 -0500 Subject: [PATCH 22/41] Aaaaaand, one more! --- tests/planner/ompl/OMPLTestHelpers.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/planner/ompl/OMPLTestHelpers.hpp b/tests/planner/ompl/OMPLTestHelpers.hpp index 3f52b7d9b7..eb1e648d39 100644 --- a/tests/planner/ompl/OMPLTestHelpers.hpp +++ b/tests/planner/ompl/OMPLTestHelpers.hpp @@ -192,7 +192,7 @@ class MockTranslationalRobotConstraint : public aikido::constraint::Testable // Documentation inherited bool isSatisfied( const aikido::statespace::StateSpace::State* _state, - aikido::constraint::TestableOutcome* = nullptr /*outcome*/) const override + aikido::constraint::TestableOutcome* /*outcome*/ = nullptr) const override { auto cst = static_cast(_state); auto subState = mStateSpace->getSubStateHandle(cst, 0); From b3da15de809e3d6d73daeb1aa2b0690d8de4c96e Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Thu, 30 Nov 2017 19:15:57 -0500 Subject: [PATCH 23/41] Implemented createOutcome functionality in Testable base class. --- include/aikido/constraint/CollisionFree.hpp | 3 ++ include/aikido/constraint/Testable.hpp | 12 +++++- .../constraint/outcome/DummyOutcome.hpp | 37 +++++++++++++++++++ src/constraint/CollisionFree.cpp | 6 +++ .../outcome/CollisionFreeOutcome.cpp | 2 +- tests/constraint/test_CollisionFree.cpp | 29 ++++++--------- 6 files changed, 69 insertions(+), 20 deletions(-) create mode 100644 include/aikido/constraint/outcome/DummyOutcome.hpp diff --git a/include/aikido/constraint/CollisionFree.hpp b/include/aikido/constraint/CollisionFree.hpp index 1a5c924d4d..04979807c6 100644 --- a/include/aikido/constraint/CollisionFree.hpp +++ b/include/aikido/constraint/CollisionFree.hpp @@ -45,6 +45,9 @@ class CollisionFree : public Testable const aikido::statespace::StateSpace::State* _state, TestableOutcome* outcome = nullptr) const override; + // Documentation inherited. + std::unique_ptr createOutcome() const override; + /// Checks collision between group1 and group2. /// \param group1 First collision group. /// \param group2 Second collision group. diff --git a/include/aikido/constraint/Testable.hpp b/include/aikido/constraint/Testable.hpp index 56ef07237c..b38318d3d1 100644 --- a/include/aikido/constraint/Testable.hpp +++ b/include/aikido/constraint/Testable.hpp @@ -3,6 +3,7 @@ #include #include "../statespace/StateSpace.hpp" +#include "outcome/DummyOutcome.hpp" namespace aikido { namespace constraint { @@ -22,6 +23,15 @@ class Testable /// 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). Default implementation + /// just returns a dummy object. + virtual std::unique_ptr createOutcome() const + { + return std::unique_ptr(new DummyOutcome()); + } }; using TestablePtr = std::shared_ptr; @@ -29,4 +39,4 @@ using TestablePtr = std::shared_ptr; } // namespace constraint } // namespace aikido -#endif // AIKIDO_CONSTRAINT_TESTABLE_HPP_ +#endif // AIKIDO_CONSTRAINT_TESTABLE_HPP_ \ No newline at end of file diff --git a/include/aikido/constraint/outcome/DummyOutcome.hpp b/include/aikido/constraint/outcome/DummyOutcome.hpp new file mode 100644 index 0000000000..6a5f914b96 --- /dev/null +++ b/include/aikido/constraint/outcome/DummyOutcome.hpp @@ -0,0 +1,37 @@ +#ifndef AIKIDO_CONSTRAINT_DUMMYOUTCOME_HPP_ +#define AIKIDO_CONSTRAINT_DUMMYOUTCOME_HPP_ + +#include "TestableOutcome.hpp" + +namespace aikido { +namespace constraint { + +/// This is a dummy 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 DummyOutcome : public TestableOutcome +{ +public: + /// Nonsense. + bool isSatisfied() const override + { + throw std::runtime_error( + "This is a dummy constraint outcome. The Testable derivative class you " + "called createOutcome() on has no corresponding TestableOutcome " + "derivative implemented. Do not call isSatisfied."); + } + + /// Nonsense. + std::string toString() const override + { + return "This is a dummy constraint outcome. The Testable derivative class " + "you called createOutcome() on has no corresponding TestableOutcome " + "derivative implemented."; + } +}; + +} // namespace constraint +} // namespace aikido + +#endif // AIKIDO_CONSTRAINT_DUMMYOUTCOME_HPP_ diff --git a/src/constraint/CollisionFree.cpp b/src/constraint/CollisionFree.cpp index 97ef2ad8cd..400b80af0d 100644 --- a/src/constraint/CollisionFree.cpp +++ b/src/constraint/CollisionFree.cpp @@ -88,6 +88,12 @@ bool CollisionFree::isSatisfied( return true; } +//============================================================================== +std::unique_ptr CollisionFree::createOutcome() const +{ + return std::unique_ptr(new CollisionFreeOutcome()); +} + //============================================================================== void CollisionFree::addPairwiseCheck( std::shared_ptr _group1, diff --git a/src/constraint/outcome/CollisionFreeOutcome.cpp b/src/constraint/outcome/CollisionFreeOutcome.cpp index ddfc44c608..3b2a94db5b 100644 --- a/src/constraint/outcome/CollisionFreeOutcome.cpp +++ b/src/constraint/outcome/CollisionFreeOutcome.cpp @@ -1,5 +1,5 @@ -#include #include +#include namespace aikido { namespace constraint { diff --git a/tests/constraint/test_CollisionFree.cpp b/tests/constraint/test_CollisionFree.cpp index 0e14c45ad8..5eb9e7e8ef 100644 --- a/tests/constraint/test_CollisionFree.cpp +++ b/tests/constraint/test_CollisionFree.cpp @@ -10,6 +10,7 @@ using aikido::statespace::dart::MetaSkeletonStateSpace; using aikido::statespace::dart::MetaSkeletonStateSpacePtr; using aikido::constraint::CollisionFree; using aikido::constraint::CollisionFreeOutcome; +using aikido::constraint::TestableOutcome; using aikido::statespace::SO2; using aikido::statespace::SE3; @@ -65,11 +66,6 @@ class CollisionFreeTest : public ::testing::Test mStateSpace = std::make_shared(group); mManipulator->enableSelfCollisionCheck(); mManipulator->enableAdjacentBodyCheck(); - - // See what CollisionFreeOutcome's toString returns when no collision - // has occurred. Compared to in tests as a sanity check. - CollisionFreeOutcome outcome; - mCollisionFreeOutcomeString = outcome.toString(); } public: @@ -84,9 +80,6 @@ class CollisionFreeTest : public ::testing::Test // statespace setup MetaSkeletonStateSpacePtr mStateSpace; - - // constraint outcome setup - std::string mCollisionFreeOutcomeString; }; TEST_F(CollisionFreeTest, ConstructorThrowsOnNullStateSpace) @@ -113,9 +106,9 @@ TEST_F(CollisionFreeTest, EmptyCollisionGroup_IsSatisfiedReturnsTrue) auto state = mStateSpace->getScopedStateFromMetaSkeleton(); mStateSpace->setState(state); - CollisionFreeOutcome outcome; - EXPECT_TRUE(constraint.isSatisfied(state, &outcome)); - EXPECT_TRUE(outcome.isSatisfied()); + std::unique_ptr outcome = constraint.createOutcome(); + EXPECT_TRUE(constraint.isSatisfied(state, outcome.get())); + EXPECT_TRUE(outcome->isSatisfied()); } TEST_F(CollisionFreeTest, AddPairwiseCheckPasses_IsSatisfied) @@ -130,9 +123,9 @@ TEST_F(CollisionFreeTest, AddPairwiseCheckPasses_IsSatisfied) mStateSpace->convertPositionsToState(position, state); mStateSpace->setState(state); - CollisionFreeOutcome outcome; - EXPECT_TRUE(constraint.isSatisfied(state, &outcome)); - EXPECT_TRUE(outcome.isSatisfied()); + std::unique_ptr outcome = constraint.createOutcome(); + EXPECT_TRUE(constraint.isSatisfied(state, outcome.get())); + EXPECT_TRUE(outcome->isSatisfied()); } TEST_F(CollisionFreeTest, AddPairwiseCheckFails_IsSatisfied) @@ -186,9 +179,9 @@ TEST_F(CollisionFreeTest, AddSelfCheckPasses_IsSatisfied) auto state = mStateSpace->getScopedStateFromMetaSkeleton(); constraint.addSelfCheck(mCollisionGroup1); - CollisionFreeOutcome outcome; - EXPECT_TRUE(constraint.isSatisfied(state, &outcome)); - EXPECT_TRUE(outcome.isSatisfied()); + std::unique_ptr outcome = constraint.createOutcome(); + EXPECT_TRUE(constraint.isSatisfied(state, outcome.get())); + EXPECT_TRUE(outcome->isSatisfied()); } TEST_F(CollisionFreeTest, AddSelfCheckFails_IsSatisfied) @@ -225,4 +218,4 @@ TEST_F(CollisionFreeTest, AddAndRemoveSelfCheckFails_IsSatisfied) constraint.removeSelfCheck(mCollisionGroup3); EXPECT_TRUE(constraint.isSatisfied(state)); -} +} \ No newline at end of file From e061d63394544e6b47f3632895786d90a7a06394 Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Thu, 30 Nov 2017 19:25:59 -0500 Subject: [PATCH 24/41] Use auto collisionOutcome as per request of @brianhou. --- src/constraint/CollisionFree.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/constraint/CollisionFree.cpp b/src/constraint/CollisionFree.cpp index 400b80af0d..42f97af841 100644 --- a/src/constraint/CollisionFree.cpp +++ b/src/constraint/CollisionFree.cpp @@ -48,8 +48,7 @@ bool CollisionFree::isSatisfied( { if (outcome != nullptr) { - const auto& collisionOutcome - = dynamic_cast(outcome); + auto collisionOutcome = dynamic_cast(outcome); if (collisionOutcome == nullptr) throw std::invalid_argument( "TestableOutcome pointer is not of type CollisionFreeOutcome."); @@ -71,8 +70,7 @@ bool CollisionFree::isSatisfied( { if (outcome != nullptr) { - const auto& collisionOutcome - = dynamic_cast(outcome); + auto collisionOutcome = dynamic_cast(outcome); if (collisionOutcome == nullptr) throw std::invalid_argument( "TestableOutcome pointer is not of type CollisionFreeOutcome."); From 6c2dbec8cdc9bdda65e80537b22749e668059ae1 Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Tue, 5 Dec 2017 15:55:28 -0500 Subject: [PATCH 25/41] Moved dynamic cast for outcome in CollisionFree. --- src/constraint/CollisionFree.cpp | 25 +++++++++++++------------ 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/src/constraint/CollisionFree.cpp b/src/constraint/CollisionFree.cpp index 42f97af841..dd8e2a9568 100644 --- a/src/constraint/CollisionFree.cpp +++ b/src/constraint/CollisionFree.cpp @@ -30,6 +30,15 @@ bool CollisionFree::isSatisfied( const aikido::statespace::StateSpace::State* _state, TestableOutcome* outcome) const { + CollisionFreeOutcome* collisionFreeOutcome = nullptr; + if (outcome) + { + collisionFreeOutcome = dynamic_cast(outcome); + if (!collisionFreeOutcome) + throw std::invalid_argument( + "TestableOutcome pointer is not of type CollisionFreeOutcome."); + } + auto skelStatePtr = static_cast(_state); mStatespace->setState(skelStatePtr); @@ -46,16 +55,12 @@ bool CollisionFree::isSatisfied( if (collision) { - if (outcome != nullptr) + if (collisionFreeOutcome) { - auto collisionOutcome = dynamic_cast(outcome); - if (collisionOutcome == nullptr) - throw std::invalid_argument( - "TestableOutcome pointer is not of type CollisionFreeOutcome."); const auto& contacts = collisionResult.getContacts(); for (const auto& elem : contacts) { - collisionOutcome->markPairwiseContact(elem); + collisionFreeOutcome->markPairwiseContact(elem); } } return false; @@ -68,16 +73,12 @@ bool CollisionFree::isSatisfied( group.get(), mCollisionOptions, &collisionResult); if (collision) { - if (outcome != nullptr) + if (collisionFreeOutcome) { - auto collisionOutcome = dynamic_cast(outcome); - if (collisionOutcome == nullptr) - throw std::invalid_argument( - "TestableOutcome pointer is not of type CollisionFreeOutcome."); const auto& contacts = collisionResult.getContacts(); for (const auto& elem : contacts) { - collisionOutcome->markSelfContact(elem); + collisionFreeOutcome->markSelfContact(elem); } } return false; From d3ada66b4f99d062dc65de0aa2782278124d5f96 Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Tue, 5 Dec 2017 17:01:24 -0500 Subject: [PATCH 26/41] 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. --- .../constraint/outcome/CollisionFreeOutcome.hpp | 5 +++++ .../aikido/constraint/outcome/DummyOutcome.hpp | 2 +- src/constraint/CollisionFree.cpp | 2 ++ src/constraint/outcome/CollisionFreeOutcome.cpp | 7 +++++++ tests/constraint/test_CollisionFree.cpp | 15 +++++++++++---- 5 files changed, 26 insertions(+), 5 deletions(-) diff --git a/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp b/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp index 597fdf80d3..08fcfe6615 100644 --- a/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp +++ b/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp @@ -25,6 +25,11 @@ class CollisionFreeOutcome : public TestableOutcome /// 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(); + /// Store a Contact object from a pairwise collision. Used by CollisionFree /// to modify this object. /// \param[in] pairwiseContact Contact object to store. diff --git a/include/aikido/constraint/outcome/DummyOutcome.hpp b/include/aikido/constraint/outcome/DummyOutcome.hpp index 6a5f914b96..a293e66c88 100644 --- a/include/aikido/constraint/outcome/DummyOutcome.hpp +++ b/include/aikido/constraint/outcome/DummyOutcome.hpp @@ -7,7 +7,7 @@ namespace aikido { namespace constraint { /// This is a dummy TestableOutcome derivative class. An instance of this class -/// is returned when createOutcome() is called on an instance of a class that +/// is returned when createOutcome() is called on an instance of a class that /// inherits Testable, but has no corresponding TestableOutcome derivative /// implemented. class DummyOutcome : public TestableOutcome diff --git a/src/constraint/CollisionFree.cpp b/src/constraint/CollisionFree.cpp index dd8e2a9568..3e6c224b79 100644 --- a/src/constraint/CollisionFree.cpp +++ b/src/constraint/CollisionFree.cpp @@ -37,6 +37,8 @@ bool CollisionFree::isSatisfied( if (!collisionFreeOutcome) throw std::invalid_argument( "TestableOutcome pointer is not of type CollisionFreeOutcome."); + // Clear the outcome object in case it's being reused. + collisionFreeOutcome->clear(); } auto skelStatePtr = static_castgetScopedStateFromMetaSkeleton(); + // Also reuse this outcome object and ensure the correct result + // is still returned, + CollisionFreeOutcome outcome; constraint.addPairwiseCheck(mCollisionGroup1, mCollisionGroup3); - EXPECT_FALSE(constraint.isSatisfied(state)); + EXPECT_FALSE(constraint.isSatisfied(state, &outcome)); + EXPECT_FALSE(outcome.isSatisfied()); constraint.removePairwiseCheck(mCollisionGroup1, mCollisionGroup3); - EXPECT_TRUE(constraint.isSatisfied(state)); + EXPECT_TRUE(constraint.isSatisfied(state, &outcome)); + EXPECT_TRUE(outcome.isSatisfied()); constraint.addPairwiseCheck(mCollisionGroup1, mCollisionGroup3); - EXPECT_FALSE(constraint.isSatisfied(state)); + EXPECT_FALSE(constraint.isSatisfied(state, &outcome)); + EXPECT_FALSE(outcome.isSatisfied()); constraint.removePairwiseCheck(mCollisionGroup3, mCollisionGroup1); - EXPECT_TRUE(constraint.isSatisfied(state)); + EXPECT_TRUE(constraint.isSatisfied(state, &outcome)); + EXPECT_TRUE(outcome.isSatisfied()); } TEST_F(CollisionFreeTest, AddSelfCheckPasses_IsSatisfied) From 7b10b679f6812b75f394de52c9ec241d7be07e74 Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Tue, 5 Dec 2017 17:29:11 -0500 Subject: [PATCH 27/41] Fixed directory placement of outcome classes. Renamed DummyOutcome ---> DefaultOutcome. --- include/aikido/constraint/CollisionFree.hpp | 2 +- .../aikido/constraint/{outcome => }/CollisionFreeOutcome.hpp | 0 .../{outcome/DummyOutcome.hpp => DefaultOutcome.hpp} | 4 ++-- include/aikido/constraint/Testable.hpp | 4 ++-- include/aikido/constraint/{outcome => }/TestableOutcome.hpp | 0 src/constraint/CMakeLists.txt | 4 ++-- src/constraint/{outcome => }/CollisionFreeOutcome.cpp | 2 +- 7 files changed, 8 insertions(+), 8 deletions(-) rename include/aikido/constraint/{outcome => }/CollisionFreeOutcome.hpp (100%) rename include/aikido/constraint/{outcome/DummyOutcome.hpp => DefaultOutcome.hpp} (89%) rename include/aikido/constraint/{outcome => }/TestableOutcome.hpp (100%) rename src/constraint/{outcome => }/CollisionFreeOutcome.cpp (97%) diff --git a/include/aikido/constraint/CollisionFree.hpp b/include/aikido/constraint/CollisionFree.hpp index 04979807c6..d8dcaced95 100644 --- a/include/aikido/constraint/CollisionFree.hpp +++ b/include/aikido/constraint/CollisionFree.hpp @@ -9,8 +9,8 @@ #include #include #include "../statespace/dart/MetaSkeletonStateSpace.hpp" +#include "CollisionFreeOutcome.hpp" #include "Testable.hpp" -#include "outcome/CollisionFreeOutcome.hpp" namespace aikido { namespace constraint { diff --git a/include/aikido/constraint/outcome/CollisionFreeOutcome.hpp b/include/aikido/constraint/CollisionFreeOutcome.hpp similarity index 100% rename from include/aikido/constraint/outcome/CollisionFreeOutcome.hpp rename to include/aikido/constraint/CollisionFreeOutcome.hpp diff --git a/include/aikido/constraint/outcome/DummyOutcome.hpp b/include/aikido/constraint/DefaultOutcome.hpp similarity index 89% rename from include/aikido/constraint/outcome/DummyOutcome.hpp rename to include/aikido/constraint/DefaultOutcome.hpp index a293e66c88..22184a1850 100644 --- a/include/aikido/constraint/outcome/DummyOutcome.hpp +++ b/include/aikido/constraint/DefaultOutcome.hpp @@ -6,11 +6,11 @@ namespace aikido { namespace constraint { -/// This is a dummy TestableOutcome derivative class. An instance of this class +/// 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 DummyOutcome : public TestableOutcome +class DefaultOutcome : public TestableOutcome { public: /// Nonsense. diff --git a/include/aikido/constraint/Testable.hpp b/include/aikido/constraint/Testable.hpp index b38318d3d1..30a32a63b6 100644 --- a/include/aikido/constraint/Testable.hpp +++ b/include/aikido/constraint/Testable.hpp @@ -3,7 +3,7 @@ #include #include "../statespace/StateSpace.hpp" -#include "outcome/DummyOutcome.hpp" +#include "DefaultOutcome.hpp" namespace aikido { namespace constraint { @@ -30,7 +30,7 @@ class Testable /// just returns a dummy object. virtual std::unique_ptr createOutcome() const { - return std::unique_ptr(new DummyOutcome()); + return std::unique_ptr(new DefaultOutcome()); } }; diff --git a/include/aikido/constraint/outcome/TestableOutcome.hpp b/include/aikido/constraint/TestableOutcome.hpp similarity index 100% rename from include/aikido/constraint/outcome/TestableOutcome.hpp rename to include/aikido/constraint/TestableOutcome.hpp diff --git a/src/constraint/CMakeLists.txt b/src/constraint/CMakeLists.txt index cdc3555830..bf5cbe983b 100644 --- a/src/constraint/CMakeLists.txt +++ b/src/constraint/CMakeLists.txt @@ -1,5 +1,4 @@ set(sources - outcome/CollisionFreeOutcome.cpp uniform/RnBoxConstraint.cpp uniform/RnConstantSampler.cpp uniform/SO2UniformSampler.cpp @@ -8,6 +7,8 @@ set(sources CartesianProductProjectable.cpp CartesianProductSampleable.cpp CartesianProductTestable.cpp + CollisionFree.cpp + CollisionFreeOutcome.cpp CyclicSampleable.cpp Differentiable.cpp DifferentiableIntersection.cpp @@ -19,7 +20,6 @@ set(sources InverseKinematicsSampleable.cpp JointStateSpaceHelpers.cpp NewtonsMethodProjectable.cpp - CollisionFree.cpp Projectable.cpp RejectionSampleable.cpp Sampleable.cpp diff --git a/src/constraint/outcome/CollisionFreeOutcome.cpp b/src/constraint/CollisionFreeOutcome.cpp similarity index 97% rename from src/constraint/outcome/CollisionFreeOutcome.cpp rename to src/constraint/CollisionFreeOutcome.cpp index f244a1b44b..0b0cfe9025 100644 --- a/src/constraint/outcome/CollisionFreeOutcome.cpp +++ b/src/constraint/CollisionFreeOutcome.cpp @@ -1,5 +1,5 @@ #include -#include +#include namespace aikido { namespace constraint { From 0868db7b5e4d1ca5fbc7e14c2bde2f2a368d62c7 Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Tue, 5 Dec 2017 18:26:57 -0500 Subject: [PATCH 28/41] Added simple functionality to DefaultOutcome. --- include/aikido/constraint/DefaultOutcome.hpp | 37 ++++++++++---------- src/constraint/CMakeLists.txt | 1 + src/constraint/DefaultOutcome.cpp | 28 +++++++++++++++ 3 files changed, 47 insertions(+), 19 deletions(-) create mode 100644 src/constraint/DefaultOutcome.cpp diff --git a/include/aikido/constraint/DefaultOutcome.hpp b/include/aikido/constraint/DefaultOutcome.hpp index 22184a1850..c0ff548341 100644 --- a/include/aikido/constraint/DefaultOutcome.hpp +++ b/include/aikido/constraint/DefaultOutcome.hpp @@ -1,5 +1,5 @@ -#ifndef AIKIDO_CONSTRAINT_DUMMYOUTCOME_HPP_ -#define AIKIDO_CONSTRAINT_DUMMYOUTCOME_HPP_ +#ifndef AIKIDO_CONSTRAINT_DEFAULTOUTCOME_HPP_ +#define AIKIDO_CONSTRAINT_DEFAULTOUTCOME_HPP_ #include "TestableOutcome.hpp" @@ -13,25 +13,24 @@ namespace constraint { class DefaultOutcome : public TestableOutcome { public: - /// Nonsense. - bool isSatisfied() const override - { - throw std::runtime_error( - "This is a dummy constraint outcome. The Testable derivative class you " - "called createOutcome() on has no corresponding TestableOutcome " - "derivative implemented. Do not call isSatisfied."); - } - - /// Nonsense. - std::string toString() const override - { - return "This is a dummy constraint outcome. The Testable derivative class " - "you called createOutcome() on has no corresponding TestableOutcome " - "derivative implemented."; - } + /// 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_DUMMYOUTCOME_HPP_ +#endif // AIKIDO_CONSTRAINT_DEFAULTOUTCOME_HPP_ diff --git a/src/constraint/CMakeLists.txt b/src/constraint/CMakeLists.txt index bf5cbe983b..6688a83c07 100644 --- a/src/constraint/CMakeLists.txt +++ b/src/constraint/CMakeLists.txt @@ -10,6 +10,7 @@ set(sources CollisionFree.cpp CollisionFreeOutcome.cpp CyclicSampleable.cpp + DefaultOutcome.cpp Differentiable.cpp DifferentiableIntersection.cpp DifferentiableSubspace.cpp diff --git a/src/constraint/DefaultOutcome.cpp b/src/constraint/DefaultOutcome.cpp new file mode 100644 index 0000000000..ad54b03c5f --- /dev/null +++ b/src/constraint/DefaultOutcome.cpp @@ -0,0 +1,28 @@ +#include + +namespace aikido { +namespace constraint { + +//============================================================================== +bool DefaultOutcome::isSatisfied() const +{ + return mSatisfiedFlag; +} + +//============================================================================== +std::string DefaultOutcome::toString() const +{ + if (mSatisfiedFlag) + return "true"; + + return "false"; +} + +//============================================================================== +void DefaultOutcome::setSatisfiedFlag(bool satisfiedFlag) +{ + mSatisfiedFlag = satisfiedFlag; +} + +} // namespace constraint +} // namespace aikido From cbc1b343f108af430c1fea1fa0947a9ddb27fa8d Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Tue, 5 Dec 2017 20:44:20 -0500 Subject: [PATCH 29/41] 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. --- .../constraint/CartesianProductTestable.hpp | 4 ++ include/aikido/constraint/FrameTestable.hpp | 4 ++ include/aikido/constraint/Satisfied.hpp | 4 ++ include/aikido/constraint/TSR.hpp | 4 ++ include/aikido/constraint/Testable.hpp | 8 +--- .../constraint/TestableIntersection.hpp | 4 ++ .../constraint/uniform/RnBoxConstraint.hpp | 4 ++ .../constraint/uniform/SE2BoxConstraint.hpp | 6 ++- .../uniform/detail/RnBoxConstraint-impl.hpp | 23 +++++++++- src/constraint/CartesianProductTestable.cpp | 24 +++++++++- src/constraint/FrameTestable.cpp | 22 ++++++++- src/constraint/Satisfied.cpp | 19 +++++++- src/constraint/TSR.cpp | 23 +++++++++- src/constraint/TestableIntersection.cpp | 24 +++++++++- src/constraint/uniform/SE2BoxConstraint.cpp | 24 +++++++++- tests/constraint/MockConstraints.hpp | 39 +++++++++++++++- tests/constraint/test_FrameTestable.cpp | 24 ++++++++-- tests/planner/ompl/OMPLTestHelpers.hpp | 46 +++++++++++++++++-- 18 files changed, 282 insertions(+), 24 deletions(-) diff --git a/include/aikido/constraint/CartesianProductTestable.hpp b/include/aikido/constraint/CartesianProductTestable.hpp index 886d572c24..a15a1fa96a 100644 --- a/include/aikido/constraint/CartesianProductTestable.hpp +++ b/include/aikido/constraint/CartesianProductTestable.hpp @@ -29,6 +29,10 @@ class CartesianProductTestable : public Testable const aikido::statespace::StateSpace::State* _state, TestableOutcome* outcome = nullptr) const override; + /// Return an instance of DefaultOutcome, since this class doesn't have a + /// more specialized TestableOutcome derivative assigned to it. + std::unique_ptr createOutcome() const override; + private: std::shared_ptr mStateSpace; std::vector mConstraints; diff --git a/include/aikido/constraint/FrameTestable.hpp b/include/aikido/constraint/FrameTestable.hpp index 16703b313d..48d5b0c3f7 100644 --- a/include/aikido/constraint/FrameTestable.hpp +++ b/include/aikido/constraint/FrameTestable.hpp @@ -33,6 +33,10 @@ class FrameTestable : public Testable const statespace::StateSpace::State* _state, TestableOutcome* outcome = nullptr) const override; + /// Return an instance of DefaultOutcome, since this class doesn't have a + /// more specialized TestableOutcome derivative assigned to it. + std::unique_ptr createOutcome() const override; + // Documentation inhereted std::shared_ptr getStateSpace() const override; diff --git a/include/aikido/constraint/Satisfied.hpp b/include/aikido/constraint/Satisfied.hpp index 592329869a..18fd280cab 100644 --- a/include/aikido/constraint/Satisfied.hpp +++ b/include/aikido/constraint/Satisfied.hpp @@ -38,6 +38,10 @@ class Satisfied : public constraint::Differentiable, const statespace::StateSpace::State* state, TestableOutcome* outcome = nullptr) const override; + /// Return an instance of DefaultOutcome, since this class doesn't have a + /// more specialized TestableOutcome derivative assigned to it. + std::unique_ptr createOutcome() const override; + /// Sets \c _out to \c _s. /// /// \param _s input state diff --git a/include/aikido/constraint/TSR.hpp b/include/aikido/constraint/TSR.hpp index 6ed6943f2d..c22a702755 100644 --- a/include/aikido/constraint/TSR.hpp +++ b/include/aikido/constraint/TSR.hpp @@ -84,6 +84,10 @@ class TSR : public Sampleable, const statespace::StateSpace::State* _s, TestableOutcome* outcome = nullptr) const override; + /// Return an instance of DefaultOutcome, since this class doesn't have a + /// more specialized TestableOutcome derivative assigned to it. + std::unique_ptr 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). void validate() const; diff --git a/include/aikido/constraint/Testable.hpp b/include/aikido/constraint/Testable.hpp index 30a32a63b6..24a7c805cd 100644 --- a/include/aikido/constraint/Testable.hpp +++ b/include/aikido/constraint/Testable.hpp @@ -26,12 +26,8 @@ class Testable /// 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). Default implementation - /// just returns a dummy object. - virtual std::unique_ptr createOutcome() const - { - return std::unique_ptr(new DefaultOutcome()); - } + /// to isSatisfied (and casts, etc do not explode). + virtual std::unique_ptr createOutcome() const = 0; }; using TestablePtr = std::shared_ptr; diff --git a/include/aikido/constraint/TestableIntersection.hpp b/include/aikido/constraint/TestableIntersection.hpp index 306f1d6991..bd0ac4b396 100644 --- a/include/aikido/constraint/TestableIntersection.hpp +++ b/include/aikido/constraint/TestableIntersection.hpp @@ -26,6 +26,10 @@ class TestableIntersection : public Testable const aikido::statespace::StateSpace::State* state, TestableOutcome* outcome = nullptr) const override; + /// Return an instance of DefaultOutcome, since this class doesn't have a + /// more specialized TestableOutcome derivative assigned to it. + std::unique_ptr createOutcome() const override; + // Documentation inherited. statespace::StateSpacePtr getStateSpace() const override; diff --git a/include/aikido/constraint/uniform/RnBoxConstraint.hpp b/include/aikido/constraint/uniform/RnBoxConstraint.hpp index 63bf7b7601..9c3932835c 100644 --- a/include/aikido/constraint/uniform/RnBoxConstraint.hpp +++ b/include/aikido/constraint/uniform/RnBoxConstraint.hpp @@ -50,6 +50,10 @@ class RBoxConstraint : public constraint::Differentiable, const statespace::StateSpace::State* state, TestableOutcome* outcome = nullptr) const override; + /// Return an instance of DefaultOutcome, since this class doesn't have a + /// more specialized TestableOutcome derivative assigned to it. + std::unique_ptr createOutcome() const override; + // Documentation inherited. bool project( const statespace::StateSpace::State* _s, diff --git a/include/aikido/constraint/uniform/SE2BoxConstraint.hpp b/include/aikido/constraint/uniform/SE2BoxConstraint.hpp index d5382b997e..efc0344e9d 100644 --- a/include/aikido/constraint/uniform/SE2BoxConstraint.hpp +++ b/include/aikido/constraint/uniform/SE2BoxConstraint.hpp @@ -35,11 +35,15 @@ class SE2BoxConstraint : public constraint::Projectable, // Documentation inherited. statespace::StateSpacePtr getStateSpace() const override; - // Documentation inherited. + /// Documentation inherited. bool isSatisfied( const statespace::StateSpace::State* state, TestableOutcome* outcome = nullptr) const override; + /// Return an instance of DefaultOutcome, since this class doesn't have a + /// more specialized TestableOutcome derivative assigned to it. + std::unique_ptr createOutcome() const override; + // Documentation inherited. bool project( const statespace::StateSpace::State* s, diff --git a/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp b/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp index 748513d4a4..bfc223e905 100644 --- a/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp +++ b/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp @@ -176,19 +176,40 @@ std::vector RBoxConstraint::getConstraintTypes() const template bool RBoxConstraint::isSatisfied( const statespace::StateSpace::State* state, - TestableOutcome* /*outcome*/) const + TestableOutcome* outcome) const { + DefaultOutcome* defaultOutcomeObject = nullptr; + if (outcome) + { + defaultOutcomeObject = dynamic_cast(outcome); + if (!defaultOutcomeObject) + throw std::invalid_argument( + "TestableOutcome pointer is not of type DefaultOutcome."); + } + const auto value = mSpace->getValue( static_cast::State*>(state)); for (auto i = 0; i < value.size(); ++i) { if (value[i] < mLowerLimits[i] || value[i] > mUpperLimits[i]) + { + defaultOutcomeObject->setSatisfiedFlag(false); return false; + } } + + defaultOutcomeObject->setSatisfiedFlag(true); return true; } +//============================================================================== +template +std::unique_ptr RBoxConstraint::createOutcome() const +{ + return std::unique_ptr(new DefaultOutcome()); +} + //============================================================================== template bool RBoxConstraint::project( diff --git a/src/constraint/CartesianProductTestable.cpp b/src/constraint/CartesianProductTestable.cpp index fbf6e5030a..d63d2b8db9 100644 --- a/src/constraint/CartesianProductTestable.cpp +++ b/src/constraint/CartesianProductTestable.cpp @@ -54,8 +54,17 @@ statespace::StateSpacePtr CartesianProductTestable::getStateSpace() const //============================================================================== bool CartesianProductTestable::isSatisfied( const aikido::statespace::StateSpace::State* _state, - TestableOutcome* /*outcome*/) const + TestableOutcome* outcome) const { + DefaultOutcome* defaultOutcomeObject = nullptr; + if (outcome) + { + defaultOutcomeObject = dynamic_cast(outcome); + if (!defaultOutcomeObject) + throw std::invalid_argument( + "TestableOutcome pointer is not of type DefaultOutcome."); + } + const auto state = static_cast(_state); @@ -63,10 +72,23 @@ bool CartesianProductTestable::isSatisfied( { auto subState = mStateSpace->getSubState<>(state, i); if (!mConstraints[i]->isSatisfied(subState)) + { + if (defaultOutcomeObject) + defaultOutcomeObject->setSatisfiedFlag(false); return false; + } } + + if (defaultOutcomeObject) + defaultOutcomeObject->setSatisfiedFlag(true); return true; } +//============================================================================== +std::unique_ptr CartesianProductTestable::createOutcome() const +{ + return std::unique_ptr(new DefaultOutcome()); +} + } // namespace constraint } // namespace aikido diff --git a/src/constraint/FrameTestable.cpp b/src/constraint/FrameTestable.cpp index d8d8ee77a8..25342bd4ed 100644 --- a/src/constraint/FrameTestable.cpp +++ b/src/constraint/FrameTestable.cpp @@ -35,8 +35,17 @@ FrameTestable::FrameTestable( //============================================================================== bool FrameTestable::isSatisfied( const statespace::StateSpace::State* _state, - TestableOutcome* /*outcome*/) const + TestableOutcome* outcome) const { + DefaultOutcome* defaultOutcomeObject = nullptr; + if (outcome) + { + defaultOutcomeObject = dynamic_cast(outcome); + if (!defaultOutcomeObject) + throw std::invalid_argument( + "TestableOutcome pointer is not of type DefaultOutcome."); + } + // Set the state auto state = static_cast( @@ -47,7 +56,16 @@ bool FrameTestable::isSatisfied( auto st = mPoseStateSpace->createState(); mPoseStateSpace->setIsometry(st, mFrame->getTransform()); - return mPoseConstraint->isSatisfied(st); + bool isSatisfiedResult = mPoseConstraint->isSatisfied(st); + if (defaultOutcomeObject) + defaultOutcomeObject->setSatisfiedFlag(isSatisfiedResult); + return isSatisfiedResult; +} + +//============================================================================== +std::unique_ptr FrameTestable::createOutcome() const +{ + return std::unique_ptr(new DefaultOutcome()); } //============================================================================== diff --git a/src/constraint/Satisfied.cpp b/src/constraint/Satisfied.cpp index b0ec12e817..be5496c124 100644 --- a/src/constraint/Satisfied.cpp +++ b/src/constraint/Satisfied.cpp @@ -32,11 +32,28 @@ std::vector Satisfied::getConstraintTypes() const //============================================================================== bool Satisfied::isSatisfied( const statespace::StateSpace::State* /*state*/, - TestableOutcome* /*outcome*/) const + TestableOutcome* outcome) const { + DefaultOutcome* defaultOutcomeObject = nullptr; + if (outcome) + { + defaultOutcomeObject = dynamic_cast(outcome); + if (!defaultOutcomeObject) + throw std::invalid_argument( + "TestableOutcome pointer is not of type DefaultOutcome."); + } + + if (defaultOutcomeObject) + defaultOutcomeObject->setSatisfiedFlag(true); return true; } +//============================================================================== +std::unique_ptr Satisfied::createOutcome() const +{ + return std::unique_ptr(new DefaultOutcome()); +} + //============================================================================== bool Satisfied::project( const statespace::StateSpace::State* _s, diff --git a/src/constraint/TSR.cpp b/src/constraint/TSR.cpp index 3369792867..32a8ec6664 100644 --- a/src/constraint/TSR.cpp +++ b/src/constraint/TSR.cpp @@ -211,11 +211,30 @@ std::unique_ptr TSR::createSampleGenerator() const //============================================================================== bool TSR::isSatisfied( - const statespace::StateSpace::State* _s, TestableOutcome* /*outcome*/) const + const statespace::StateSpace::State* _s, TestableOutcome* outcome) const { + DefaultOutcome* defaultOutcomeObject = nullptr; + if (outcome) + { + defaultOutcomeObject = dynamic_cast(outcome); + if (!defaultOutcomeObject) + throw std::invalid_argument( + "TestableOutcome pointer is not of type DefaultOutcome."); + } + Eigen::VectorXd dist; getValue(_s, dist); - return dist.norm() < mTestableTolerance; + + bool isSatisfiedResult = dist.norm() < mTestableTolerance; + if (defaultOutcomeObject) + defaultOutcomeObject->setSatisfiedFlag(isSatisfiedResult); + return isSatisfiedResult; +} + +//============================================================================== +std::unique_ptr TSR::createOutcome() const +{ + return std::unique_ptr(new DefaultOutcome()); } //============================================================================== diff --git a/src/constraint/TestableIntersection.cpp b/src/constraint/TestableIntersection.cpp index 790d7ae3d4..667579695e 100644 --- a/src/constraint/TestableIntersection.cpp +++ b/src/constraint/TestableIntersection.cpp @@ -21,16 +21,38 @@ TestableIntersection::TestableIntersection( //============================================================================== bool TestableIntersection::isSatisfied( const aikido::statespace::StateSpace::State* _state, - TestableOutcome* /*outcome*/) const + TestableOutcome* outcome) const { + DefaultOutcome* defaultOutcomeObject = nullptr; + if (outcome) + { + defaultOutcomeObject = dynamic_cast(outcome); + if (!defaultOutcomeObject) + throw std::invalid_argument( + "TestableOutcome pointer is not of type DefaultOutcome."); + } + for (auto c : mConstraints) { if (!c->isSatisfied(_state)) + { + if (defaultOutcomeObject) + defaultOutcomeObject->setSatisfiedFlag(false); return false; + } } + + if (defaultOutcomeObject) + defaultOutcomeObject->setSatisfiedFlag(true); return true; } +//============================================================================== +std::unique_ptr TestableIntersection::createOutcome() const +{ + return std::unique_ptr(new DefaultOutcome()); +} + //============================================================================== statespace::StateSpacePtr TestableIntersection::getStateSpace() const { diff --git a/src/constraint/uniform/SE2BoxConstraint.cpp b/src/constraint/uniform/SE2BoxConstraint.cpp index d906d015ff..764390fd0e 100644 --- a/src/constraint/uniform/SE2BoxConstraint.cpp +++ b/src/constraint/uniform/SE2BoxConstraint.cpp @@ -138,19 +138,41 @@ statespace::StateSpacePtr SE2BoxConstraint::getStateSpace() const //============================================================================== bool SE2BoxConstraint::isSatisfied( const statespace::StateSpace::State* state, - TestableOutcome* /*outcome*/) const + TestableOutcome* outcome) const { + DefaultOutcome* defaultOutcomeObject = nullptr; + if (outcome) + { + defaultOutcomeObject = dynamic_cast(outcome); + if (!defaultOutcomeObject) + throw std::invalid_argument( + "TestableOutcome pointer is not of type DefaultOutcome."); + } + Eigen::VectorXd tangent; mSpace->logMap(static_cast(state), tangent); for (std::size_t i = mDimension - mRnDimension; i < mDimension; ++i) { if (tangent[i] < mLowerLimits[i] || tangent[i] > mUpperLimits[i]) + { + if (defaultOutcomeObject) + defaultOutcomeObject->setSatisfiedFlag(false); return false; + } } + + if (defaultOutcomeObject) + defaultOutcomeObject->setSatisfiedFlag(true); return true; } +//============================================================================== +std::unique_ptr SE2BoxConstraint::createOutcome() const +{ + return std::unique_ptr(new DefaultOutcome()); +} + //============================================================================== bool SE2BoxConstraint::project( const statespace::StateSpace::State* s, diff --git a/tests/constraint/MockConstraints.hpp b/tests/constraint/MockConstraints.hpp index 280594278b..0a9f580a07 100644 --- a/tests/constraint/MockConstraints.hpp +++ b/tests/constraint/MockConstraints.hpp @@ -4,6 +4,9 @@ #include #include +using aikido::constraint::DefaultOutcome; +using aikido::constraint::TestableOutcome; + class PassingConstraint : public aikido::constraint::Testable { public: @@ -15,11 +18,27 @@ class PassingConstraint : public aikido::constraint::Testable bool isSatisfied( const aikido::statespace::StateSpace::State* /*state*/, - aikido::constraint::TestableOutcome* /*outcome*/ = nullptr) const override + TestableOutcome* outcome = nullptr) const override { + DefaultOutcome* defaultOutcomeObject = nullptr; + if (outcome) + { + defaultOutcomeObject = dynamic_cast(outcome); + if (!defaultOutcomeObject) + throw std::invalid_argument( + "TestableOutcome pointer is not of type DefaultOutcome."); + } + + if (defaultOutcomeObject) + defaultOutcomeObject->setSatisfiedFlag(true); return true; } + std::unique_ptr createOutcome() const + { + return std::unique_ptr(new DefaultOutcome()); + } + std::shared_ptr getStateSpace() const override { return stateSpace; @@ -40,11 +59,27 @@ class FailingConstraint : public aikido::constraint::Testable bool isSatisfied( const aikido::statespace::StateSpace::State* /*state*/, - aikido::constraint::TestableOutcome* /*outcome*/ = nullptr) const override + TestableOutcome* outcome = nullptr) const override { + DefaultOutcome* defaultOutcomeObject = nullptr; + if (outcome) + { + defaultOutcomeObject = dynamic_cast(outcome); + if (!defaultOutcomeObject) + throw std::invalid_argument( + "TestableOutcome pointer is not of type DefaultOutcome."); + } + + if (defaultOutcomeObject) + defaultOutcomeObject->setSatisfiedFlag(false); return false; } + std::unique_ptr createOutcome() const + { + return std::unique_ptr(new DefaultOutcome()); + } + std::shared_ptr getStateSpace() const override { return stateSpace; diff --git a/tests/constraint/test_FrameTestable.cpp b/tests/constraint/test_FrameTestable.cpp index 144a348a8d..0904bf2616 100644 --- a/tests/constraint/test_FrameTestable.cpp +++ b/tests/constraint/test_FrameTestable.cpp @@ -4,6 +4,7 @@ #include #include "MockConstraints.hpp" +using aikido::constraint::DefaultOutcome; using aikido::constraint::FrameTestable; using aikido::constraint::Testable; using aikido::constraint::TestablePtr; @@ -30,13 +31,30 @@ class EndEffectorTestable : public aikido::constraint::Testable bool isSatisfied( const aikido::statespace::StateSpace::State* _state, - aikido::constraint::TestableOutcome* /*outcome*/ = nullptr) const override + aikido::constraint::TestableOutcome* outcome = nullptr) const override { + DefaultOutcome* defaultOutcomeObject = nullptr; + if (outcome) + { + defaultOutcomeObject = dynamic_cast(outcome); + if (!defaultOutcomeObject) + throw std::invalid_argument( + "TestableOutcome pointer is not of type DefaultOutcome."); + } + auto st = static_cast(_state); auto val = st->getIsometry(); auto trans = val.translation(); - return ( - trans(0) > 0.5 && trans(0) < 1.5 && trans(1) > 0.0 && trans(1) < 1.0); + + bool isSatisfiedResult = trans(0) > 0.5 && trans(0) < 1.5 && trans(1) > 0.0 && trans(1) < 1.0; + if (defaultOutcomeObject) + defaultOutcomeObject->setSatisfiedFlag(isSatisfiedResult); + return isSatisfiedResult; + } + + std::unique_ptr createOutcome() const + { + return std::unique_ptr(new DefaultOutcome()); } private: diff --git a/tests/planner/ompl/OMPLTestHelpers.hpp b/tests/planner/ompl/OMPLTestHelpers.hpp index eb1e648d39..bd599c6e9e 100644 --- a/tests/planner/ompl/OMPLTestHelpers.hpp +++ b/tests/planner/ompl/OMPLTestHelpers.hpp @@ -15,6 +15,8 @@ #include using dart::common::make_unique; +using aikido::constraint::DefaultOutcome; +using aikido::constraint::TestableOutcome; using aikido::statespace::CartesianProduct; using aikido::statespace::R3; using aikido::planner::ompl::GeometricStateSpace; @@ -155,11 +157,30 @@ class MockProjectionConstraint : public aikido::constraint::Projectable, bool isSatisfied( const aikido::statespace::StateSpace::State* _s, - aikido::constraint::TestableOutcome* /*outcome*/ = nullptr) const override + TestableOutcome* outcome = nullptr) const override { + DefaultOutcome* defaultOutcomeObject = nullptr; + if (outcome) + { + defaultOutcomeObject = dynamic_cast(outcome); + if (!defaultOutcomeObject) + throw std::invalid_argument( + "TestableOutcome pointer is not of type DefaultOutcome."); + } + auto state = static_cast(_s); auto val = mStateSpace->getSubStateHandle(state, 0).getValue(); - return std::fabs(val[0] - mValue) < 1e-6; + + + bool isSatisfiedResult = std::fabs(val[0] - mValue) < 1e-6; + if (defaultOutcomeObject) + defaultOutcomeObject->setSatisfiedFlag(isSatisfiedResult); + return isSatisfiedResult; + } + + std::unique_ptr createOutcome() const + { + return std::unique_ptr(new DefaultOutcome()); } std::unique_ptr createSampleGenerator() @@ -192,8 +213,17 @@ class MockTranslationalRobotConstraint : public aikido::constraint::Testable // Documentation inherited bool isSatisfied( const aikido::statespace::StateSpace::State* _state, - aikido::constraint::TestableOutcome* /*outcome*/ = nullptr) const override + TestableOutcome* outcome = nullptr) const override { + DefaultOutcome* defaultOutcomeObject = nullptr; + if (outcome) + { + defaultOutcomeObject = dynamic_cast(outcome); + if (!defaultOutcomeObject) + throw std::invalid_argument( + "TestableOutcome pointer is not of type DefaultOutcome."); + } + auto cst = static_cast(_state); auto subState = mStateSpace->getSubStateHandle(cst, 0); auto val = subState.getValue(); @@ -202,13 +232,23 @@ class MockTranslationalRobotConstraint : public aikido::constraint::Testable { if (val[i] < mLowerBounds[i] || val[i] > mUpperBounds[i]) { + if (defaultOutcomeObject) + defaultOutcomeObject->setSatisfiedFlag(true); return true; } } + if (defaultOutcomeObject) + defaultOutcomeObject->setSatisfiedFlag(false); return false; } + std::unique_ptr createOutcome() const + { + return std::unique_ptr(new DefaultOutcome()); + } + + // Documentatoin inherited std::shared_ptr getStateSpace() const override { From 7a3b7622e1f115a1468734fc46e6152ad3b76e09 Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Tue, 5 Dec 2017 20:47:17 -0500 Subject: [PATCH 30/41] Run make format. --- include/aikido/constraint/DefaultOutcome.hpp | 1 - .../aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp | 3 +-- src/constraint/CartesianProductTestable.cpp | 2 +- src/constraint/DefaultOutcome.cpp | 2 +- src/constraint/FrameTestable.cpp | 3 +-- src/constraint/uniform/SE2BoxConstraint.cpp | 3 +-- tests/constraint/test_FrameTestable.cpp | 3 ++- tests/planner/ompl/OMPLTestHelpers.hpp | 2 -- 8 files changed, 7 insertions(+), 12 deletions(-) diff --git a/include/aikido/constraint/DefaultOutcome.hpp b/include/aikido/constraint/DefaultOutcome.hpp index c0ff548341..9d3a2b4b4d 100644 --- a/include/aikido/constraint/DefaultOutcome.hpp +++ b/include/aikido/constraint/DefaultOutcome.hpp @@ -25,7 +25,6 @@ class DefaultOutcome : public TestableOutcome /// \param[in] satisfiedFlag whether the constraint was satisfied or not. void setSatisfiedFlag(bool satisfiedFlag); - protected: bool mSatisfiedFlag; }; diff --git a/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp b/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp index bfc223e905..d988b06e81 100644 --- a/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp +++ b/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp @@ -175,8 +175,7 @@ std::vector RBoxConstraint::getConstraintTypes() const //============================================================================== template bool RBoxConstraint::isSatisfied( - const statespace::StateSpace::State* state, - TestableOutcome* outcome) const + const statespace::StateSpace::State* state, TestableOutcome* outcome) const { DefaultOutcome* defaultOutcomeObject = nullptr; if (outcome) diff --git a/src/constraint/CartesianProductTestable.cpp b/src/constraint/CartesianProductTestable.cpp index d63d2b8db9..2d2c9ee70d 100644 --- a/src/constraint/CartesianProductTestable.cpp +++ b/src/constraint/CartesianProductTestable.cpp @@ -76,7 +76,7 @@ bool CartesianProductTestable::isSatisfied( if (defaultOutcomeObject) defaultOutcomeObject->setSatisfiedFlag(false); return false; - } + } } if (defaultOutcomeObject) diff --git a/src/constraint/DefaultOutcome.cpp b/src/constraint/DefaultOutcome.cpp index ad54b03c5f..275678e059 100644 --- a/src/constraint/DefaultOutcome.cpp +++ b/src/constraint/DefaultOutcome.cpp @@ -14,7 +14,7 @@ std::string DefaultOutcome::toString() const { if (mSatisfiedFlag) return "true"; - + return "false"; } diff --git a/src/constraint/FrameTestable.cpp b/src/constraint/FrameTestable.cpp index 25342bd4ed..e7cec8068e 100644 --- a/src/constraint/FrameTestable.cpp +++ b/src/constraint/FrameTestable.cpp @@ -34,8 +34,7 @@ FrameTestable::FrameTestable( //============================================================================== bool FrameTestable::isSatisfied( - const statespace::StateSpace::State* _state, - TestableOutcome* outcome) const + const statespace::StateSpace::State* _state, TestableOutcome* outcome) const { DefaultOutcome* defaultOutcomeObject = nullptr; if (outcome) diff --git a/src/constraint/uniform/SE2BoxConstraint.cpp b/src/constraint/uniform/SE2BoxConstraint.cpp index 764390fd0e..0a015fb9ee 100644 --- a/src/constraint/uniform/SE2BoxConstraint.cpp +++ b/src/constraint/uniform/SE2BoxConstraint.cpp @@ -137,8 +137,7 @@ statespace::StateSpacePtr SE2BoxConstraint::getStateSpace() const //============================================================================== bool SE2BoxConstraint::isSatisfied( - const statespace::StateSpace::State* state, - TestableOutcome* outcome) const + const statespace::StateSpace::State* state, TestableOutcome* outcome) const { DefaultOutcome* defaultOutcomeObject = nullptr; if (outcome) diff --git a/tests/constraint/test_FrameTestable.cpp b/tests/constraint/test_FrameTestable.cpp index 0904bf2616..3d513e7c32 100644 --- a/tests/constraint/test_FrameTestable.cpp +++ b/tests/constraint/test_FrameTestable.cpp @@ -46,7 +46,8 @@ class EndEffectorTestable : public aikido::constraint::Testable auto val = st->getIsometry(); auto trans = val.translation(); - bool isSatisfiedResult = trans(0) > 0.5 && trans(0) < 1.5 && trans(1) > 0.0 && trans(1) < 1.0; + bool isSatisfiedResult + = trans(0) > 0.5 && trans(0) < 1.5 && trans(1) > 0.0 && trans(1) < 1.0; if (defaultOutcomeObject) defaultOutcomeObject->setSatisfiedFlag(isSatisfiedResult); return isSatisfiedResult; diff --git a/tests/planner/ompl/OMPLTestHelpers.hpp b/tests/planner/ompl/OMPLTestHelpers.hpp index bd599c6e9e..9077105d3c 100644 --- a/tests/planner/ompl/OMPLTestHelpers.hpp +++ b/tests/planner/ompl/OMPLTestHelpers.hpp @@ -171,7 +171,6 @@ class MockProjectionConstraint : public aikido::constraint::Projectable, auto state = static_cast(_s); auto val = mStateSpace->getSubStateHandle(state, 0).getValue(); - bool isSatisfiedResult = std::fabs(val[0] - mValue) < 1e-6; if (defaultOutcomeObject) defaultOutcomeObject->setSatisfiedFlag(isSatisfiedResult); @@ -248,7 +247,6 @@ class MockTranslationalRobotConstraint : public aikido::constraint::Testable return std::unique_ptr(new DefaultOutcome()); } - // Documentatoin inherited std::shared_ptr getStateSpace() const override { From 079b2f3d8fb559f614d1c690fa8e8454d58c063b Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Wed, 6 Dec 2017 16:54:49 -0500 Subject: [PATCH 31/41] [WIP] Refactor castic logic into helper, and use this in CollisionFree. Will stamp this into every other class now. --- include/aikido/constraint/TestableOutcome.hpp | 24 ++++++++++++++++++- src/constraint/CollisionFree.cpp | 10 +++----- 2 files changed, 26 insertions(+), 8 deletions(-) diff --git a/include/aikido/constraint/TestableOutcome.hpp b/include/aikido/constraint/TestableOutcome.hpp index 8e2e58927e..18b6227ed0 100644 --- a/include/aikido/constraint/TestableOutcome.hpp +++ b/include/aikido/constraint/TestableOutcome.hpp @@ -1,7 +1,9 @@ #ifndef AIKIDO_CONSTRAINT_TESTABLEOUTCOME_HPP_ #define AIKIDO_CONSTRAINT_TESTABLEOUTCOME_HPP_ -#include +#include +#include +#include namespace aikido { namespace constraint { @@ -22,6 +24,26 @@ class TestableOutcome 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 +Child* dynamic_cast_if_present(TestableOutcome* _outcome) +{ + if (!_outcome) + return nullptr; + + auto childPtr = dynamic_cast(_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 diff --git a/src/constraint/CollisionFree.cpp b/src/constraint/CollisionFree.cpp index 3e6c224b79..5badaeea32 100644 --- a/src/constraint/CollisionFree.cpp +++ b/src/constraint/CollisionFree.cpp @@ -30,14 +30,10 @@ bool CollisionFree::isSatisfied( const aikido::statespace::StateSpace::State* _state, TestableOutcome* outcome) const { - CollisionFreeOutcome* collisionFreeOutcome = nullptr; - if (outcome) + auto collisionFreeOutcome = dynamic_cast_if_present(outcome); + + if (collisionFreeOutcome) { - collisionFreeOutcome = dynamic_cast(outcome); - if (!collisionFreeOutcome) - throw std::invalid_argument( - "TestableOutcome pointer is not of type CollisionFreeOutcome."); - // Clear the outcome object in case it's being reused. collisionFreeOutcome->clear(); } From e9908625dc6f76af85674920cdb2f2207adcf7c0 Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Wed, 6 Dec 2017 17:15:58 -0500 Subject: [PATCH 32/41] [WIP] Replace redundant casting login in different Testables with helper. --- include/aikido/constraint/TestableOutcome.hpp | 3 ++- .../uniform/detail/RnBoxConstraint-impl.hpp | 17 ++++++---------- src/constraint/CartesianProductTestable.cpp | 9 +-------- src/constraint/CollisionFree.cpp | 3 ++- src/constraint/FrameTestable.cpp | 9 +-------- src/constraint/Satisfied.cpp | 9 +-------- src/constraint/TSR.cpp | 9 +-------- src/constraint/TestableIntersection.cpp | 9 +-------- src/constraint/uniform/SE2BoxConstraint.cpp | 9 +-------- tests/constraint/MockConstraints.hpp | 14 +++++-------- tests/constraint/test_FrameTestable.cpp | 3 ++- tests/planner/ompl/OMPLTestHelpers.hpp | 20 ++++--------------- 12 files changed, 27 insertions(+), 87 deletions(-) diff --git a/include/aikido/constraint/TestableOutcome.hpp b/include/aikido/constraint/TestableOutcome.hpp index 18b6227ed0..7830c44589 100644 --- a/include/aikido/constraint/TestableOutcome.hpp +++ b/include/aikido/constraint/TestableOutcome.hpp @@ -37,7 +37,8 @@ Child* dynamic_cast_if_present(TestableOutcome* _outcome) if (!childPtr) { std::stringstream message; - message << "TestableOutcome pointer is not of type " << typeid(Child).name() << "."; + message << "TestableOutcome pointer is not of type " << typeid(Child).name() + << "."; throw std::invalid_argument(message.str()); } diff --git a/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp b/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp index d988b06e81..f6d4680a0a 100644 --- a/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp +++ b/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp @@ -177,14 +177,7 @@ template bool RBoxConstraint::isSatisfied( const statespace::StateSpace::State* state, TestableOutcome* outcome) const { - DefaultOutcome* defaultOutcomeObject = nullptr; - if (outcome) - { - defaultOutcomeObject = dynamic_cast(outcome); - if (!defaultOutcomeObject) - throw std::invalid_argument( - "TestableOutcome pointer is not of type DefaultOutcome."); - } + auto defaultOutcomeObject = dynamic_cast_if_present(outcome); const auto value = mSpace->getValue( static_cast::State*>(state)); @@ -193,12 +186,14 @@ bool RBoxConstraint::isSatisfied( { if (value[i] < mLowerLimits[i] || value[i] > mUpperLimits[i]) { - defaultOutcomeObject->setSatisfiedFlag(false); + if (defaultOutcomeObject) + defaultOutcomeObject->setSatisfiedFlag(false); return false; } } - defaultOutcomeObject->setSatisfiedFlag(true); + if (defaultOutcomeObject) + defaultOutcomeObject->setSatisfiedFlag(true); return true; } @@ -206,7 +201,7 @@ bool RBoxConstraint::isSatisfied( template std::unique_ptr RBoxConstraint::createOutcome() const { - return std::unique_ptr(new DefaultOutcome()); + return std::unique_ptr(new DefaultOutcome); } //============================================================================== diff --git a/src/constraint/CartesianProductTestable.cpp b/src/constraint/CartesianProductTestable.cpp index 2d2c9ee70d..93c22415e6 100644 --- a/src/constraint/CartesianProductTestable.cpp +++ b/src/constraint/CartesianProductTestable.cpp @@ -56,14 +56,7 @@ bool CartesianProductTestable::isSatisfied( const aikido::statespace::StateSpace::State* _state, TestableOutcome* outcome) const { - DefaultOutcome* defaultOutcomeObject = nullptr; - if (outcome) - { - defaultOutcomeObject = dynamic_cast(outcome); - if (!defaultOutcomeObject) - throw std::invalid_argument( - "TestableOutcome pointer is not of type DefaultOutcome."); - } + auto defaultOutcomeObject = dynamic_cast_if_present(outcome); const auto state = static_cast(_state); diff --git a/src/constraint/CollisionFree.cpp b/src/constraint/CollisionFree.cpp index 5badaeea32..b5d759a853 100644 --- a/src/constraint/CollisionFree.cpp +++ b/src/constraint/CollisionFree.cpp @@ -30,7 +30,8 @@ bool CollisionFree::isSatisfied( const aikido::statespace::StateSpace::State* _state, TestableOutcome* outcome) const { - auto collisionFreeOutcome = dynamic_cast_if_present(outcome); + auto collisionFreeOutcome + = dynamic_cast_if_present(outcome); if (collisionFreeOutcome) { diff --git a/src/constraint/FrameTestable.cpp b/src/constraint/FrameTestable.cpp index e7cec8068e..c6037c60f2 100644 --- a/src/constraint/FrameTestable.cpp +++ b/src/constraint/FrameTestable.cpp @@ -36,14 +36,7 @@ FrameTestable::FrameTestable( bool FrameTestable::isSatisfied( const statespace::StateSpace::State* _state, TestableOutcome* outcome) const { - DefaultOutcome* defaultOutcomeObject = nullptr; - if (outcome) - { - defaultOutcomeObject = dynamic_cast(outcome); - if (!defaultOutcomeObject) - throw std::invalid_argument( - "TestableOutcome pointer is not of type DefaultOutcome."); - } + auto defaultOutcomeObject = dynamic_cast_if_present(outcome); // Set the state auto state diff --git a/src/constraint/Satisfied.cpp b/src/constraint/Satisfied.cpp index be5496c124..af3658f612 100644 --- a/src/constraint/Satisfied.cpp +++ b/src/constraint/Satisfied.cpp @@ -34,14 +34,7 @@ bool Satisfied::isSatisfied( const statespace::StateSpace::State* /*state*/, TestableOutcome* outcome) const { - DefaultOutcome* defaultOutcomeObject = nullptr; - if (outcome) - { - defaultOutcomeObject = dynamic_cast(outcome); - if (!defaultOutcomeObject) - throw std::invalid_argument( - "TestableOutcome pointer is not of type DefaultOutcome."); - } + auto defaultOutcomeObject = dynamic_cast_if_present(outcome); if (defaultOutcomeObject) defaultOutcomeObject->setSatisfiedFlag(true); diff --git a/src/constraint/TSR.cpp b/src/constraint/TSR.cpp index 32a8ec6664..b529d54723 100644 --- a/src/constraint/TSR.cpp +++ b/src/constraint/TSR.cpp @@ -213,14 +213,7 @@ std::unique_ptr TSR::createSampleGenerator() const bool TSR::isSatisfied( const statespace::StateSpace::State* _s, TestableOutcome* outcome) const { - DefaultOutcome* defaultOutcomeObject = nullptr; - if (outcome) - { - defaultOutcomeObject = dynamic_cast(outcome); - if (!defaultOutcomeObject) - throw std::invalid_argument( - "TestableOutcome pointer is not of type DefaultOutcome."); - } + auto defaultOutcomeObject = dynamic_cast_if_present(outcome); Eigen::VectorXd dist; getValue(_s, dist); diff --git a/src/constraint/TestableIntersection.cpp b/src/constraint/TestableIntersection.cpp index 667579695e..272e7605e3 100644 --- a/src/constraint/TestableIntersection.cpp +++ b/src/constraint/TestableIntersection.cpp @@ -23,14 +23,7 @@ bool TestableIntersection::isSatisfied( const aikido::statespace::StateSpace::State* _state, TestableOutcome* outcome) const { - DefaultOutcome* defaultOutcomeObject = nullptr; - if (outcome) - { - defaultOutcomeObject = dynamic_cast(outcome); - if (!defaultOutcomeObject) - throw std::invalid_argument( - "TestableOutcome pointer is not of type DefaultOutcome."); - } + auto defaultOutcomeObject = dynamic_cast_if_present(outcome); for (auto c : mConstraints) { diff --git a/src/constraint/uniform/SE2BoxConstraint.cpp b/src/constraint/uniform/SE2BoxConstraint.cpp index 0a015fb9ee..b05d1731f1 100644 --- a/src/constraint/uniform/SE2BoxConstraint.cpp +++ b/src/constraint/uniform/SE2BoxConstraint.cpp @@ -139,14 +139,7 @@ statespace::StateSpacePtr SE2BoxConstraint::getStateSpace() const bool SE2BoxConstraint::isSatisfied( const statespace::StateSpace::State* state, TestableOutcome* outcome) const { - DefaultOutcome* defaultOutcomeObject = nullptr; - if (outcome) - { - defaultOutcomeObject = dynamic_cast(outcome); - if (!defaultOutcomeObject) - throw std::invalid_argument( - "TestableOutcome pointer is not of type DefaultOutcome."); - } + auto defaultOutcomeObject = dynamic_cast_if_present(outcome); Eigen::VectorXd tangent; mSpace->logMap(static_cast(state), tangent); diff --git a/tests/constraint/MockConstraints.hpp b/tests/constraint/MockConstraints.hpp index 0a9f580a07..b4e887ab57 100644 --- a/tests/constraint/MockConstraints.hpp +++ b/tests/constraint/MockConstraints.hpp @@ -20,14 +20,8 @@ class PassingConstraint : public aikido::constraint::Testable const aikido::statespace::StateSpace::State* /*state*/, TestableOutcome* outcome = nullptr) const override { - DefaultOutcome* defaultOutcomeObject = nullptr; - if (outcome) - { - defaultOutcomeObject = dynamic_cast(outcome); - if (!defaultOutcomeObject) - throw std::invalid_argument( - "TestableOutcome pointer is not of type DefaultOutcome."); - } + auto defaultOutcomeObject + = aikido::constraint::dynamic_cast_if_present(outcome); if (defaultOutcomeObject) defaultOutcomeObject->setSatisfiedFlag(true); @@ -61,7 +55,9 @@ class FailingConstraint : public aikido::constraint::Testable const aikido::statespace::StateSpace::State* /*state*/, TestableOutcome* outcome = nullptr) const override { - DefaultOutcome* defaultOutcomeObject = nullptr; + auto defaultOutcomeObject + = aikido::constraint::dynamic_cast_if_present(outcome); + if (outcome) { defaultOutcomeObject = dynamic_cast(outcome); diff --git a/tests/constraint/test_FrameTestable.cpp b/tests/constraint/test_FrameTestable.cpp index 3d513e7c32..266ba706d6 100644 --- a/tests/constraint/test_FrameTestable.cpp +++ b/tests/constraint/test_FrameTestable.cpp @@ -33,7 +33,8 @@ class EndEffectorTestable : public aikido::constraint::Testable const aikido::statespace::StateSpace::State* _state, aikido::constraint::TestableOutcome* outcome = nullptr) const override { - DefaultOutcome* defaultOutcomeObject = nullptr; + auto defaultOutcomeObject + = aikido::constraint::dynamic_cast_if_present(outcome); if (outcome) { defaultOutcomeObject = dynamic_cast(outcome); diff --git a/tests/planner/ompl/OMPLTestHelpers.hpp b/tests/planner/ompl/OMPLTestHelpers.hpp index 9077105d3c..54530579b0 100644 --- a/tests/planner/ompl/OMPLTestHelpers.hpp +++ b/tests/planner/ompl/OMPLTestHelpers.hpp @@ -159,14 +159,8 @@ class MockProjectionConstraint : public aikido::constraint::Projectable, const aikido::statespace::StateSpace::State* _s, TestableOutcome* outcome = nullptr) const override { - DefaultOutcome* defaultOutcomeObject = nullptr; - if (outcome) - { - defaultOutcomeObject = dynamic_cast(outcome); - if (!defaultOutcomeObject) - throw std::invalid_argument( - "TestableOutcome pointer is not of type DefaultOutcome."); - } + auto defaultOutcomeObject + = aikido::constraint::dynamic_cast_if_present(outcome); auto state = static_cast(_s); auto val = mStateSpace->getSubStateHandle(state, 0).getValue(); @@ -214,14 +208,8 @@ class MockTranslationalRobotConstraint : public aikido::constraint::Testable const aikido::statespace::StateSpace::State* _state, TestableOutcome* outcome = nullptr) const override { - DefaultOutcome* defaultOutcomeObject = nullptr; - if (outcome) - { - defaultOutcomeObject = dynamic_cast(outcome); - if (!defaultOutcomeObject) - throw std::invalid_argument( - "TestableOutcome pointer is not of type DefaultOutcome."); - } + auto defaultOutcomeObject + = aikido::constraint::dynamic_cast_if_present(outcome); auto cst = static_cast(_state); auto subState = mStateSpace->getSubStateHandle(cst, 0); From ce4160d27c1ec3c591755830b75c046b1d831f37 Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Wed, 6 Dec 2017 17:20:42 -0500 Subject: [PATCH 33/41] [WIP] Mop up some extra casts that were missed prior. --- tests/constraint/MockConstraints.hpp | 8 -------- tests/constraint/test_FrameTestable.cpp | 7 ------- 2 files changed, 15 deletions(-) diff --git a/tests/constraint/MockConstraints.hpp b/tests/constraint/MockConstraints.hpp index b4e887ab57..2e557ba07b 100644 --- a/tests/constraint/MockConstraints.hpp +++ b/tests/constraint/MockConstraints.hpp @@ -58,14 +58,6 @@ class FailingConstraint : public aikido::constraint::Testable auto defaultOutcomeObject = aikido::constraint::dynamic_cast_if_present(outcome); - if (outcome) - { - defaultOutcomeObject = dynamic_cast(outcome); - if (!defaultOutcomeObject) - throw std::invalid_argument( - "TestableOutcome pointer is not of type DefaultOutcome."); - } - if (defaultOutcomeObject) defaultOutcomeObject->setSatisfiedFlag(false); return false; diff --git a/tests/constraint/test_FrameTestable.cpp b/tests/constraint/test_FrameTestable.cpp index 266ba706d6..80038b9d3d 100644 --- a/tests/constraint/test_FrameTestable.cpp +++ b/tests/constraint/test_FrameTestable.cpp @@ -35,13 +35,6 @@ class EndEffectorTestable : public aikido::constraint::Testable { auto defaultOutcomeObject = aikido::constraint::dynamic_cast_if_present(outcome); - if (outcome) - { - defaultOutcomeObject = dynamic_cast(outcome); - if (!defaultOutcomeObject) - throw std::invalid_argument( - "TestableOutcome pointer is not of type DefaultOutcome."); - } auto st = static_cast(_state); auto val = st->getIsometry(); From 4768cda5d40b870668c64608327e020738027bc5 Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Wed, 6 Dec 2017 17:39:55 -0500 Subject: [PATCH 34/41] Added documentation. --- include/aikido/constraint/CollisionFree.hpp | 7 +++++-- include/aikido/constraint/Testable.hpp | 6 ++++++ include/aikido/constraint/TestableOutcome.hpp | 6 +++--- 3 files changed, 14 insertions(+), 5 deletions(-) diff --git a/include/aikido/constraint/CollisionFree.hpp b/include/aikido/constraint/CollisionFree.hpp index d8dcaced95..2a68184455 100644 --- a/include/aikido/constraint/CollisionFree.hpp +++ b/include/aikido/constraint/CollisionFree.hpp @@ -40,12 +40,15 @@ class CollisionFree : public Testable // Documentation inherited. statespace::StateSpacePtr getStateSpace() const override; - // Documentation inherited. + // Documentation inherited. As an important 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, TestableOutcome* outcome = nullptr) const override; - // Documentation inherited. + // Documentation inherited. Returns an instance of CollisionFreeOutcome. std::unique_ptr createOutcome() const override; /// Checks collision between group1 and group2. diff --git a/include/aikido/constraint/Testable.hpp b/include/aikido/constraint/Testable.hpp index 24a7c805cd..c2a3a956f8 100644 --- a/include/aikido/constraint/Testable.hpp +++ b/include/aikido/constraint/Testable.hpp @@ -17,6 +17,12 @@ class Testable 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, TestableOutcome* outcome = nullptr) const = 0; diff --git a/include/aikido/constraint/TestableOutcome.hpp b/include/aikido/constraint/TestableOutcome.hpp index 7830c44589..6b160f103d 100644 --- a/include/aikido/constraint/TestableOutcome.hpp +++ b/include/aikido/constraint/TestableOutcome.hpp @@ -28,12 +28,12 @@ class TestableOutcome /// pointers down to pointers for a derivative class. Mostly used in the /// isSatisfied methods of classes that inherit Testable. template -Child* dynamic_cast_if_present(TestableOutcome* _outcome) +Child* dynamic_cast_if_present(TestableOutcome* outcome) { - if (!_outcome) + if (!outcome) return nullptr; - auto childPtr = dynamic_cast(_outcome); + auto childPtr = dynamic_cast(outcome); if (!childPtr) { std::stringstream message; From 43c8f92c6c3ba42c191b9620bacc2a2dec7cbbec Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Wed, 6 Dec 2017 18:14:14 -0500 Subject: [PATCH 35/41] Make CollisionFree a friend class of CollisionFreeOutcome, use Eigen alocator. --- include/aikido/constraint/CollisionFree.hpp | 4 ++-- .../constraint/CollisionFreeOutcome.hpp | 21 +++++++------------ include/aikido/constraint/Testable.hpp | 2 +- src/constraint/CollisionFree.cpp | 4 ++-- src/constraint/CollisionFreeOutcome.cpp | 18 ++-------------- tests/constraint/test_CollisionFree.cpp | 4 ++-- 6 files changed, 16 insertions(+), 37 deletions(-) diff --git a/include/aikido/constraint/CollisionFree.hpp b/include/aikido/constraint/CollisionFree.hpp index 2a68184455..2539e4fc81 100644 --- a/include/aikido/constraint/CollisionFree.hpp +++ b/include/aikido/constraint/CollisionFree.hpp @@ -41,8 +41,8 @@ class CollisionFree : public Testable statespace::StateSpacePtr getStateSpace() const override; // Documentation inherited. As an important 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 + // 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, diff --git a/include/aikido/constraint/CollisionFreeOutcome.hpp b/include/aikido/constraint/CollisionFreeOutcome.hpp index 08fcfe6615..64c25d8c5c 100644 --- a/include/aikido/constraint/CollisionFreeOutcome.hpp +++ b/include/aikido/constraint/CollisionFreeOutcome.hpp @@ -8,6 +8,7 @@ #include "dart/dynamics/BodyNode.hpp" #include "dart/dynamics/ShapeFrame.hpp" #include "dart/dynamics/ShapeNode.hpp" +#include namespace aikido { namespace constraint { @@ -16,6 +17,8 @@ namespace constraint { /// method in CollisionFree class. class CollisionFreeOutcome : public TestableOutcome { + friend class CollisionFree; + public: /// Documentation inherited. bool isSatisfied() const override; @@ -30,23 +33,13 @@ class CollisionFreeOutcome : public TestableOutcome /// object. void clear(); - /// Store a Contact object from a pairwise collision. Used by CollisionFree - /// to modify this object. - /// \param[in] pairwiseContact Contact object to store. - void markPairwiseContact(const dart::collision::Contact& pairwiseContact); - - /// Store a Contact object from a self collision. Used by CollisionFree to - /// modify this object. - /// \param[in] selfContact Contact object to store. - void markSelfContact(const dart::collision::Contact& selfContact); - /// Return a copy of the vector storing the Contact objects from pairwise /// collisions. - std::vector getPairwiseContacts() const; + std::vector> getPairwiseContacts() const; /// Return a copy of the vector storing the Contact objects from self /// collisions. - std::vector getSelfContacts() const; + std::vector> 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 @@ -57,10 +50,10 @@ class CollisionFreeOutcome : public TestableOutcome protected: /// Holds Contact objects from pairwise collisions. - std::vector mPairwiseContacts; + std::vector> mPairwiseContacts; /// Holds Contact objects from self collisions. - std::vector mSelfContacts; + std::vector> mSelfContacts; }; } // namespace constraint diff --git a/include/aikido/constraint/Testable.hpp b/include/aikido/constraint/Testable.hpp index c2a3a956f8..e910d7c3f9 100644 --- a/include/aikido/constraint/Testable.hpp +++ b/include/aikido/constraint/Testable.hpp @@ -19,7 +19,7 @@ class Testable /// 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 + /// 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. diff --git a/src/constraint/CollisionFree.cpp b/src/constraint/CollisionFree.cpp index b5d759a853..6df1b697d9 100644 --- a/src/constraint/CollisionFree.cpp +++ b/src/constraint/CollisionFree.cpp @@ -59,7 +59,7 @@ bool CollisionFree::isSatisfied( const auto& contacts = collisionResult.getContacts(); for (const auto& elem : contacts) { - collisionFreeOutcome->markPairwiseContact(elem); + collisionFreeOutcome->mPairwiseContacts.push_back(elem); } } return false; @@ -77,7 +77,7 @@ bool CollisionFree::isSatisfied( const auto& contacts = collisionResult.getContacts(); for (const auto& elem : contacts) { - collisionFreeOutcome->markSelfContact(elem); + collisionFreeOutcome->mSelfContacts.push_back(elem); } } return false; diff --git a/src/constraint/CollisionFreeOutcome.cpp b/src/constraint/CollisionFreeOutcome.cpp index 0b0cfe9025..224e601e96 100644 --- a/src/constraint/CollisionFreeOutcome.cpp +++ b/src/constraint/CollisionFreeOutcome.cpp @@ -41,28 +41,14 @@ void CollisionFreeOutcome::clear() } //============================================================================== -void CollisionFreeOutcome::markPairwiseContact( - const dart::collision::Contact& pairwiseContact) -{ - mPairwiseContacts.push_back(pairwiseContact); -} - -//============================================================================== -void CollisionFreeOutcome::markSelfContact( - const dart::collision::Contact& selfContact) -{ - mSelfContacts.push_back(selfContact); -} - -//============================================================================== -std::vector +std::vector> CollisionFreeOutcome::getPairwiseContacts() const { return mPairwiseContacts; } //============================================================================== -std::vector CollisionFreeOutcome::getSelfContacts() +std::vector> CollisionFreeOutcome::getSelfContacts() const { return mSelfContacts; diff --git a/tests/constraint/test_CollisionFree.cpp b/tests/constraint/test_CollisionFree.cpp index bc7c91217f..38880e22cf 100644 --- a/tests/constraint/test_CollisionFree.cpp +++ b/tests/constraint/test_CollisionFree.cpp @@ -140,7 +140,7 @@ TEST_F(CollisionFreeTest, AddPairwiseCheckFails_IsSatisfied) EXPECT_FALSE(outcome.isSatisfied()); EXPECT_EQ(0, outcome.getSelfContacts().size()); - std::vector pairwiseContacts + auto pairwiseContacts = outcome.getPairwiseContacts(); EXPECT_EQ(1, pairwiseContacts.size()); EXPECT_EQ( @@ -203,7 +203,7 @@ TEST_F(CollisionFreeTest, AddSelfCheckFails_IsSatisfied) EXPECT_FALSE(outcome.isSatisfied()); EXPECT_EQ(0, outcome.getPairwiseContacts().size()); - std::vector selfContacts + auto selfContacts = outcome.getSelfContacts(); EXPECT_EQ(1, selfContacts.size()); EXPECT_EQ( From e5e5d5bc28be625a82eee253b7001c43da5634ec Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Wed, 6 Dec 2017 18:33:27 -0500 Subject: [PATCH 36/41] Modify FrameTestable to "pass through" outcome object to mPoseConstraint. --- include/aikido/constraint/FrameTestable.hpp | 8 ++++++-- src/constraint/FrameTestable.cpp | 12 +++++------- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/include/aikido/constraint/FrameTestable.hpp b/include/aikido/constraint/FrameTestable.hpp index 48d5b0c3f7..e89c13a77c 100644 --- a/include/aikido/constraint/FrameTestable.hpp +++ b/include/aikido/constraint/FrameTestable.hpp @@ -29,12 +29,16 @@ 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(). + /// \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 an instance of DefaultOutcome, since this class doesn't have a - /// more specialized TestableOutcome derivative assigned to it. + /// 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 createOutcome() const override; // Documentation inhereted diff --git a/src/constraint/FrameTestable.cpp b/src/constraint/FrameTestable.cpp index c6037c60f2..030c8d2e95 100644 --- a/src/constraint/FrameTestable.cpp +++ b/src/constraint/FrameTestable.cpp @@ -36,8 +36,6 @@ FrameTestable::FrameTestable( bool FrameTestable::isSatisfied( const statespace::StateSpace::State* _state, TestableOutcome* outcome) const { - auto defaultOutcomeObject = dynamic_cast_if_present(outcome); - // Set the state auto state = static_cast( @@ -48,16 +46,16 @@ bool FrameTestable::isSatisfied( auto st = mPoseStateSpace->createState(); mPoseStateSpace->setIsometry(st, mFrame->getTransform()); - bool isSatisfiedResult = mPoseConstraint->isSatisfied(st); - if (defaultOutcomeObject) - defaultOutcomeObject->setSatisfiedFlag(isSatisfiedResult); - return isSatisfiedResult; + if (outcome) + return mPoseConstraint->isSatisfied(st, outcome); + + return mPoseConstraint->isSatisfied(st); } //============================================================================== std::unique_ptr FrameTestable::createOutcome() const { - return std::unique_ptr(new DefaultOutcome()); + return mPoseConstraint->createOutcome(); } //============================================================================== From 354d3d56bed6acb87db932d5adf4673f48a8dd74 Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Wed, 6 Dec 2017 18:57:07 -0500 Subject: [PATCH 37/41] Rename DefaultOutcome ---> DefaultTestableOutcome. Final clean up as well. --- .../constraint/CartesianProductTestable.hpp | 4 ++-- .../aikido/constraint/CollisionFreeOutcome.hpp | 18 +++++++++++++----- ...tOutcome.hpp => DefaultTestableOutcome.hpp} | 8 ++++---- include/aikido/constraint/FrameTestable.hpp | 6 +++--- include/aikido/constraint/Satisfied.hpp | 4 ++-- include/aikido/constraint/TSR.hpp | 4 ++-- include/aikido/constraint/Testable.hpp | 2 +- .../aikido/constraint/TestableIntersection.hpp | 4 ++-- .../constraint/uniform/RnBoxConstraint.hpp | 4 ++-- .../constraint/uniform/SE2BoxConstraint.hpp | 4 ++-- .../uniform/detail/RnBoxConstraint-impl.hpp | 5 +++-- src/constraint/CMakeLists.txt | 2 +- src/constraint/CartesianProductTestable.cpp | 5 +++-- src/constraint/CollisionFreeOutcome.cpp | 8 +++++--- ...tOutcome.cpp => DefaultTestableOutcome.cpp} | 8 ++++---- src/constraint/Satisfied.cpp | 5 +++-- src/constraint/TSR.cpp | 5 +++-- src/constraint/TestableIntersection.cpp | 5 +++-- src/constraint/uniform/SE2BoxConstraint.cpp | 5 +++-- tests/constraint/MockConstraints.hpp | 12 +++++++----- tests/constraint/test_CollisionFree.cpp | 6 ++---- tests/constraint/test_FrameTestable.cpp | 7 ++++--- tests/planner/ompl/OMPLTestHelpers.hpp | 12 +++++++----- 23 files changed, 81 insertions(+), 62 deletions(-) rename include/aikido/constraint/{DefaultOutcome.hpp => DefaultTestableOutcome.hpp} (80%) rename src/constraint/{DefaultOutcome.cpp => DefaultTestableOutcome.cpp} (67%) diff --git a/include/aikido/constraint/CartesianProductTestable.hpp b/include/aikido/constraint/CartesianProductTestable.hpp index a15a1fa96a..ba2c0370e6 100644 --- a/include/aikido/constraint/CartesianProductTestable.hpp +++ b/include/aikido/constraint/CartesianProductTestable.hpp @@ -29,8 +29,8 @@ class CartesianProductTestable : public Testable const aikido::statespace::StateSpace::State* _state, TestableOutcome* outcome = nullptr) const override; - /// Return an instance of DefaultOutcome, since this class doesn't have a - /// more specialized TestableOutcome derivative assigned to it. + /// Return an instance of DefaultTestableOutcome, since this class doesn't + /// have a more specialized TestableOutcome derivative assigned to it. std::unique_ptr createOutcome() const override; private: diff --git a/include/aikido/constraint/CollisionFreeOutcome.hpp b/include/aikido/constraint/CollisionFreeOutcome.hpp index 64c25d8c5c..768a254d9b 100644 --- a/include/aikido/constraint/CollisionFreeOutcome.hpp +++ b/include/aikido/constraint/CollisionFreeOutcome.hpp @@ -2,13 +2,13 @@ #define AIKIDO_CONSTRAINT_COLLISIONFREEOUTCOME_HPP_ #include +#include #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" -#include namespace aikido { namespace constraint { @@ -35,11 +35,15 @@ class CollisionFreeOutcome : public TestableOutcome /// Return a copy of the vector storing the Contact objects from pairwise /// collisions. - std::vector> getPairwiseContacts() const; + std::vector> + getPairwiseContacts() const; /// Return a copy of the vector storing the Contact objects from self /// collisions. - std::vector> getSelfContacts() const; + std::vector> + 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 @@ -50,10 +54,14 @@ class CollisionFreeOutcome : public TestableOutcome protected: /// Holds Contact objects from pairwise collisions. - std::vector> mPairwiseContacts; + std::vector> + mPairwiseContacts; /// Holds Contact objects from self collisions. - std::vector> mSelfContacts; + std::vector> + mSelfContacts; }; } // namespace constraint diff --git a/include/aikido/constraint/DefaultOutcome.hpp b/include/aikido/constraint/DefaultTestableOutcome.hpp similarity index 80% rename from include/aikido/constraint/DefaultOutcome.hpp rename to include/aikido/constraint/DefaultTestableOutcome.hpp index 9d3a2b4b4d..e019d8d25a 100644 --- a/include/aikido/constraint/DefaultOutcome.hpp +++ b/include/aikido/constraint/DefaultTestableOutcome.hpp @@ -1,5 +1,5 @@ -#ifndef AIKIDO_CONSTRAINT_DEFAULTOUTCOME_HPP_ -#define AIKIDO_CONSTRAINT_DEFAULTOUTCOME_HPP_ +#ifndef AIKIDO_CONSTRAINT_DEFAULTTESTABLEOUTCOME_HPP_ +#define AIKIDO_CONSTRAINT_DEFAULTTESTABLEOUTCOME_HPP_ #include "TestableOutcome.hpp" @@ -10,7 +10,7 @@ namespace constraint { /// is returned when createOutcome() is called on an instance of a class that /// inherits Testable, but has no corresponding TestableOutcome derivative /// implemented. -class DefaultOutcome : public TestableOutcome +class DefaultTestableOutcome : public TestableOutcome { public: /// Returns whether the isSatisfied method this object was passed to @@ -32,4 +32,4 @@ class DefaultOutcome : public TestableOutcome } // namespace constraint } // namespace aikido -#endif // AIKIDO_CONSTRAINT_DEFAULTOUTCOME_HPP_ +#endif // AIKIDO_CONSTRAINT_DEFAULTTESTABLEOUTCOME_HPP_ diff --git a/include/aikido/constraint/FrameTestable.hpp b/include/aikido/constraint/FrameTestable.hpp index e89c13a77c..759db3cdfd 100644 --- a/include/aikido/constraint/FrameTestable.hpp +++ b/include/aikido/constraint/FrameTestable.hpp @@ -29,15 +29,15 @@ 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(). - /// \param outcome Testable outcome derivative class. Passed to the + /// \param outcome Testable outcome derivative class. Passed to the /// isSatisfied method of mPoseConstraint to allow "pass through" of - /// debugging information. + /// 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 + /// isSatisfied in this class will just pass outcome to the isSatisfied /// method of of mPoseConstraint. std::unique_ptr createOutcome() const override; diff --git a/include/aikido/constraint/Satisfied.hpp b/include/aikido/constraint/Satisfied.hpp index 18fd280cab..26ce2bd9f6 100644 --- a/include/aikido/constraint/Satisfied.hpp +++ b/include/aikido/constraint/Satisfied.hpp @@ -38,8 +38,8 @@ class Satisfied : public constraint::Differentiable, const statespace::StateSpace::State* state, TestableOutcome* outcome = nullptr) const override; - /// Return an instance of DefaultOutcome, since this class doesn't have a - /// more specialized TestableOutcome derivative assigned to it. + /// Return an instance of DefaultTestableOutcome, since this class doesn't + /// have a more specialized TestableOutcome derivative assigned to it. std::unique_ptr createOutcome() const override; /// Sets \c _out to \c _s. diff --git a/include/aikido/constraint/TSR.hpp b/include/aikido/constraint/TSR.hpp index c22a702755..1b27c39df3 100644 --- a/include/aikido/constraint/TSR.hpp +++ b/include/aikido/constraint/TSR.hpp @@ -84,8 +84,8 @@ class TSR : public Sampleable, const statespace::StateSpace::State* _s, TestableOutcome* outcome = nullptr) const override; - /// Return an instance of DefaultOutcome, since this class doesn't have a - /// more specialized TestableOutcome derivative assigned to it. + /// Return an instance of DefaultTestableOutcome, since this class doesn't + /// have a more specialized TestableOutcome derivative assigned to it. std::unique_ptr createOutcome() const override; /// Throws an invalid_argument exception if this TSR is invalid. diff --git a/include/aikido/constraint/Testable.hpp b/include/aikido/constraint/Testable.hpp index e910d7c3f9..2f703900fa 100644 --- a/include/aikido/constraint/Testable.hpp +++ b/include/aikido/constraint/Testable.hpp @@ -3,7 +3,7 @@ #include #include "../statespace/StateSpace.hpp" -#include "DefaultOutcome.hpp" +#include "DefaultTestableOutcome.hpp" namespace aikido { namespace constraint { diff --git a/include/aikido/constraint/TestableIntersection.hpp b/include/aikido/constraint/TestableIntersection.hpp index bd0ac4b396..5c8b5e53de 100644 --- a/include/aikido/constraint/TestableIntersection.hpp +++ b/include/aikido/constraint/TestableIntersection.hpp @@ -26,8 +26,8 @@ class TestableIntersection : public Testable const aikido::statespace::StateSpace::State* state, TestableOutcome* outcome = nullptr) const override; - /// Return an instance of DefaultOutcome, since this class doesn't have a - /// more specialized TestableOutcome derivative assigned to it. + /// Return an instance of DefaultTestableOutcome, since this class doesn't + /// have a more specialized TestableOutcome derivative assigned to it. std::unique_ptr createOutcome() const override; // Documentation inherited. diff --git a/include/aikido/constraint/uniform/RnBoxConstraint.hpp b/include/aikido/constraint/uniform/RnBoxConstraint.hpp index 9c3932835c..9c688e3b20 100644 --- a/include/aikido/constraint/uniform/RnBoxConstraint.hpp +++ b/include/aikido/constraint/uniform/RnBoxConstraint.hpp @@ -50,8 +50,8 @@ class RBoxConstraint : public constraint::Differentiable, const statespace::StateSpace::State* state, TestableOutcome* outcome = nullptr) const override; - /// Return an instance of DefaultOutcome, since this class doesn't have a - /// more specialized TestableOutcome derivative assigned to it. + /// Return an instance of DefaultTestableOutcome, since this class doesn't + /// have a more specialized TestableOutcome derivative assigned to it. std::unique_ptr createOutcome() const override; // Documentation inherited. diff --git a/include/aikido/constraint/uniform/SE2BoxConstraint.hpp b/include/aikido/constraint/uniform/SE2BoxConstraint.hpp index efc0344e9d..cb3b0591a6 100644 --- a/include/aikido/constraint/uniform/SE2BoxConstraint.hpp +++ b/include/aikido/constraint/uniform/SE2BoxConstraint.hpp @@ -40,8 +40,8 @@ class SE2BoxConstraint : public constraint::Projectable, const statespace::StateSpace::State* state, TestableOutcome* outcome = nullptr) const override; - /// Return an instance of DefaultOutcome, since this class doesn't have a - /// more specialized TestableOutcome derivative assigned to it. + /// Return an instance of DefaultTestableOutcome, since this class doesn't + /// have a more specialized TestableOutcome derivative assigned to it. std::unique_ptr createOutcome() const override; // Documentation inherited. diff --git a/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp b/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp index f6d4680a0a..02c56bac8d 100644 --- a/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp +++ b/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp @@ -177,7 +177,8 @@ template bool RBoxConstraint::isSatisfied( const statespace::StateSpace::State* state, TestableOutcome* outcome) const { - auto defaultOutcomeObject = dynamic_cast_if_present(outcome); + auto defaultOutcomeObject + = dynamic_cast_if_present(outcome); const auto value = mSpace->getValue( static_cast::State*>(state)); @@ -201,7 +202,7 @@ bool RBoxConstraint::isSatisfied( template std::unique_ptr RBoxConstraint::createOutcome() const { - return std::unique_ptr(new DefaultOutcome); + return std::unique_ptr(new DefaultTestableOutcome); } //============================================================================== diff --git a/src/constraint/CMakeLists.txt b/src/constraint/CMakeLists.txt index 6688a83c07..74b4a6c57b 100644 --- a/src/constraint/CMakeLists.txt +++ b/src/constraint/CMakeLists.txt @@ -10,7 +10,7 @@ set(sources CollisionFree.cpp CollisionFreeOutcome.cpp CyclicSampleable.cpp - DefaultOutcome.cpp + DefaultTestableOutcome.cpp Differentiable.cpp DifferentiableIntersection.cpp DifferentiableSubspace.cpp diff --git a/src/constraint/CartesianProductTestable.cpp b/src/constraint/CartesianProductTestable.cpp index 93c22415e6..09d2687213 100644 --- a/src/constraint/CartesianProductTestable.cpp +++ b/src/constraint/CartesianProductTestable.cpp @@ -56,7 +56,8 @@ bool CartesianProductTestable::isSatisfied( const aikido::statespace::StateSpace::State* _state, TestableOutcome* outcome) const { - auto defaultOutcomeObject = dynamic_cast_if_present(outcome); + auto defaultOutcomeObject + = dynamic_cast_if_present(outcome); const auto state = static_cast(_state); @@ -80,7 +81,7 @@ bool CartesianProductTestable::isSatisfied( //============================================================================== std::unique_ptr CartesianProductTestable::createOutcome() const { - return std::unique_ptr(new DefaultOutcome()); + return std::unique_ptr(new DefaultTestableOutcome); } } // namespace constraint diff --git a/src/constraint/CollisionFreeOutcome.cpp b/src/constraint/CollisionFreeOutcome.cpp index 224e601e96..9e17ed4a6c 100644 --- a/src/constraint/CollisionFreeOutcome.cpp +++ b/src/constraint/CollisionFreeOutcome.cpp @@ -41,15 +41,17 @@ void CollisionFreeOutcome::clear() } //============================================================================== -std::vector> +std::vector> CollisionFreeOutcome::getPairwiseContacts() const { return mPairwiseContacts; } //============================================================================== -std::vector> CollisionFreeOutcome::getSelfContacts() - const +std::vector> +CollisionFreeOutcome::getSelfContacts() const { return mSelfContacts; } diff --git a/src/constraint/DefaultOutcome.cpp b/src/constraint/DefaultTestableOutcome.cpp similarity index 67% rename from src/constraint/DefaultOutcome.cpp rename to src/constraint/DefaultTestableOutcome.cpp index 275678e059..e7973b06cb 100644 --- a/src/constraint/DefaultOutcome.cpp +++ b/src/constraint/DefaultTestableOutcome.cpp @@ -1,16 +1,16 @@ -#include +#include namespace aikido { namespace constraint { //============================================================================== -bool DefaultOutcome::isSatisfied() const +bool DefaultTestableOutcome::isSatisfied() const { return mSatisfiedFlag; } //============================================================================== -std::string DefaultOutcome::toString() const +std::string DefaultTestableOutcome::toString() const { if (mSatisfiedFlag) return "true"; @@ -19,7 +19,7 @@ std::string DefaultOutcome::toString() const } //============================================================================== -void DefaultOutcome::setSatisfiedFlag(bool satisfiedFlag) +void DefaultTestableOutcome::setSatisfiedFlag(bool satisfiedFlag) { mSatisfiedFlag = satisfiedFlag; } diff --git a/src/constraint/Satisfied.cpp b/src/constraint/Satisfied.cpp index af3658f612..c5f77c8c2d 100644 --- a/src/constraint/Satisfied.cpp +++ b/src/constraint/Satisfied.cpp @@ -34,7 +34,8 @@ bool Satisfied::isSatisfied( const statespace::StateSpace::State* /*state*/, TestableOutcome* outcome) const { - auto defaultOutcomeObject = dynamic_cast_if_present(outcome); + auto defaultOutcomeObject + = dynamic_cast_if_present(outcome); if (defaultOutcomeObject) defaultOutcomeObject->setSatisfiedFlag(true); @@ -44,7 +45,7 @@ bool Satisfied::isSatisfied( //============================================================================== std::unique_ptr Satisfied::createOutcome() const { - return std::unique_ptr(new DefaultOutcome()); + return std::unique_ptr(new DefaultTestableOutcome); } //============================================================================== diff --git a/src/constraint/TSR.cpp b/src/constraint/TSR.cpp index b529d54723..e7be74c495 100644 --- a/src/constraint/TSR.cpp +++ b/src/constraint/TSR.cpp @@ -213,7 +213,8 @@ std::unique_ptr TSR::createSampleGenerator() const bool TSR::isSatisfied( const statespace::StateSpace::State* _s, TestableOutcome* outcome) const { - auto defaultOutcomeObject = dynamic_cast_if_present(outcome); + auto defaultOutcomeObject + = dynamic_cast_if_present(outcome); Eigen::VectorXd dist; getValue(_s, dist); @@ -227,7 +228,7 @@ bool TSR::isSatisfied( //============================================================================== std::unique_ptr TSR::createOutcome() const { - return std::unique_ptr(new DefaultOutcome()); + return std::unique_ptr(new DefaultTestableOutcome); } //============================================================================== diff --git a/src/constraint/TestableIntersection.cpp b/src/constraint/TestableIntersection.cpp index 272e7605e3..04c6bde2fc 100644 --- a/src/constraint/TestableIntersection.cpp +++ b/src/constraint/TestableIntersection.cpp @@ -23,7 +23,8 @@ bool TestableIntersection::isSatisfied( const aikido::statespace::StateSpace::State* _state, TestableOutcome* outcome) const { - auto defaultOutcomeObject = dynamic_cast_if_present(outcome); + auto defaultOutcomeObject + = dynamic_cast_if_present(outcome); for (auto c : mConstraints) { @@ -43,7 +44,7 @@ bool TestableIntersection::isSatisfied( //============================================================================== std::unique_ptr TestableIntersection::createOutcome() const { - return std::unique_ptr(new DefaultOutcome()); + return std::unique_ptr(new DefaultTestableOutcome); } //============================================================================== diff --git a/src/constraint/uniform/SE2BoxConstraint.cpp b/src/constraint/uniform/SE2BoxConstraint.cpp index b05d1731f1..4f4273d51f 100644 --- a/src/constraint/uniform/SE2BoxConstraint.cpp +++ b/src/constraint/uniform/SE2BoxConstraint.cpp @@ -139,7 +139,8 @@ statespace::StateSpacePtr SE2BoxConstraint::getStateSpace() const bool SE2BoxConstraint::isSatisfied( const statespace::StateSpace::State* state, TestableOutcome* outcome) const { - auto defaultOutcomeObject = dynamic_cast_if_present(outcome); + auto defaultOutcomeObject + = dynamic_cast_if_present(outcome); Eigen::VectorXd tangent; mSpace->logMap(static_cast(state), tangent); @@ -162,7 +163,7 @@ bool SE2BoxConstraint::isSatisfied( //============================================================================== std::unique_ptr SE2BoxConstraint::createOutcome() const { - return std::unique_ptr(new DefaultOutcome()); + return std::unique_ptr(new DefaultTestableOutcome); } //============================================================================== diff --git a/tests/constraint/MockConstraints.hpp b/tests/constraint/MockConstraints.hpp index 2e557ba07b..76570f1266 100644 --- a/tests/constraint/MockConstraints.hpp +++ b/tests/constraint/MockConstraints.hpp @@ -4,7 +4,7 @@ #include #include -using aikido::constraint::DefaultOutcome; +using aikido::constraint::DefaultTestableOutcome; using aikido::constraint::TestableOutcome; class PassingConstraint : public aikido::constraint::Testable @@ -21,7 +21,8 @@ class PassingConstraint : public aikido::constraint::Testable TestableOutcome* outcome = nullptr) const override { auto defaultOutcomeObject - = aikido::constraint::dynamic_cast_if_present(outcome); + = aikido::constraint::dynamic_cast_if_present( + outcome); if (defaultOutcomeObject) defaultOutcomeObject->setSatisfiedFlag(true); @@ -30,7 +31,7 @@ class PassingConstraint : public aikido::constraint::Testable std::unique_ptr createOutcome() const { - return std::unique_ptr(new DefaultOutcome()); + return std::unique_ptr(new DefaultTestableOutcome); } std::shared_ptr getStateSpace() const override @@ -56,7 +57,8 @@ class FailingConstraint : public aikido::constraint::Testable TestableOutcome* outcome = nullptr) const override { auto defaultOutcomeObject - = aikido::constraint::dynamic_cast_if_present(outcome); + = aikido::constraint::dynamic_cast_if_present( + outcome); if (defaultOutcomeObject) defaultOutcomeObject->setSatisfiedFlag(false); @@ -65,7 +67,7 @@ class FailingConstraint : public aikido::constraint::Testable std::unique_ptr createOutcome() const { - return std::unique_ptr(new DefaultOutcome()); + return std::unique_ptr(new DefaultTestableOutcome); } std::shared_ptr getStateSpace() const override diff --git a/tests/constraint/test_CollisionFree.cpp b/tests/constraint/test_CollisionFree.cpp index 38880e22cf..a10524adc3 100644 --- a/tests/constraint/test_CollisionFree.cpp +++ b/tests/constraint/test_CollisionFree.cpp @@ -140,8 +140,7 @@ TEST_F(CollisionFreeTest, AddPairwiseCheckFails_IsSatisfied) EXPECT_FALSE(outcome.isSatisfied()); EXPECT_EQ(0, outcome.getSelfContacts().size()); - auto pairwiseContacts - = outcome.getPairwiseContacts(); + auto pairwiseContacts = outcome.getPairwiseContacts(); EXPECT_EQ(1, pairwiseContacts.size()); EXPECT_EQ( "BodyNode", @@ -203,8 +202,7 @@ TEST_F(CollisionFreeTest, AddSelfCheckFails_IsSatisfied) EXPECT_FALSE(outcome.isSatisfied()); EXPECT_EQ(0, outcome.getPairwiseContacts().size()); - auto selfContacts - = outcome.getSelfContacts(); + auto selfContacts = outcome.getSelfContacts(); EXPECT_EQ(1, selfContacts.size()); EXPECT_EQ( "BodyNode", diff --git a/tests/constraint/test_FrameTestable.cpp b/tests/constraint/test_FrameTestable.cpp index 80038b9d3d..cb1e952cb5 100644 --- a/tests/constraint/test_FrameTestable.cpp +++ b/tests/constraint/test_FrameTestable.cpp @@ -4,7 +4,7 @@ #include #include "MockConstraints.hpp" -using aikido::constraint::DefaultOutcome; +using aikido::constraint::DefaultTestableOutcome; using aikido::constraint::FrameTestable; using aikido::constraint::Testable; using aikido::constraint::TestablePtr; @@ -34,7 +34,8 @@ class EndEffectorTestable : public aikido::constraint::Testable aikido::constraint::TestableOutcome* outcome = nullptr) const override { auto defaultOutcomeObject - = aikido::constraint::dynamic_cast_if_present(outcome); + = aikido::constraint::dynamic_cast_if_present( + outcome); auto st = static_cast(_state); auto val = st->getIsometry(); @@ -49,7 +50,7 @@ class EndEffectorTestable : public aikido::constraint::Testable std::unique_ptr createOutcome() const { - return std::unique_ptr(new DefaultOutcome()); + return std::unique_ptr(new DefaultTestableOutcome); } private: diff --git a/tests/planner/ompl/OMPLTestHelpers.hpp b/tests/planner/ompl/OMPLTestHelpers.hpp index 54530579b0..1e2617de01 100644 --- a/tests/planner/ompl/OMPLTestHelpers.hpp +++ b/tests/planner/ompl/OMPLTestHelpers.hpp @@ -15,7 +15,7 @@ #include using dart::common::make_unique; -using aikido::constraint::DefaultOutcome; +using aikido::constraint::DefaultTestableOutcome; using aikido::constraint::TestableOutcome; using aikido::statespace::CartesianProduct; using aikido::statespace::R3; @@ -160,7 +160,8 @@ class MockProjectionConstraint : public aikido::constraint::Projectable, TestableOutcome* outcome = nullptr) const override { auto defaultOutcomeObject - = aikido::constraint::dynamic_cast_if_present(outcome); + = aikido::constraint::dynamic_cast_if_present( + outcome); auto state = static_cast(_s); auto val = mStateSpace->getSubStateHandle(state, 0).getValue(); @@ -173,7 +174,7 @@ class MockProjectionConstraint : public aikido::constraint::Projectable, std::unique_ptr createOutcome() const { - return std::unique_ptr(new DefaultOutcome()); + return std::unique_ptr(new DefaultTestableOutcome); } std::unique_ptr createSampleGenerator() @@ -209,7 +210,8 @@ class MockTranslationalRobotConstraint : public aikido::constraint::Testable TestableOutcome* outcome = nullptr) const override { auto defaultOutcomeObject - = aikido::constraint::dynamic_cast_if_present(outcome); + = aikido::constraint::dynamic_cast_if_present( + outcome); auto cst = static_cast(_state); auto subState = mStateSpace->getSubStateHandle(cst, 0); @@ -232,7 +234,7 @@ class MockTranslationalRobotConstraint : public aikido::constraint::Testable std::unique_ptr createOutcome() const { - return std::unique_ptr(new DefaultOutcome()); + return std::unique_ptr(new DefaultTestableOutcome); } // Documentatoin inherited From 0f6731bc723b2dc051d17e7a8ff5d2f8d6b10b37 Mon Sep 17 00:00:00 2001 From: Brian Hou Date: Mon, 11 Dec 2017 01:52:09 -0800 Subject: [PATCH 38/41] Address @mkoval's comments. --- include/aikido/constraint/TestableOutcome.hpp | 23 +++------------ .../detail/TestableOutcome-impl.hpp | 28 +++++++++++++++++++ src/constraint/CollisionFree.cpp | 2 +- src/constraint/FrameTestable.cpp | 6 +--- 4 files changed, 34 insertions(+), 25 deletions(-) create mode 100644 include/aikido/constraint/detail/TestableOutcome-impl.hpp diff --git a/include/aikido/constraint/TestableOutcome.hpp b/include/aikido/constraint/TestableOutcome.hpp index 6b160f103d..f12167f4c7 100644 --- a/include/aikido/constraint/TestableOutcome.hpp +++ b/include/aikido/constraint/TestableOutcome.hpp @@ -1,9 +1,7 @@ #ifndef AIKIDO_CONSTRAINT_TESTABLEOUTCOME_HPP_ #define AIKIDO_CONSTRAINT_TESTABLEOUTCOME_HPP_ -#include -#include -#include +#include namespace aikido { namespace constraint { @@ -28,24 +26,11 @@ class TestableOutcome /// pointers down to pointers for a derivative class. Mostly used in the /// isSatisfied methods of classes that inherit Testable. template -Child* dynamic_cast_if_present(TestableOutcome* outcome) -{ - if (!outcome) - return nullptr; - - auto childPtr = dynamic_cast(outcome); - if (!childPtr) - { - std::stringstream message; - message << "TestableOutcome pointer is not of type " << typeid(Child).name() - << "."; - throw std::invalid_argument(message.str()); - } - - return childPtr; -} +Child* dynamic_cast_if_present(TestableOutcome* outcome); } // namespace constraint } // namespace aikido +#include "detail/TestableOutcome-impl.hpp" + #endif // AIKIDO_CONSTRAINT_TESTABLEOUTCOME_HPP_ diff --git a/include/aikido/constraint/detail/TestableOutcome-impl.hpp b/include/aikido/constraint/detail/TestableOutcome-impl.hpp new file mode 100644 index 0000000000..cbecea56c9 --- /dev/null +++ b/include/aikido/constraint/detail/TestableOutcome-impl.hpp @@ -0,0 +1,28 @@ +#include +#include +#include + +namespace aikido { +namespace constraint { + +//============================================================================== +template +Child* dynamic_cast_if_present(TestableOutcome* outcome) +{ + if (!outcome) + return nullptr; + + auto childPtr = dynamic_cast(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 diff --git a/src/constraint/CollisionFree.cpp b/src/constraint/CollisionFree.cpp index 6df1b697d9..ee0f7cd790 100644 --- a/src/constraint/CollisionFree.cpp +++ b/src/constraint/CollisionFree.cpp @@ -89,7 +89,7 @@ bool CollisionFree::isSatisfied( //============================================================================== std::unique_ptr CollisionFree::createOutcome() const { - return std::unique_ptr(new CollisionFreeOutcome()); + return std::unique_ptr(new CollisionFreeOutcome); } //============================================================================== diff --git a/src/constraint/FrameTestable.cpp b/src/constraint/FrameTestable.cpp index 030c8d2e95..aaf0599591 100644 --- a/src/constraint/FrameTestable.cpp +++ b/src/constraint/FrameTestable.cpp @@ -45,11 +45,7 @@ bool FrameTestable::isSatisfied( // Check the pose constraint auto st = mPoseStateSpace->createState(); mPoseStateSpace->setIsometry(st, mFrame->getTransform()); - - if (outcome) - return mPoseConstraint->isSatisfied(st, outcome); - - return mPoseConstraint->isSatisfied(st); + return mPoseConstraint->isSatisfied(st, outcome); } //============================================================================== From b70af0a45c00ca086d92f7cb1bdea1d80fa28fdd Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Thu, 4 Jan 2018 22:48:55 -0500 Subject: [PATCH 39/41] Update nits: comment and renamed dynamic_cast_if_present to dynamic_cast_or_throw. --- include/aikido/constraint/CollisionFree.hpp | 11 ++++++----- include/aikido/constraint/TestableOutcome.hpp | 2 +- .../aikido/constraint/detail/TestableOutcome-impl.hpp | 2 +- .../uniform/detail/RnBoxConstraint-impl.hpp | 2 +- src/constraint/CartesianProductTestable.cpp | 2 +- src/constraint/CollisionFree.cpp | 2 +- src/constraint/Satisfied.cpp | 2 +- src/constraint/TSR.cpp | 2 +- src/constraint/TestableIntersection.cpp | 2 +- src/constraint/uniform/SE2BoxConstraint.cpp | 2 +- tests/constraint/MockConstraints.hpp | 4 ++-- tests/constraint/test_FrameTestable.cpp | 2 +- tests/planner/ompl/OMPLTestHelpers.hpp | 4 ++-- 13 files changed, 20 insertions(+), 19 deletions(-) diff --git a/include/aikido/constraint/CollisionFree.hpp b/include/aikido/constraint/CollisionFree.hpp index 2539e4fc81..45914bba7e 100644 --- a/include/aikido/constraint/CollisionFree.hpp +++ b/include/aikido/constraint/CollisionFree.hpp @@ -40,15 +40,16 @@ class CollisionFree : public Testable // Documentation inherited. statespace::StateSpacePtr getStateSpace() const override; - // Documentation inherited. As an important 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). + /// \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, TestableOutcome* outcome = nullptr) const override; - // Documentation inherited. Returns an instance of CollisionFreeOutcome. + /// \copydoc Testable::createOutcome() + /// \note Returns an instance of CollisionFreeOutcome. std::unique_ptr createOutcome() const override; /// Checks collision between group1 and group2. diff --git a/include/aikido/constraint/TestableOutcome.hpp b/include/aikido/constraint/TestableOutcome.hpp index f12167f4c7..48bbe4aedd 100644 --- a/include/aikido/constraint/TestableOutcome.hpp +++ b/include/aikido/constraint/TestableOutcome.hpp @@ -26,7 +26,7 @@ class TestableOutcome /// pointers down to pointers for a derivative class. Mostly used in the /// isSatisfied methods of classes that inherit Testable. template -Child* dynamic_cast_if_present(TestableOutcome* outcome); +Child* dynamic_cast_or_throw(TestableOutcome* outcome); } // namespace constraint } // namespace aikido diff --git a/include/aikido/constraint/detail/TestableOutcome-impl.hpp b/include/aikido/constraint/detail/TestableOutcome-impl.hpp index cbecea56c9..b89b191206 100644 --- a/include/aikido/constraint/detail/TestableOutcome-impl.hpp +++ b/include/aikido/constraint/detail/TestableOutcome-impl.hpp @@ -7,7 +7,7 @@ namespace constraint { //============================================================================== template -Child* dynamic_cast_if_present(TestableOutcome* outcome) +Child* dynamic_cast_or_throw(TestableOutcome* outcome) { if (!outcome) return nullptr; diff --git a/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp b/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp index 02c56bac8d..e575aba8dc 100644 --- a/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp +++ b/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp @@ -178,7 +178,7 @@ bool RBoxConstraint::isSatisfied( const statespace::StateSpace::State* state, TestableOutcome* outcome) const { auto defaultOutcomeObject - = dynamic_cast_if_present(outcome); + = dynamic_cast_or_throw(outcome); const auto value = mSpace->getValue( static_cast::State*>(state)); diff --git a/src/constraint/CartesianProductTestable.cpp b/src/constraint/CartesianProductTestable.cpp index 09d2687213..12fd6da5fb 100644 --- a/src/constraint/CartesianProductTestable.cpp +++ b/src/constraint/CartesianProductTestable.cpp @@ -57,7 +57,7 @@ bool CartesianProductTestable::isSatisfied( TestableOutcome* outcome) const { auto defaultOutcomeObject - = dynamic_cast_if_present(outcome); + = dynamic_cast_or_throw(outcome); const auto state = static_cast(_state); diff --git a/src/constraint/CollisionFree.cpp b/src/constraint/CollisionFree.cpp index ee0f7cd790..f6b7d21542 100644 --- a/src/constraint/CollisionFree.cpp +++ b/src/constraint/CollisionFree.cpp @@ -31,7 +31,7 @@ bool CollisionFree::isSatisfied( TestableOutcome* outcome) const { auto collisionFreeOutcome - = dynamic_cast_if_present(outcome); + = dynamic_cast_or_throw(outcome); if (collisionFreeOutcome) { diff --git a/src/constraint/Satisfied.cpp b/src/constraint/Satisfied.cpp index c5f77c8c2d..eb8cf73503 100644 --- a/src/constraint/Satisfied.cpp +++ b/src/constraint/Satisfied.cpp @@ -35,7 +35,7 @@ bool Satisfied::isSatisfied( TestableOutcome* outcome) const { auto defaultOutcomeObject - = dynamic_cast_if_present(outcome); + = dynamic_cast_or_throw(outcome); if (defaultOutcomeObject) defaultOutcomeObject->setSatisfiedFlag(true); diff --git a/src/constraint/TSR.cpp b/src/constraint/TSR.cpp index e7be74c495..dc108afe08 100644 --- a/src/constraint/TSR.cpp +++ b/src/constraint/TSR.cpp @@ -214,7 +214,7 @@ bool TSR::isSatisfied( const statespace::StateSpace::State* _s, TestableOutcome* outcome) const { auto defaultOutcomeObject - = dynamic_cast_if_present(outcome); + = dynamic_cast_or_throw(outcome); Eigen::VectorXd dist; getValue(_s, dist); diff --git a/src/constraint/TestableIntersection.cpp b/src/constraint/TestableIntersection.cpp index 04c6bde2fc..a8533f393c 100644 --- a/src/constraint/TestableIntersection.cpp +++ b/src/constraint/TestableIntersection.cpp @@ -24,7 +24,7 @@ bool TestableIntersection::isSatisfied( TestableOutcome* outcome) const { auto defaultOutcomeObject - = dynamic_cast_if_present(outcome); + = dynamic_cast_or_throw(outcome); for (auto c : mConstraints) { diff --git a/src/constraint/uniform/SE2BoxConstraint.cpp b/src/constraint/uniform/SE2BoxConstraint.cpp index 4f4273d51f..7e916f0609 100644 --- a/src/constraint/uniform/SE2BoxConstraint.cpp +++ b/src/constraint/uniform/SE2BoxConstraint.cpp @@ -140,7 +140,7 @@ bool SE2BoxConstraint::isSatisfied( const statespace::StateSpace::State* state, TestableOutcome* outcome) const { auto defaultOutcomeObject - = dynamic_cast_if_present(outcome); + = dynamic_cast_or_throw(outcome); Eigen::VectorXd tangent; mSpace->logMap(static_cast(state), tangent); diff --git a/tests/constraint/MockConstraints.hpp b/tests/constraint/MockConstraints.hpp index 76570f1266..ea5508420b 100644 --- a/tests/constraint/MockConstraints.hpp +++ b/tests/constraint/MockConstraints.hpp @@ -21,7 +21,7 @@ class PassingConstraint : public aikido::constraint::Testable TestableOutcome* outcome = nullptr) const override { auto defaultOutcomeObject - = aikido::constraint::dynamic_cast_if_present( + = aikido::constraint::dynamic_cast_or_throw( outcome); if (defaultOutcomeObject) @@ -57,7 +57,7 @@ class FailingConstraint : public aikido::constraint::Testable TestableOutcome* outcome = nullptr) const override { auto defaultOutcomeObject - = aikido::constraint::dynamic_cast_if_present( + = aikido::constraint::dynamic_cast_or_throw( outcome); if (defaultOutcomeObject) diff --git a/tests/constraint/test_FrameTestable.cpp b/tests/constraint/test_FrameTestable.cpp index cb1e952cb5..a0d0f2322d 100644 --- a/tests/constraint/test_FrameTestable.cpp +++ b/tests/constraint/test_FrameTestable.cpp @@ -34,7 +34,7 @@ class EndEffectorTestable : public aikido::constraint::Testable aikido::constraint::TestableOutcome* outcome = nullptr) const override { auto defaultOutcomeObject - = aikido::constraint::dynamic_cast_if_present( + = aikido::constraint::dynamic_cast_or_throw( outcome); auto st = static_cast(_state); diff --git a/tests/planner/ompl/OMPLTestHelpers.hpp b/tests/planner/ompl/OMPLTestHelpers.hpp index 1e2617de01..ca1398b715 100644 --- a/tests/planner/ompl/OMPLTestHelpers.hpp +++ b/tests/planner/ompl/OMPLTestHelpers.hpp @@ -160,7 +160,7 @@ class MockProjectionConstraint : public aikido::constraint::Projectable, TestableOutcome* outcome = nullptr) const override { auto defaultOutcomeObject - = aikido::constraint::dynamic_cast_if_present( + = aikido::constraint::dynamic_cast_or_throw( outcome); auto state = static_cast(_s); @@ -210,7 +210,7 @@ class MockTranslationalRobotConstraint : public aikido::constraint::Testable TestableOutcome* outcome = nullptr) const override { auto defaultOutcomeObject - = aikido::constraint::dynamic_cast_if_present( + = aikido::constraint::dynamic_cast_or_throw( outcome); auto cst = static_cast(_state); From 9208f04fbabf96230f655c06d96da5e80959ed3c Mon Sep 17 00:00:00 2001 From: Sherdil Niyaz Date: Thu, 4 Jan 2018 23:10:13 -0500 Subject: [PATCH 40/41] Undo the use of the Eigen allocator in CollisionFreeOutcome. --- .../aikido/constraint/CollisionFreeOutcome.hpp | 17 ++++------------- src/constraint/CollisionFree.cpp | 12 ++---------- src/constraint/CollisionFreeOutcome.cpp | 8 +++----- 3 files changed, 9 insertions(+), 28 deletions(-) diff --git a/include/aikido/constraint/CollisionFreeOutcome.hpp b/include/aikido/constraint/CollisionFreeOutcome.hpp index 768a254d9b..8e9557f844 100644 --- a/include/aikido/constraint/CollisionFreeOutcome.hpp +++ b/include/aikido/constraint/CollisionFreeOutcome.hpp @@ -2,7 +2,6 @@ #define AIKIDO_CONSTRAINT_COLLISIONFREEOUTCOME_HPP_ #include -#include #include "TestableOutcome.hpp" #include "dart/collision/CollisionObject.hpp" #include "dart/collision/Contact.hpp" @@ -35,15 +34,11 @@ class CollisionFreeOutcome : public TestableOutcome /// Return a copy of the vector storing the Contact objects from pairwise /// collisions. - std::vector> - getPairwiseContacts() const; + std::vector getPairwiseContacts() const; /// Return a copy of the vector storing the Contact objects from self /// collisions. - std::vector> - getSelfContacts() const; + std::vector 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 @@ -54,14 +49,10 @@ class CollisionFreeOutcome : public TestableOutcome protected: /// Holds Contact objects from pairwise collisions. - std::vector> - mPairwiseContacts; + std::vector mPairwiseContacts; /// Holds Contact objects from self collisions. - std::vector> - mSelfContacts; + std::vector mSelfContacts; }; } // namespace constraint diff --git a/src/constraint/CollisionFree.cpp b/src/constraint/CollisionFree.cpp index f6b7d21542..9b6b35b6d4 100644 --- a/src/constraint/CollisionFree.cpp +++ b/src/constraint/CollisionFree.cpp @@ -56,11 +56,7 @@ bool CollisionFree::isSatisfied( { if (collisionFreeOutcome) { - const auto& contacts = collisionResult.getContacts(); - for (const auto& elem : contacts) - { - collisionFreeOutcome->mPairwiseContacts.push_back(elem); - } + collisionFreeOutcome->mPairwiseContacts = collisionResult.getContacts(); } return false; } @@ -74,11 +70,7 @@ bool CollisionFree::isSatisfied( { if (collisionFreeOutcome) { - const auto& contacts = collisionResult.getContacts(); - for (const auto& elem : contacts) - { - collisionFreeOutcome->mSelfContacts.push_back(elem); - } + collisionFreeOutcome->mSelfContacts = collisionResult.getContacts(); } return false; } diff --git a/src/constraint/CollisionFreeOutcome.cpp b/src/constraint/CollisionFreeOutcome.cpp index 9e17ed4a6c..d7fb0402a3 100644 --- a/src/constraint/CollisionFreeOutcome.cpp +++ b/src/constraint/CollisionFreeOutcome.cpp @@ -41,17 +41,15 @@ void CollisionFreeOutcome::clear() } //============================================================================== -std::vector> +std::vector CollisionFreeOutcome::getPairwiseContacts() const { return mPairwiseContacts; } //============================================================================== -std::vector> -CollisionFreeOutcome::getSelfContacts() const +std::vector CollisionFreeOutcome::getSelfContacts() + const { return mSelfContacts; } From 12647a375805cae1dd96c3245fa171d84fc0b435 Mon Sep 17 00:00:00 2001 From: Brian Hou Date: Fri, 5 Jan 2018 18:17:13 -0800 Subject: [PATCH 41/41] Update CHANGELOG.md --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 6a16d87349..2ed53d6ade 100644 --- a/CHANGELOG.md +++ b/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