diff --git a/include/hpp/manipulation/path-planner/end-effector-trajectory.hh b/include/hpp/manipulation/path-planner/end-effector-trajectory.hh
index 098de5447ceb5fef03ecc157b2321b74d013f519..783ad7fe5b89633dc0c8a93481783edb79dc2500 100644
--- a/include/hpp/manipulation/path-planner/end-effector-trajectory.hh
+++ b/include/hpp/manipulation/path-planner/end-effector-trajectory.hh
@@ -52,6 +52,46 @@ typedef shared_ptr<IkSolverInitialization> IkSolverInitializationPtr_t;
HPP_PREDEF_CLASS(EndEffectorTrajectory);
typedef shared_ptr<EndEffectorTrajectory> EndEffectorTrajectoryPtr_t;
+/// \addtogroup path_planning
+/// \{
+
+/// Plan a path for a robot with constrained trajectory of an end effector
+///
+/// This path planner only works with a steering method of type
+/// steeringMethod::EndEffectorTrajectory. The steering
+/// method defines the desired end-effector trajectory using a time-varying
+/// constraint.
+///
+/// To plan a path between two configurations \c q_init and \c q_goal, the
+/// configurations must satisfy the constraint at the beginning and at the
+/// end of the definition interval respectively.
+///
+/// The interval of definition \f$[0,T]\f$ of the output path is defined by the
+/// time-varying constraint of the steering method. This interval is uniformly
+/// discretized in a number of samples that can be accessed using method \link
+/// EndEffectorTrajectory::nDiscreteSteps nDiscreteSteps \endlink.
+///
+/// The path is planned by successively calling method \link
+/// EndEffectorTrajectory::oneStep oneStep \endlink that performs the following
+/// actions.
+/// - A vector of configurations is produced by appending random
+/// configurations to \c q_init. The number of random configurations can
+/// be accessed by methods \link EndEffectorTrajectory::nRandomConfig
+/// nRandomConfig \endlink.
+/// - for each configuration in the vector,
+/// - the initial configuration of the path is computed by projecting the
+/// configuration on the constraint,
+/// - the configuration at following samples is computed by projecting the
+/// configuration at the previous sample using the time-varying
+/// constraint.
+/// - In case of failure
+/// - in projecting a configuration or
+/// - in validating the path for collision,
+/// the loop restart with the next random configuration.
+///
+/// Note that continuity is not tested but enforced by projecting the
+/// configuration of the previous sample to compute the configuration at
+/// a given sample.
class HPP_MANIPULATION_DLLAPI EndEffectorTrajectory : public core::PathPlanner {
public:
/// Return shared pointer to new instance
@@ -129,6 +169,7 @@ class HPP_MANIPULATION_DLLAPI EndEffectorTrajectory : public core::PathPlanner {
/// Feasibility
bool feasibilityOnly_;
}; // class EndEffectorTrajectory
+// \}
} // namespace pathPlanner
} // namespace manipulation
} // namespace hpp
diff --git a/include/hpp/manipulation/steering-method/end-effector-trajectory.hh b/include/hpp/manipulation/steering-method/end-effector-trajectory.hh
index 5117641a261fb9bb4106c2155c891c14f75228db..623cb92b1f442230951580015727c9f8d4d15aa4 100644
--- a/include/hpp/manipulation/steering-method/end-effector-trajectory.hh
+++ b/include/hpp/manipulation/steering-method/end-effector-trajectory.hh
@@ -35,15 +35,40 @@
namespace hpp {
namespace manipulation {
-/// \addtogroup steering_method
-/// \{
namespace steeringMethod {
HPP_PREDEF_CLASS(EndEffectorTrajectory);
typedef shared_ptr<EndEffectorTrajectory> EndEffectorTrajectoryPtr_t;
using core::PathPtr_t;
-/// Build StraightPath constrained by a varying right hand side constraint.
+/// \addtogroup steering_method
+/// \{
+
+/// Build piecewise straight paths for a robot end-effector
+///
+/// To use this steering method, the user needs to provide
+/// \li a constraint with value in \f$SE(3)\f$. An easy way to create such a
+/// constraint is to use method hpp::manipulation::Handle::createGrasp. The
+/// constraint is passed to the steering method using method \link
+/// EndEffectorTrajectory::trajectoryConstraint trajectoryConstraint \endlink.
+/// \li the time-varying right hand side of this constraint along the path
+/// the user wants to create in the form of a hpp::core::Path instance
+/// with values in \f$SE(3)\f$. For that, \link
+/// EndEffectorTrajectory::makePiecewiseLinearTrajectory
+/// makePiecewiseLinearTrajectory \endlink method may be useful.
+///
+/// \warning the constraint should also be inserted in the \link
+/// hpp::core::SteeringMethod::constraints set of constraints \endlink
+/// of the steering method.
+///
+/// Once the steering method has been initialized, it can be called between
+/// two configurations \c q1 and \c q2. The interval of definition \f$[0,T]\f$
+/// of the output path is the same as the one of the path provided as the right
+/// hand side of the constraint.
+/// Note that \c q1 and \c q2 should satisfy the constraint at times 0 and
+/// \f$T\f$ respectively. The output path is a \link hpp::core::StraightPath
+/// linear interpolation \endlink between \c q1 and \c q2 projected on the
+/// steering method constraints.
class HPP_MANIPULATION_DLLAPI EndEffectorTrajectory
: public core::SteeringMethod {
public:
@@ -141,8 +166,8 @@ class HPP_MANIPULATION_DLLAPI EndEffectorTrajectory
interval_t timeRange_;
constraints::ImplicitPtr_t constraint_;
};
-} // namespace steeringMethod
/// \}
+} // namespace steeringMethod
} // namespace manipulation
} // namespace hpp
diff --git a/src/path-planner/end-effector-trajectory.cc b/src/path-planner/end-effector-trajectory.cc
index 1a874c7c0350a5cf34a5729625f66bc2bfda8075..4a708bb2b930c394be6c2f38e7c4b54b3491dc3b 100644
--- a/src/path-planner/end-effector-trajectory.cc
+++ b/src/path-planner/end-effector-trajectory.cc
@@ -151,6 +151,8 @@ void EndEffectorTrajectory::oneStep() {
core::interval_t timeRange(sm->timeRange());
+ // Generate a vector if configuration where the first one is the initial
+ // configuration and the following ones are random configurations
std::vector<core::Configuration_t> qs(
configurations(*problem()->initConfig()));
if (qs.empty()) {
@@ -166,12 +168,23 @@ void EndEffectorTrajectory::oneStep() {
vector_t times(nDiscreteSteps_ + 1);
matrix_t steps(problem()->robot()->configSize(), nDiscreteSteps_ + 1);
+ // Discretize definition interval of the steering method into times
times[0] = timeRange.first;
for (int j = 1; j < nDiscreteSteps_; ++j)
times[j] = timeRange.first +
j * (timeRange.second - timeRange.first) / nDiscreteSteps_;
times[nDiscreteSteps_] = timeRange.second;
+ // For each random configuration,
+ // - compute initial configuration of path by projecting the random
+ // configuration (initial configuration for the first time),
+ // - compute following samples by projecting current sample after
+ // updating right hand side.
+ // If failure, try next random configuration.
+ // Failure can be due to
+ // - projection,
+ // - collision of final configuration,
+ // - validation of path (for collision mainly).
for (i = 0; i < qs.size(); ++i) {
if (resetRightHandSide) {
constraints->rightHandSideAt(times[0]);
@@ -200,6 +213,7 @@ void EndEffectorTrajectory::oneStep() {
if (!success) continue;
success = false;
+ // Test collision of final configuration.
if (!cfgValidation->validate(steps.col(nDiscreteSteps_), cfgReport)) {
hppDout(info, "Destination config is in collision.");
continue;
@@ -220,6 +234,10 @@ void EndEffectorTrajectory::oneStep() {
continue;
}
+ // In case of success,
+ // - insert q_init as initial configuration of the roadmap,
+ // - insert final configuration as goal node in the roadmap,
+ // - add a roadmap edge between them and stop.
roadmap()->initNode(make_shared<Configuration_t>(steps.col(0)));
core::NodePtr_t init = roadmap()->initNode();
core::NodePtr_t goal = roadmap()->addGoalNode(