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 in state Placement, this transition will produce a target configuration (method generateTargetConfig) as follows.
pick a random configuration , in the edge histogram (see method histogram)
compute right hand side of grasp_comp with ,
compute right hand side of placement_comp with ,
solve (grasp, placement, placement_comp, grasp_comp) using input configuration . 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.
Generate a reachable configuration in the target state
Parameters
qStart
node containing the configuration defining the right hand side of the edge path constraint,
[in,out]
q
input configuration used to initialize the numerical solver and output configuration lying in the target state and reachable along the edge from nnear.
Generate a reachable configuration in the target state
Parameters
qStart
configuration defining the right hand side of the edge path constraint,
[in,out]
q
input configuration used to initialize the numerical solver and output configuration lying in the target state and reachable along the edge from nnear.
Generate a reachable configuration in the target state
Parameters
nStart
node containing the configuration defining the right hand side of the edge path constraint,
[in,out]
q
input configuration used to initialize the numerical solver and output configuration lying in the target state and reachable along the edge from nStart.
The edge histogram is a container of configurations defined by a set of constraints called the condition constraints that a configuration should satisfy to be inserted in the histogram.
The histogram is passed to the Roadmap via the graph (method Graph::insertHistogram). The roadmap then populates the histogram with all new configurations satisfying the condition constraints.
The condition constraints should therefore be the constraints of the target state of the level set edge.