Testable Constraint Output #266
There are no files selected for viewing
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,27 @@ | ||
| #ifndef AIKIDO_CONSTRAINT_COLLISIONFREEOUTCOME_HPP_ | ||
| #define AIKIDO_CONSTRAINT_COLLISIONFREEOUTCOME_HPP_ | ||
|
|
||
| #include <sstream> | ||
|
There was a problem hiding this comment. Nit: It seems we need to include There was a problem hiding this comment. Could you elaborate on this a bit? If is not included, then it looks like the string building in toString won't work (since a string stream is used there, and then converted to a string object). There was a problem hiding this comment.
The motivation behind this is that this way prevents unnecessarily including |
||
| #include <vector> | ||
| #include "TestableOutcome.hpp" | ||
|
|
||
| namespace aikido { | ||
| namespace constraint { | ||
|
|
||
| class CollisionFreeOutcome : public TestableOutcome | ||
|
There was a problem hiding this comment. Could we add a simple docstring for this class like saying this class is a testable outcome for |
||
| { | ||
| public: | ||
| bool isSatisfied() const; | ||
|
|
||
| std::string toString() const; | ||
|
|
||
| void markCollisionBodyNode(const std::string& bodyNodeName); | ||
|
There was a problem hiding this comment. Can we call this |
||
| void markSelfCollisionBodyNode(const std::string& bodyNodeName); | ||
|
|
||
| private: | ||
| std::vector<std::string> mCollisionBodyNodes; | ||
|
There was a problem hiding this comment. ...and then call this |
||
| std::vector<std::string> mSelfCollisionBodyNodes; | ||
| }; | ||
|
There was a problem hiding this comment. Could we have docstring for this class and its members? |
||
|
|
||
| } // namespace constraint | ||
| } // namespace aikido | ||
|
|
||
| #endif // ifndef AIKIDO_CONSTRAINT_COLLISIONFREEOUTCOME_HPP_ | ||
|
There was a problem hiding this comment. Nit: |
||
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,19 @@ | ||
| #ifndef AIKIDO_CONSTRAINT_TESTABLEOUTCOME_HPP_ | ||
|
There was a problem hiding this comment. I believe our convention is that we use a subdirectory when the class is in a nested namespace. The only exception (off the top of my head) is putting implementations of template classes under There was a problem hiding this comment. That makes sense. My main reasoning here was that all of the things in the main directory seem to be constraints, and these new files aren't constraints- they're supposed to correspond to the outputs of constraints. If nobody else has strong feelings either way, I'll move the files into the main constraint directory before merging. There was a problem hiding this comment. I actually suggest putting this class definition directly in If you think this would clutter that file too much, then moving it up to |
||
| #define AIKIDO_CONSTRAINT_TESTABLEOUTCOME_HPP_ | ||
|
|
||
| #include <string> | ||
|
|
||
| namespace aikido { | ||
| namespace constraint { | ||
|
|
||
| class TestableOutcome | ||
|
There was a problem hiding this comment. Could we add docstring for this class? |
||
| { | ||
| public: | ||
| virtual bool isSatisfied() const = 0; | ||
| virtual std::string toString() const = 0; | ||
| }; | ||
|
There was a problem hiding this comment. Could we have docstring for this class and its members? |
||
|
|
||
| } // namespace constraint | ||
| } // namespace aikido | ||
|
|
||
| #endif // ifndef AIKIDO_CONSTRAINT_TESTABLEOUTCOME_HPP_ | ||
|
There was a problem hiding this comment. Nit: |
||
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -27,7 +27,8 @@ statespace::StateSpacePtr CollisionFree::getStateSpace() const | |
|
|
||
| //============================================================================== | ||
| bool CollisionFree::isSatisfied( | ||
| const aikido::statespace::StateSpace::State* _state, | ||
| TestableOutcome* _outcome) const | ||
| { | ||
|
There was a problem hiding this comment. This function should clear the |
||
| auto skelStatePtr = static_cast<const aikido::statespace::dart:: | ||
| MetaSkeletonStateSpace::State*>(_state); | ||
|
|
@@ -42,16 +43,39 @@ bool CollisionFree::isSatisfied( | |
| groups.second.get(), | ||
| mCollisionOptions, | ||
| &collisionResult); | ||
|
|
||
| if (collision) | ||
| { | ||
| if (_outcome != nullptr) | ||
| { | ||
| auto collisionOutcome = static_cast<CollisionFreeOutcome*>(_outcome); | ||
|
There was a problem hiding this comment. It's not guaranteed if I'm not sure what would be the preferred way here. @brianhou , @mkoval, @gilwoolee thoughts? There was a problem hiding this comment. Is the only difference between (2) and (3) whether we silently fail or raise an exception when the My (limited) understanding is that we won't be passing a non-null There was a problem hiding this comment. I agree with this logic (assuming that @brianhou's reasoning about (2) and (3) is correct). I'm going to with (3) right now, and we can change if people have strong feelings towards either approach. |
||
| auto collidingBodies = collisionResult.getCollidingBodyNodes(); | ||
|
There was a problem hiding this comment. Nit: |
||
| for (const auto& elem : collidingBodies) | ||
|
There was a problem hiding this comment. Maybe this should iterate over This isn't a problem now because our default |
||
| { | ||
| collisionOutcome->markCollisionBodyNode(elem->getName()); | ||
| } | ||
|
There was a problem hiding this comment. Nit: If you make collisionFreeOutcome->mContacts = collisionResult.getContacts();There was a problem hiding this comment. Thanks for all the awesome feedback! 😄 I had a question about this one- when using Eigen::aligned_allocator (as mentioned in a previous comment), wouldn't this be not possible? The two vectors appear to use different allocators. There was a problem hiding this comment. |
||
| } | ||
| return false; | ||
| } | ||
| } | ||
|
|
||
| for (auto group : mGroupsToSelfCheck) | ||
|
There was a problem hiding this comment. Nit: This is irrelevant to this PR, but let's avoid unnecessary copy here by changing this line to Also |
||
| { | ||
| collision = mCollisionDetector->collide( | ||
| group.get(), mCollisionOptions, &collisionResult); | ||
| if (collision) | ||
| { | ||
| if (_outcome != nullptr) | ||
| { | ||
| auto collisionOutcome = static_cast<CollisionFreeOutcome*>(_outcome); | ||
| auto collidingBodies = collisionResult.getCollidingBodyNodes(); | ||
| for (const auto& elem : collidingBodies) | ||
| { | ||
| collisionOutcome->markSelfCollisionBodyNode(elem->getName()); | ||
| } | ||
| } | ||
|
There was a problem hiding this comment. Nit: If you make collisionFreeOutcome->mSelfContacts = collisionResult.getContacts(); |
||
| return false; | ||
| } | ||
| } | ||
| return true; | ||
| } | ||
|
|
||
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,51 @@ | ||
| #include <aikido/constraint/outcome/CollisionFreeOutcome.hpp> | ||
|
There was a problem hiding this comment. Super nit: In a source file, we include the associated header very first then others in order of STL headers, 3rd party library headers, and AIKIDO headers. |
||
|
|
||
|
There was a problem hiding this comment. We might want to include |
||
| namespace aikido { | ||
| namespace constraint { | ||
|
|
||
| //============================================================================== | ||
| bool CollisionFreeOutcome::isSatisfied() const | ||
| { | ||
| if (mCollisionBodyNodes.size() > 0 || mSelfCollisionBodyNodes.size() > 0) | ||
|
There was a problem hiding this comment. Nit: |
||
| { | ||
| return false; | ||
| } | ||
|
|
||
| return true; | ||
| } | ||
|
|
||
| //============================================================================== | ||
| std::string CollisionFreeOutcome::toString() const | ||
| { | ||
| 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 std::move(ss.str()); | ||
|
There was a problem hiding this comment. I believe we don't need to use |
||
| } | ||
|
|
||
| //============================================================================== | ||
| 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 | ||
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Document the
TestableOutcomeargument. I suggest making it simply return whateverTestableOutcomeis returned by the_poseConstraintpassed to this class' constructor.