Skip to content
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

Merged
merged 52 commits into from
Jan 7, 2018
Merged

Conversation

sniyaz
Copy link

@sniyaz sniyaz commented Nov 26, 2017

See #237.

Summary of changes:

  1. New TestableOutcome class and corresponding CollisionFreeOutcome derivative class for CollisionFree (NonColliding was renamed to CollisionFree in Rename NonColliding to CollisionFree #256).

  2. Change base constraint class (Testable) to add TestableOutcome (base constraint outcome class) as parameter to isSatisfied.

  3. 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

  • Document new methods and classes
  • Format code with make format

Before merging a pull request

  • Set version target by selecting a milestone on the right side
  • Summarize this change in CHANGELOG.md
  • Add unit test(s) for this change

@sniyaz sniyaz self-assigned this Nov 26, 2017
@codecov
Copy link

codecov bot commented Nov 26, 2017

Codecov Report

Merging #266 into master will decrease coverage by 0.53%.
The diff coverage is 51.96%.

@@            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
Impacted Files Coverage Δ
include/aikido/constraint/CollisionFree.hpp 100% <ø> (ø) ⬆️
...lude/aikido/constraint/uniform/RnBoxConstraint.hpp 100% <ø> (ø) ⬆️
include/aikido/constraint/Testable.hpp 100% <ø> (ø) ⬆️
include/aikido/constraint/TestableIntersection.hpp 100% <ø> (ø) ⬆️
include/aikido/constraint/TSR.hpp 100% <ø> (ø) ⬆️
include/aikido/constraint/FrameTestable.hpp 100% <ø> (ø) ⬆️
...ude/aikido/constraint/CartesianProductTestable.hpp 100% <ø> (ø) ⬆️
include/aikido/constraint/Satisfied.hpp 100% <ø> (ø) ⬆️
...ude/aikido/constraint/uniform/SE2BoxConstraint.hpp 100% <ø> (ø) ⬆️
src/constraint/DefaultTestableOutcome.cpp 0% <0%> (ø)
... and 20 more

@sniyaz sniyaz changed the title [WIP]: Testable Constraint Output Testable Constraint Output Nov 27, 2017
@jslee02
Copy link
Member

jslee02 commented Nov 27, 2017

I added the checklist that is for our PRs.

Copy link
Member

@jslee02 jslee02 left a 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;
};
Copy link
Member

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>
Copy link
Member

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>.

Copy link
Author

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).

Copy link
Member

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;
Copy link
Member

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;
Copy link
Member

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;
};
Copy link
Member

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)
Copy link
Member

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());
Copy link
Member

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
Copy link
Member

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.

Copy link
Author

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.

if (_outcome != nullptr)
{
auto collisionOutcome = static_cast<CollisionFreeOutcome*>(_outcome);
auto collidingBodies = collisionResult.getCollidingBodyNodes();
Copy link
Member

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.

{
if (_outcome != nullptr)
{
auto collisionOutcome = static_cast<CollisionFreeOutcome*>(_outcome);
Copy link
Member

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?

Copy link
Contributor

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).

Copy link
Author

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.

@jslee02
Copy link
Member

jslee02 commented Nov 27, 2017

@mkoval I also have a similar question about item 4 in the comment.

In the case that we don't know the type of Testable at compile time, I think we also have a problem of not knowing what the outcome type should be. For this, I would suggest adding a virtual function std::unique_ptr<Outcome> createOutcome() to Testable so that we could create a right type of outcome from the testable like:

auto outcome = testable->createOutcome();
testable->isSatisfied(state, outcome);
std::cout << outcome->toString() << std::endl;

@jslee02
Copy link
Member

jslee02 commented Nov 27, 2017

(sorry for reviewing in multiple comments)

Also, I'm not sure if we really want to put Outcome class in a nested directory. Do we have a reason for putting the class in the nested directory?

@jslee02 jslee02 added this to the Aikido 0.2.0 milestone Nov 27, 2017
@sniyaz
Copy link
Author

sniyaz commented Nov 28, 2017

Awesome feedback! Working through this now.

public:
bool isSatisfied() const;
std::string toString() const;
void markCollisionBodyNode(const std::string& bodyNodeName);
Copy link
Contributor

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;
Copy link
Contributor

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.

{
if (_outcome != nullptr)
{
auto collisionOutcome = static_cast<CollisionFreeOutcome*>(_outcome);
Copy link
Contributor

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).

{
auto collisionOutcome = static_cast<CollisionFreeOutcome*>(_outcome);
auto collidingBodies = collisionResult.getCollidingBodyNodes();
for (const auto& elem : collidingBodies)
Copy link
Contributor

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
Copy link
Contributor

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.

@sniyaz
Copy link
Author

sniyaz commented Dec 7, 2017

Pretty much there. Two biggest changes:

  1. Made helper method to cast between types of TestableOutcome.

  2. Make every Testable derivative class populate the isSatisfied output. Made every Testable derivative also implement createOutcome.

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.

Copy link
Member

@mkoval mkoval left a 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;
}
Copy link
Member

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?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Where would be the best place to document this type of convention?

@mkoval For now, I believe the best place would be STYLE.md adding a subsection under C++ Style.

//==============================================================================
std::unique_ptr<TestableOutcome> CollisionFree::createOutcome() const
{
return std::unique_ptr<TestableOutcome>(new CollisionFreeOutcome());
Copy link
Member

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

@@ -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);
Copy link
Member

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
Copy link
Member

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.
Copy link
Member

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;
}
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Where would be the best place to document this type of convention?

@mkoval For now, I believe the best place would be STYLE.md adding a subsection under C++ Style.

Copy link
Member

@jslee02 jslee02 left a 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);
Copy link
Member

@jslee02 jslee02 Dec 12, 2017

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?

@jslee02 jslee02 modified the milestones: Aikido 0.2.0, Aikido 0.3.0 Dec 15, 2017
@sniyaz
Copy link
Author

sniyaz commented Jan 5, 2018

Sorry for the pause- this was left unmerged because of the demo. I've addressed the final comments, and this should be ready to merge once and for all.

@brianhou or @jslee02, could one of you do the honors if these updates look good to you?

Copy link
Member

@jslee02 jslee02 left a 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! 💯

@jslee02
Copy link
Member

jslee02 commented Jan 5, 2018

Oh, one minor thing. It would be great to add an item for this change to CHANGELOG.md.

@jslee02 jslee02 modified the milestones: Aikido 0.3.0, Aikido 0.2.0 Jan 6, 2018
@jslee02
Copy link
Member

jslee02 commented Jan 6, 2018

@brianhou Thanks for updating CHANGELOG.md. Please merge this if this looks good to you as well.

This might be the last change for AIKIDO 0.2.0.

@brianhou brianhou merged commit 2390fbb into master Jan 7, 2018
@brianhou brianhou deleted the sniyaz/feature/testableConstraintOutput branch January 7, 2018 01:06
@mkoval mkoval mentioned this pull request Feb 4, 2018
5 tasks
gilwoolee pushed a commit that referenced this pull request Jan 21, 2019
* 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
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

5 participants