[EndEffectorTrajectory] Document steering method and path planner.

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

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
 

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(