-
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
Changes from all commits
3fcb61f
46ed3d8
4dfa900
9368a14
235e0da
98ef161
9c36bae
62af07b
081e23e
d28417c
faffb52
3ecb343
a276c6a
c281a03
f83c72a
9863a86
924041b
a4a0309
2af9453
4ec21f8
4e2e20f
b8e0010
816e86e
4174efc
511555a
b3da15d
e061d63
e1ed415
6c2dbec
d3ada66
7b10b67
0868db7
cbc1b34
7a3b762
079b2f3
e990862
ce4160d
4768cda
43c8f92
e5e5d5b
354d3d5
18b2dda
cd9d522
2c67a67
0f6731b
d1b2040
ab1556f
b70af0a
9208f04
12647a3
2fae405
1357778
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,61 @@ | ||
#ifndef AIKIDO_CONSTRAINT_COLLISIONFREEOUTCOME_HPP_ | ||
#define AIKIDO_CONSTRAINT_COLLISIONFREEOUTCOME_HPP_ | ||
|
||
#include <vector> | ||
#include "TestableOutcome.hpp" | ||
#include "dart/collision/CollisionObject.hpp" | ||
#include "dart/collision/Contact.hpp" | ||
#include "dart/dynamics/BodyNode.hpp" | ||
#include "dart/dynamics/ShapeFrame.hpp" | ||
#include "dart/dynamics/ShapeNode.hpp" | ||
|
||
namespace aikido { | ||
namespace constraint { | ||
|
||
/// TestableOutcome derivative class intended as (optional) input to isSatisfied | ||
/// method in CollisionFree class. | ||
class CollisionFreeOutcome : public TestableOutcome | ||
{ | ||
friend class CollisionFree; | ||
|
||
public: | ||
/// Documentation inherited. | ||
bool isSatisfied() const override; | ||
|
||
/// Returns a string with each pair of CollisionObject names on a separate | ||
/// line. Each pair is also marked as being a normal collision or self | ||
/// collision. | ||
std::string toString() const override; | ||
|
||
/// Clears this outcome object. Useful in the event that a CollisionFree | ||
/// object is passed to the isSatisfied() method of more than one constraint | ||
/// object. | ||
void clear(); | ||
|
||
/// Return a copy of the vector storing the Contact objects from pairwise | ||
/// collisions. | ||
std::vector<dart::collision::Contact> getPairwiseContacts() const; | ||
|
||
/// Return a copy of the vector storing the Contact objects from self | ||
/// collisions. | ||
std::vector<dart::collision::Contact> getSelfContacts() const; | ||
|
||
/// Gets the name of a CollisionObject. The name returned is that of the | ||
/// corresponding BodyNode (if possible). If not, the name of the ShapeFrame | ||
/// is returned instead. This is a helper for toString(). | ||
/// \param[in] object object pointer to CollisionObject we want the name of. | ||
std::string getCollisionObjectName( | ||
const dart::collision::CollisionObject* object) const; | ||
|
||
protected: | ||
/// Holds Contact objects from pairwise collisions. | ||
std::vector<dart::collision::Contact> mPairwiseContacts; | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe 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 commentThe 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 commentThe reason will be displayed to describe this comment to others. Learn more. You're right! 😱 I always forget that |
||
|
||
/// Holds Contact objects from self collisions. | ||
std::vector<dart::collision::Contact> mSelfContacts; | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. This should us an |
||
}; | ||
|
||
} // namespace constraint | ||
} // namespace aikido | ||
|
||
#endif // AIKIDO_CONSTRAINT_COLLISIONFREEOUTCOME_HPP_ |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,35 @@ | ||
#ifndef AIKIDO_CONSTRAINT_DEFAULTTESTABLEOUTCOME_HPP_ | ||
#define AIKIDO_CONSTRAINT_DEFAULTTESTABLEOUTCOME_HPP_ | ||
|
||
#include "TestableOutcome.hpp" | ||
|
||
namespace aikido { | ||
namespace constraint { | ||
|
||
/// Simple default TestableOutcome derivative class. An instance of this class | ||
/// is returned when createOutcome() is called on an instance of a class that | ||
/// inherits Testable, but has no corresponding TestableOutcome derivative | ||
/// implemented. | ||
class DefaultTestableOutcome : public TestableOutcome | ||
{ | ||
public: | ||
/// Returns whether the isSatisfied method this object was passed to | ||
/// returned true or false. | ||
bool isSatisfied() const override; | ||
|
||
/// String representation of isSatisfied return value. | ||
std::string toString() const override; | ||
|
||
/// Used by the isSatisfied this outcome object is passed to set whether the | ||
/// constraint was satisifed or not. | ||
/// \param[in] satisfiedFlag whether the constraint was satisfied or not. | ||
void setSatisfiedFlag(bool satisfiedFlag); | ||
|
||
protected: | ||
bool mSatisfiedFlag; | ||
}; | ||
|
||
} // namespace constraint | ||
} // namespace aikido | ||
|
||
#endif // AIKIDO_CONSTRAINT_DEFAULTTESTABLEOUTCOME_HPP_ |
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -29,7 +29,17 @@ class FrameTestable : public Testable | |
/// \param _state a MetaskeletonState to set the configuration of this | ||
/// constraint's metaskeketon. This state's StateSpace should match | ||
/// StateSpace returend by getStateSpace(). | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Document the |
||
bool isSatisfied(const statespace::StateSpace::State* _state) const override; | ||
/// \param outcome Testable outcome derivative class. Passed to the | ||
/// isSatisfied method of mPoseConstraint to allow "pass through" of | ||
/// debugging information. | ||
bool isSatisfied( | ||
const statespace::StateSpace::State* _state, | ||
TestableOutcome* outcome = nullptr) const override; | ||
|
||
/// Return the outcome of mPoseConstraint->createOutcome(). Reason: | ||
/// isSatisfied in this class will just pass outcome to the isSatisfied | ||
/// method of of mPoseConstraint. | ||
std::unique_ptr<TestableOutcome> createOutcome() const override; | ||
|
||
// Documentation inhereted | ||
std::shared_ptr<statespace::StateSpace> getStateSpace() const override; | ||
|
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -3,27 +3,42 @@ | |
|
||
#include <memory> | ||
#include "../statespace/StateSpace.hpp" | ||
#include "DefaultTestableOutcome.hpp" | ||
|
||
namespace aikido { | ||
namespace constraint { | ||
|
||
class TestableOutcome; | ||
|
||
/// Constraint which can be tested. | ||
class Testable | ||
{ | ||
public: | ||
virtual ~Testable() = default; | ||
|
||
/// Returns true if state satisfies this constraint. | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Document the |
||
/// \param[in] _state given state to test. | ||
/// \param[in] outcome pointer to TestableOutcome derivative instance that | ||
/// method will populate with useful information. Each derivative class of | ||
/// Testable may expect outcome to be a different derivative class of | ||
/// TestableOutcome (this casting and population is done under the hood). If | ||
/// this argument is missing, it is ignored. | ||
virtual bool isSatisfied( | ||
const statespace::StateSpace::State* _state) const = 0; | ||
const statespace::StateSpace::State* _state, | ||
TestableOutcome* outcome = nullptr) const = 0; | ||
|
||
/// Returns StateSpace in which this constraint operates. | ||
virtual statespace::StateSpacePtr getStateSpace() const = 0; | ||
|
||
/// Return an instance of a TestableOutcome derivative class that corresponds | ||
/// to this constraint class. Ensures that correct outcome object is passed | ||
/// to isSatisfied (and casts, etc do not explode). | ||
virtual std::unique_ptr<TestableOutcome> createOutcome() const = 0; | ||
}; | ||
|
||
using TestablePtr = std::shared_ptr<Testable>; | ||
|
||
} // namespace constraint | ||
} // namespace aikido | ||
|
||
#endif // AIKIDO_CONSTRAINT_TESTABLE_HPP_ | ||
#endif // AIKIDO_CONSTRAINT_TESTABLE_HPP_ |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,36 @@ | ||
#ifndef AIKIDO_CONSTRAINT_TESTABLEOUTCOME_HPP_ | ||
#define AIKIDO_CONSTRAINT_TESTABLEOUTCOME_HPP_ | ||
|
||
#include <string> | ||
|
||
namespace aikido { | ||
namespace constraint { | ||
|
||
/// Base class for constraint outcomes. At a high level, each constraint can | ||
/// have a corresponding derivative of TestableOutcome that is passed as an | ||
/// optional parameter to its isSatisfied method. This allow programmatic access | ||
/// to data on why the constraint was (or was not) satisfied. | ||
class TestableOutcome | ||
{ | ||
public: | ||
/// Returns true if isSatisfied call this outcome object was passed to | ||
/// returned true. False otherwise. | ||
virtual bool isSatisfied() const = 0; | ||
|
||
/// String representation of the outcome. Provides useful, programmatic | ||
/// access to debug information. | ||
virtual std::string toString() const = 0; | ||
}; | ||
|
||
/// Helper function. Avoids repeating logic for casting TestableOutcome | ||
/// pointers down to pointers for a derivative class. Mostly used in the | ||
/// isSatisfied methods of classes that inherit Testable. | ||
template <class Child> | ||
Child* dynamic_cast_or_throw(TestableOutcome* outcome); | ||
|
||
} // namespace constraint | ||
} // namespace aikido | ||
|
||
#include "detail/TestableOutcome-impl.hpp" | ||
|
||
#endif // AIKIDO_CONSTRAINT_TESTABLEOUTCOME_HPP_ |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,28 @@ | ||
#include <sstream> | ||
#include <stdexcept> | ||
#include <typeinfo> | ||
|
||
namespace aikido { | ||
namespace constraint { | ||
|
||
//============================================================================== | ||
template <class Child> | ||
Child* dynamic_cast_or_throw(TestableOutcome* outcome) | ||
{ | ||
if (!outcome) | ||
return nullptr; | ||
|
||
auto childPtr = dynamic_cast<Child*>(outcome); | ||
if (!childPtr) | ||
{ | ||
std::stringstream message; | ||
message << "TestableOutcome pointer is not of type " << typeid(Child).name() | ||
<< "."; | ||
throw std::invalid_argument(message.str()); | ||
} | ||
|
||
return childPtr; | ||
} | ||
|
||
} // namespace constraint | ||
} // namespace aikido |
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.
Very tiny nitpick: It's also useful if an instance is passed to
isSatisfied()
multiple times on the same constraint object. 🙃 This is actually the more common case in a planner.