-
Notifications
You must be signed in to change notification settings - Fork 30
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Testable Constraint Output #266
Conversation
…res in derived classes. [TODO]: Figure out the unused param warnings.
…. Included it in CollisionFree.
… it was added as a source file.
Codecov Report
@@ Coverage Diff @@
## master #266 +/- ##
==========================================
- Coverage 80.26% 79.73% -0.54%
==========================================
Files 195 201 +6
Lines 5671 5768 +97
==========================================
+ Hits 4552 4599 +47
- Misses 1119 1169 +50
|
I added the checklist that is for our PRs. |
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.
P.S. You pinged a wrong guy for your second question in TODOs. Our Mike is @mkoval (not at_mkova) 😉
private: | ||
std::vector<std::string> mCollisionBodyNodes; | ||
std::vector<std::string> mSelfCollisionBodyNodes; | ||
}; |
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.
Could we have docstring for this class and its members?
#ifndef AIKIDO_CONSTRAINT_COLLISIONFREEOUTCOME_HPP_ | ||
#define AIKIDO_CONSTRAINT_COLLISIONFREEOUTCOME_HPP_ | ||
|
||
#include <sstream> |
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.
Nit: It seems we need to include <string>
rather than <sstream>
.
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.
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.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
sstream
is not exposed in this header file but used in the source file. So I suggest we include <string>
as std::string
is explicitly exposed in this header and include <sstream>
in the source file where it's actually used.
The motivation behind this is that this way prevents unnecessarily including <sstream>
by including CollisionFreeOutcome.hpp
.
class CollisionFreeOutcome : public TestableOutcome | ||
{ | ||
public: | ||
bool isSatisfied() const; |
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.
Nit: Need override
specifier.
{ | ||
public: | ||
bool isSatisfied() const; | ||
std::string toString() const; |
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.
Nit: Need override
specifier.
public: | ||
virtual bool isSatisfied() const = 0; | ||
virtual std::string toString() const = 0; | ||
}; |
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.
Could we have docstring for this class and its members?
ss << "[COLLISION]: " << collBodyNodeName << std::endl; | ||
} | ||
|
||
for (auto selfCollBodyNodeName : mSelfCollisionBodyNodes) |
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.
Nit: const auto&
ss << "[SELF COLLISION]: " << selfCollBodyNodeName << std::endl; | ||
} | ||
|
||
return std::move(ss.str()); |
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.
I believe we don't need to use std::move()
for returning local variable in favor of RVO.
@@ -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 |
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.
A general way to suppress warnings for unused parameter is simply commenting out the parameter name as
bool CartesianProductTestable::isSatisfied(
const aikido::statespace::StateSpace::State* _state,
TestableOutcome* /*_outcome*/) const
This is an answer to TODOs.1.
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.
Thanks! 😄 This is a super handy thing to know.
src/constraint/CollisionFree.cpp
Outdated
if (_outcome != nullptr) | ||
{ | ||
auto collisionOutcome = static_cast<CollisionFreeOutcome*>(_outcome); | ||
auto collidingBodies = collisionResult.getCollidingBodyNodes(); |
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.
Nit: const auto&
to avoid the unnecessary copy.
src/constraint/CollisionFree.cpp
Outdated
{ | ||
if (_outcome != nullptr) | ||
{ | ||
auto collisionOutcome = static_cast<CollisionFreeOutcome*>(_outcome); |
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.
It's not guaranteed if _outcome
is the type of CollisionFreeOutcome*
. I think we should do one of the follows:
(1) use assertion (in debug mode) and dynamic_cast
to check if _outcome
is the right type. The downside would be that we still have problem in release mode
(2) run the routine only when it succeded to cast to the right type using dynamic_cast
(warnings if the type is not the right one). The downside would be the overhaed of using dynamic_cast
.
(3) throw exception when the type is not right. The downside would be that it has to use dynamic_cast
anyway.
I'm not sure what would be the preferred way here. @brianhou , @mkoval, @gilwoolee thoughts?
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.
Is the only difference between (2) and (3) whether we silently fail or raise an exception when the TestableOutcome
is the wrong type? In that case, I think I'd prefer (3) over (2) to make sure we know that we've made a mistake.
My (limited) understanding is that we won't be passing a non-null TestableOutcome
that often, so I'm also not as concerned about the overhead of using a dynamic_cast
in release mode. Perhaps we can use (3) now and if it becomes performance-critical we can re-evaluate and perhaps switch to (1).
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.
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.
@mkoval I also have a similar question about item 4 in the comment. In the case that we don't know the type of auto outcome = testable->createOutcome();
testable->isSatisfied(state, outcome);
std::cout << outcome->toString() << std::endl; |
(sorry for reviewing in multiple comments) Also, I'm not sure if we really want to put |
Awesome feedback! Working through this now. |
public: | ||
bool isSatisfied() const; | ||
std::string toString() const; | ||
void markCollisionBodyNode(const std::string& bodyNodeName); |
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.
Can we call this markPairwiseCollisionBodyNode
?
void markSelfCollisionBodyNode(const std::string& bodyNodeName); | ||
|
||
private: | ||
std::vector<std::string> mCollisionBodyNodes; |
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.
...and then call this mPairwiseCollisionBodyNodes
.
src/constraint/CollisionFree.cpp
Outdated
{ | ||
if (_outcome != nullptr) | ||
{ | ||
auto collisionOutcome = static_cast<CollisionFreeOutcome*>(_outcome); |
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.
Is the only difference between (2) and (3) whether we silently fail or raise an exception when the TestableOutcome
is the wrong type? In that case, I think I'd prefer (3) over (2) to make sure we know that we've made a mistake.
My (limited) understanding is that we won't be passing a non-null TestableOutcome
that often, so I'm also not as concerned about the overhead of using a dynamic_cast
in release mode. Perhaps we can use (3) now and if it becomes performance-critical we can re-evaluate and perhaps switch to (1).
src/constraint/CollisionFree.cpp
Outdated
{ | ||
auto collisionOutcome = static_cast<CollisionFreeOutcome*>(_outcome); | ||
auto collidingBodies = collisionResult.getCollidingBodyNodes(); | ||
for (const auto& elem : collidingBodies) |
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.
Maybe this should iterate over CollisionResult::getContacts()
instead. That will allow us to specify which pairs of bodies are in collision, rather than squashing that information into a list of bodies that are in collision.
This isn't a problem now because our default CollisionOption
only detects a maximum of one contact between two bodies, but we may as well support multiple contacts on the off-chance that becomes helpful.
@@ -64,6 +65,11 @@ class CollisionFreeTest : public ::testing::Test | |||
mStateSpace = std::make_shared<MetaSkeletonStateSpace>(group); | |||
mManipulator->enableSelfCollisionCheck(); | |||
mManipulator->enableAdjacentBodyCheck(); | |||
|
|||
// See what CollisionFreeOutcome's toString returns when no collision |
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.
We should take advantage of having programmatic access to the information (rather than as a giant string). Maybe we can instead have a vector of body names that we check the outcome's vectors against.
…hub.com/personalrobotics/aikido into sniyaz/feature/testableConstraintOutput
Pretty much there. Two biggest changes:
Various other tweaks as well (see comments). I think this should be ready to merge soon, so @jslee02 @brianhou @mkoval, let me know if you have any other thoughts. |
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.
This looks good. Thanks for making all of those changes - the final implementation is very nice. I would be happy to merge this once you shift the implementation of dynamic_cast_if_present
to the correct file.
} | ||
|
||
return childPtr; | ||
} |
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.
Put the declaration of this function here, but move the implementation of the function into detail/TestableOutcome-impl.hpp
. So the end result should look like this:
TestableOutcome.hpp
:
namespace aikido {
namespace constraint {
template <class Child>
Child* dynamic_cast_if_present(TestableOutcome* outcome);
} // namespace constraint
} // namespace aikido
#include "detail/TestableOutcome-impl.hpp"
detail/TestableOutcome-impl.hpp
:
namespace aikido {
namespace constraint {
Child* dynamic_cast_if_present(TestableOutcome* outcome);
{
// implementation goes here
}
} // namespace constraint
} // namespace aikido
See JointSpaceHelpers.hpp
for an example of this paradigm. The goal of these machinations is keep our "public" headers free of implementation details, even when C++ forces us to put some of our definitions (e.g templates) in our headers.
@jslee02: Where would be the best place to document this type of convention?
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.
src/constraint/CollisionFree.cpp
Outdated
//============================================================================== | ||
std::unique_ptr<TestableOutcome> CollisionFree::createOutcome() const | ||
{ | ||
return std::unique_ptr<TestableOutcome>(new CollisionFreeOutcome()); |
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.
Nit: new CollisionFreeOutcome()
-> new CollisionFreeOutcome
src/constraint/FrameTestable.cpp
Outdated
@@ -46,9 +46,18 @@ bool FrameTestable::isSatisfied( | |||
auto st = mPoseStateSpace->createState(); | |||
mPoseStateSpace->setIsometry(st, mFrame->getTransform()); | |||
|
|||
if (outcome) | |||
return mPoseConstraint->isSatisfied(st, outcome); | |||
|
|||
return mPoseConstraint->isSatisfied(st); |
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.
You could collapse all of this to just:
mPoseConstraint->isSatisfied(st, outcome);
There is no harm in forwarding nullptr
along to the wrapped constraint.
@@ -39,9 +40,16 @@ class CollisionFree : public Testable | |||
// Documentation inherited. | |||
statespace::StateSpacePtr getStateSpace() const override; | |||
|
|||
// Documentation inherited. | |||
// Documentation inherited. As an important note, outcome is expected to be |
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.
Nit: It would be great to expose this note by making this doxygen comment as:
/// \copydoc Testable::isSatisfied()
/// \note Outcome is expected to be an instance of ...
By doing above, we could see this note in addition to the original docstring of Testable::isSatisfied()
.
const aikido::statespace::StateSpace::State* _state, | ||
TestableOutcome* outcome = nullptr) const override; | ||
|
||
// Documentation inherited. Returns an instance of CollisionFreeOutcome. |
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.
Nit: ditto
/// \copydoc Testable::createOutcome()
/// Returns an instance of CollisionFreeOutcome.
} | ||
|
||
return childPtr; | ||
} |
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.
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.
I believe aligned_allocator
is not necessary for dart::collision::Contact
for the reason I mentioned above. If @mkoval agree on this, I think we should revert the change.
Otherwise, this PR looks good to go.
/// pointers down to pointers for a derivative class. Mostly used in the | ||
/// isSatisfied methods of classes that inherit Testable. | ||
template <class Child> | ||
Child* dynamic_cast_if_present(TestableOutcome* outcome); |
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.
Suggestion: I think it would be more informative if the name of this function represents that it throws an exception when it fails to dynamic-cast, which is the main difference from dynamic_cast
, something like dynamic_cast_or_throw
or strict_dynamic_cast
. How does this sound?
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.
Looks good to me! 💯
Oh, one minor thing. It would be great to add an item for this change to |
@brianhou Thanks for updating This might be the last change for AIKIDO 0.2.0. |
* Added TestableOutcomeClass. * Added NonCollidingOutcome.hpp * Include TestableOutcome in Testable. Fix TestableOutcome to make it work. * Updated Testable isSatisfied signature and changed *bunch* of signatures in derived classes. [TODO]: Figure out the unused param warnings. * Changed NonCollidingOutcome name to reflect CollisionFree name change. Included it in CollisionFree. * Started CollisionFreeOutcome.cpp. Small changed to make it build once it was added as a source file. * Wrote toString for CollisionFreeOutcome. * Made CollisionFree fill _outcome on a collision (if not nullptr). * Fixed test files to make tests build. Will run make format in just a sec! * Added testing for CollisionFreeOutcome. * Use move semantics in CollisionFreeOutcome toString method. * Ran make format. * Adressed nits from @jslee02. Moving to squish those unused param warnings next. * Silence unused param warning, ran make format again. * Used dynamic_cast in CollisionFree when populating outcome fields. Barf if type mismatch. * Modify CollisionFreeOutcome to take Contact references as inputs as per feedback from @brianhou. Added a method to get the name from a CollisionObject. * Move include of sstream into .cpp file instead of header. * Made tests take advantage of programatic acess to data. Required adding a getter for Contact vectors in CollisionFreeOutcome and making getCollisionObjectName public. * Made getter methods for Contact vectors const because they can be. * Address nits from @jslee02. * Two small nits I missed in the last commit. * Aaaaaand, one more! * Implemented createOutcome functionality in Testable base class. * Use auto collisionOutcome as per request of @brianhou. * Moved dynamic cast for outcome in CollisionFree. * Added method to clear CollisionFreeOutcome. Use this method in CollisionFree so outcome objects can be reused. Also added stuff to test to make sure outcomes can actually be reused. * Fixed directory placement of outcome classes. Renamed DummyOutcome ---> DefaultOutcome. * Added simple functionality to DefaultOutcome. * Responded to most recent review comments by making every Testable derivative class implement createOutcome (createOutcome in Testable is now a pure virtual function). Also made each implementation of isSatisfied in any Testable derivative class populate the isSatisfied field of DefaultOutcome. * Run make format. * [WIP] Refactor castic logic into helper, and use this in CollisionFree. Will stamp this into every other class now. * [WIP] Replace redundant casting login in different Testables with helper. * [WIP] Mop up some extra casts that were missed prior. * Added documentation. * Make CollisionFree a friend class of CollisionFreeOutcome, use Eigen alocator. * Modify FrameTestable to "pass through" outcome object to mPoseConstraint. * Rename DefaultOutcome ---> DefaultTestableOutcome. Final clean up as well. * Address @mkoval's comments. * Update nits: comment and renamed dynamic_cast_if_present to dynamic_cast_or_throw. * Undo the use of the Eigen allocator in CollisionFreeOutcome. * Update CHANGELOG.md
See #237.
Summary of changes:
New TestableOutcome class and corresponding CollisionFreeOutcome derivative class for CollisionFree (NonColliding was renamed to CollisionFree in Rename NonColliding to CollisionFree #256).
Change base constraint class (Testable) to add TestableOutcome (base constraint outcome class) as parameter to isSatisfied.
CollisionFree constraint now populates this field on a collision (self or otherwise).
TODO:
@mkoval, l I had a question about #235, item 4. This is something I'd be happy to implement before merging, but I was curious what sort of use case this scenario would pop up in.
Test Plan
Also checked CollisionFreeOutcome's toString and isSatisfied methods in test_CollisionFree unit tests. Printed the return value of toString separately, and result seemed sane.
Before creating a pull request
make format
Before merging a pull request
CHANGELOG.md