|
virtual | ~LevelSetEdge () |
|
virtual bool | generateTargetConfig (core::NodePtr_t nStart, ConfigurationOut_t q) const |
|
virtual bool | generateTargetConfig (ConfigurationIn_t qStart, ConfigurationOut_t q) const |
|
bool | generateTargetConfigOnLeaf (ConfigurationIn_t qStart, ConfigurationIn_t qLeaf, ConfigurationOut_t q) const |
|
virtual ConstraintSetPtr_t | buildTargetConstraint () |
| Build path and target state constraints.
|
|
void | buildHistogram () |
|
LeafHistogramPtr_t | histogram () const |
|
virtual std::ostream & | dotPrint (std::ostream &os, dot::DrawingAttributes da=dot::DrawingAttributes()) const |
| Print the object in a stream.
|
|
|
void | insertParamConstraint (const ImplicitPtr_t &nm) |
|
const NumericalConstraints_t & | paramConstraints () const |
| Get constraints parameterizing the target state foliation.
|
|
void | insertConditionConstraint (const ImplicitPtr_t &nm) |
|
const NumericalConstraints_t & | conditionConstraints () const |
|
virtual | ~Edge () |
| Destructor.
|
|
virtual bool | canConnect (ConfigurationIn_t q1, ConfigurationIn_t q2) const |
|
virtual bool | build (core::PathPtr_t &path, ConfigurationIn_t q1, ConfigurationIn_t q2) const |
|
StatePtr_t | stateTo () const |
| Get the destination.
|
|
StatePtr_t | stateFrom () const |
| Get the origin.
|
|
StatePtr_t | state () const |
| Get the state in which path is.
|
|
void | state (StatePtr_t state) |
|
const core::SteeringMethodPtr_t & | steeringMethod () const |
| Get steering method associated to the edge.
|
|
const core::PathValidationPtr_t & | pathValidation () const |
| Get path validation associated to the edge.
|
|
const RelativeMotion::matrix_type & | relativeMotion () const |
|
void | relativeMotion (const RelativeMotion::matrix_type &m) |
| Update the relative motion matrix.
|
|
void | securityMarginForPair (const size_type &row, const size_type &col, const value_type &margin) |
|
const matrix_t & | securityMargins () const |
| Accessor to the security margin.
|
|
virtual bool | direction (const core::PathPtr_t &path) const |
|
virtual bool | intersectionConstraint (const EdgePtr_t &other, ConfigProjectorPtr_t projector) const |
|
ConstraintSetPtr_t | targetConstraint () const |
| Constraint of the destination state and of the path.
|
|
void | setShort (bool isShort) |
|
bool | isShort () const |
|
ConstraintSetPtr_t | pathConstraint () const |
|
virtual | ~GraphComponent () |
|
const std::string & | name () const |
| Get the component name.
|
|
const std::size_t & | id () const |
| Return the component id.
|
|
virtual void | addNumericalConstraint (const ImplicitPtr_t &numConstraint) |
| Add constraint to the component.
|
|
virtual void | addNumericalCost (const ImplicitPtr_t &numCost) |
| Add a cost function Implicit to the component.
|
|
virtual void | resetNumericalConstraints () |
| Reset the numerical constraints stored in the component.
|
|
bool | insertNumericalConstraints (ConfigProjectorPtr_t &proj) const |
|
const NumericalConstraints_t & | numericalConstraints () const |
| Get a reference to the NumericalConstraints_t.
|
|
const NumericalConstraints_t & | numericalCosts () const |
| Get a reference to the NumericalConstraints_t.
|
|
void | parentGraph (const GraphWkPtr_t &parent) |
| Set the parent graph.
|
|
GraphPtr_t | parentGraph () const |
| Set the parent graph.
|
|
virtual void | invalidate () |
|
void | solveLevelByLevel (bool solveLevelByLevel) |
|
bool | solveLevelByLevel () const |
|
|
void | init (const LevelSetEdgeWkPtr_t &weak, const GraphWkPtr_t &graph, const StateWkPtr_t &from, const StateWkPtr_t &to) |
| Initialization of the object.
|
|
| LevelSetEdge (const std::string &name) |
|
virtual std::ostream & | print (std::ostream &os) const |
| Print the object in a stream.
|
|
virtual void | populateTooltip (dot::Tooltip &tp) const |
| Populate DrawingAttributes tooltip.
|
|
virtual void | initialize () |
|
void | init (const EdgeWkPtr_t &weak, const GraphWkPtr_t &graph, const StateWkPtr_t &from, const StateWkPtr_t &to) |
| Initialization of the object.
|
|
| Edge (const std::string &name) |
| Constructor.
|
|
virtual ConstraintSetPtr_t | buildPathConstraint () |
| Build path constraints.
|
|
void | init (const GraphComponentWkPtr_t &weak) |
| Initialize the component.
|
|
| GraphComponent (const std::string &name) |
|
void | throwIfNotInitialized () const |
|
Edge that handles crossed foliations
Let us consider the following simple constraint graph corresponding to a robot grasping an object with one gripper.
Simple constraint graph corresponding to a
robot grasping an object."
In order to disambiguate, we assume here that
\li transition <b>Grasp object</b> is in \b Placement state,
\li transition <b>Release object</b> is in \b Grasp state.
If state \b Placement is defined by the object lying on a planar
polygonal surface, then
\li state \b Placement,
\li transition \b Transit, and
\li transition <b>Grasp object</b>
are all constrained in a foliated manifold parameterized by the
position of the box on the surface.
Likewise, if the object is cylindrical the grasp may have a degree
of freedom corresponding to the angle around z-axis of the gripper
with respect to the object. See classes
\link hpp::manipulation::Handle Handle\endlink and
\link hpp::pinocchio::Gripper Gripper\endlink for details.
In this latter case,
\li state \b Grasp,
\li transition \b Transfer, and
\li transition <b>Release object</b>
are all constrained in a foliated manifold parameterized by the
angle around z-axis of the gripper with respect to the object.
Let us denote
\li \c grasp the numerical constraint defining state \b Grasp,
\li \c placement the numerical constraint defining state \b Placement,
\li \c grasp_comp the parameterized constraint defining a leaf
of \c Transfer (the angle between the gripper and the
object),
\li \c placement_comp the parameterized constraint defining a leaf
of \b Placement (the position of the object on the contact
surface).
As explained in <a
href="https://hal.archives-ouvertes.fr/hal-01358767">this paper , we are in the crossed foliation case and manipulation RRT will never be able to connect trees expanding in different leaves of the foliation.
This class solves this issue in the following way by creating an instance of LevelSetEdge between Placement and Grasp.
When extending a configuration \(\mathbf{q}_{start}\) in state Placement, this transition will produce a target configuration (method generateTargetConfig) as follows.
- pick a random configuration \(\mathbf{q}_rand\), in the edge histogram (see method histogram)
- compute right hand side of
grasp_comp
with \(\mathbf{q}_{rand}\),
- compute right hand side of
placement_comp
with \(\mathbf{q}_{start}\),
- solve (
grasp
, placement
, placement_comp
, grasp_comp
) using input configuration \(\mathbf{q}\). Note that the parent method Edge::generateTargetConfig does the same without adding grasp_comp
.
The constraints parameterizing the target state foliation (graps_comp
in our example) are passed to class instances using method insertParamConstraint.