diff --git a/CMakeLists.txt b/CMakeLists.txt
index b23425c17395281887538fc309aebe885367bd71..e4a67b1bb2ab83919808446a47eab3863bf7fe50 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -93,6 +93,8 @@ SET (${PROJECT_NAME}_HEADERS
include/hpp/manipulation/path-optimization/random-shortcut.hh
include/hpp/manipulation/path-optimization/spline-gradient-based.hh
+ include/hpp/manipulation/path-planner/end-effector-trajectory.hh
+
include/hpp/manipulation/problem-target/state.hh
include/hpp/manipulation/steering-method/cross-state-optimization.hh
diff --git a/include/hpp/manipulation/path-planner/end-effector-trajectory.hh b/include/hpp/manipulation/path-planner/end-effector-trajectory.hh
new file mode 100644
index 0000000000000000000000000000000000000000..e39dd0f5cbf1a851a24d0891a12f28877c8d9646
--- /dev/null
+++ b/include/hpp/manipulation/path-planner/end-effector-trajectory.hh
@@ -0,0 +1,137 @@
+// Copyright (c) 2019 CNRS
+// Authors: Joseph Mirabel
+//
+// This file is part of hpp-manipulation
+// hpp-manipulation is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// hpp-manipulation is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// hpp-manipulation If not, see
+// <http://www.gnu.org/licenses/>.
+
+
+#ifndef HPP_MANIPULATION_PATH_PLANNER_END_EFFECTOR_TRAJECTORY_HH
+# define HPP_MANIPULATION_PATH_PLANNER_END_EFFECTOR_TRAJECTORY_HH
+
+# include <hpp/manipulation/config.hh>
+# include <hpp/manipulation/fwd.hh>
+
+# include <pinocchio/spatial/se3.hpp>
+
+# include <hpp/pinocchio/frame.hh>
+# include <hpp/core/path-planner.hh>
+
+namespace hpp {
+ namespace manipulation {
+ namespace pathPlanner {
+ class HPP_MANIPULATION_DLLAPI IkSolverInitialization
+ {
+ public:
+ typedef std::vector<Configuration_t> Configurations_t;
+
+ Configurations_t solve (vectorIn_t target)
+ {
+ return impl_solve (target);
+ }
+
+ protected:
+ virtual Configurations_t impl_solve (vectorIn_t target) = 0;
+ };
+ typedef boost::shared_ptr<IkSolverInitialization> IkSolverInitializationPtr_t;
+
+ HPP_PREDEF_CLASS (EndEffectorTrajectory);
+ typedef boost::shared_ptr<EndEffectorTrajectory> EndEffectorTrajectoryPtr_t;
+
+ class HPP_MANIPULATION_DLLAPI EndEffectorTrajectory : public core::PathPlanner
+ {
+ public:
+ /// Return shared pointer to new instance
+ /// \param problem the path planning problem
+ static EndEffectorTrajectoryPtr_t create (const core::Problem& problem);
+ /// Return shared pointer to new instance
+ /// \param problem the path planning problem
+ /// \param roadmap previously built roadmap
+ static EndEffectorTrajectoryPtr_t createWithRoadmap
+ (const core::Problem& problem, const core::RoadmapPtr_t& roadmap);
+
+ /// Initialize the problem resolution
+ /// \li call parent implementation
+ /// \li get number nodes in problem parameter map
+ virtual void startSolve ();
+
+ /// One step of the algorithm
+ virtual void oneStep ();
+
+ /// Get the number of random configurations shoot (after using init
+ /// config) in order to generate the initial config of the final path.
+ int nRandomConfig () const
+ { return nRandomConfig_; }
+
+ void nRandomConfig (int n)
+ {
+ assert (n >= 0);
+ nRandomConfig_ = n;
+ }
+
+ /// Number of steps to generate goal config (successive projections).
+ int nDiscreteSteps () const
+ { return nDiscreteSteps_; }
+
+ void nDiscreteSteps (int n)
+ {
+ assert (n > 0);
+ nDiscreteSteps_ = n;
+ }
+
+ /// If enabled, only add one solution to the roadmap.
+ /// Otherwise add all solution.
+ void checkFeasibilityOnly (bool enable);
+
+ bool checkFeasibilityOnly () const
+ {
+ return feasibilityOnly_;
+ }
+
+ void ikSolverInitialization (IkSolverInitializationPtr_t solver)
+ {
+ ikSolverInit_ = solver;
+ }
+
+ void tryConnectInitAndGoals ();
+
+ protected:
+ /// Protected constructor
+ /// \param problem the path planning problem
+ EndEffectorTrajectory (const core::Problem& problem);
+ /// Protected constructor
+ /// \param problem the path planning problem
+ /// \param roadmap previously built roadmap
+ EndEffectorTrajectory (const core::Problem& problem, const core::RoadmapPtr_t& roadmap);
+ /// Store weak pointer to itself
+ void init (const EndEffectorTrajectoryWkPtr_t& weak);
+
+ private:
+ std::vector<core::Configuration_t> configurations(const core::Configuration_t& q_init);
+
+ /// Weak pointer to itself
+ EndEffectorTrajectoryWkPtr_t weak_;
+ /// Number of random config.
+ int nRandomConfig_;
+ /// Number of steps to generate goal config.
+ int nDiscreteSteps_;
+ /// Ik solver initialization. An external Ik solver can be plugged here.
+ IkSolverInitializationPtr_t ikSolverInit_;
+ /// Feasibility
+ bool feasibilityOnly_;
+ }; // class EndEffectorTrajectory
+ } // namespace pathPlanner
+ } // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_PATH_PLANNER_END_EFFECTOR_TRAJECTORY_HH
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 0ae2c0ecb6b5ff76d5a8902ebd461fa6a955c59d..19c8702c7a5ffe6f9b887ceb0f7d50b8ce02a0ab 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -49,6 +49,8 @@ SET(SOURCES
path-optimization/random-shortcut.cc
path-optimization/enforce-transition-semantic.cc
+ path-planner/end-effector-trajectory.cc
+
problem-target/state.cc
steering-method/end-effector-trajectory.cc
diff --git a/src/path-planner/end-effector-trajectory.cc b/src/path-planner/end-effector-trajectory.cc
new file mode 100644
index 0000000000000000000000000000000000000000..a2a5a8f122443bc8269ed2c0d2e310d2be3d3b22
--- /dev/null
+++ b/src/path-planner/end-effector-trajectory.cc
@@ -0,0 +1,248 @@
+// Copyright (c) 2019 CNRS
+// Authors: Joseph Mirabel
+//
+// This file is part of hpp-manipulation
+// hpp-manipulation is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// hpp-manipulation is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// hpp-manipulation If not, see
+// <http://www.gnu.org/licenses/>.
+
+# include <hpp/manipulation/path-planner/end-effector-trajectory.hh>
+
+# include <pinocchio/multibody/data.hpp>
+
+# include <hpp/util/exception-factory.hh>
+# include <hpp/pinocchio/util.hh>
+# include <hpp/pinocchio/device-sync.hh>
+# include <hpp/pinocchio/liegroup-element.hh>
+
+# include <hpp/constraints/differentiable-function.hh>
+# include <hpp/constraints/implicit.hh>
+
+# include <hpp/core/config-projector.hh>
+# include <hpp/core/config-validations.hh>
+# include <hpp/core/configuration-shooter.hh>
+# include <hpp/core/path-validation.hh>
+# include <hpp/core/problem.hh>
+# include <hpp/core/roadmap.hh>
+# include <hpp/manipulation/steering-method/end-effector-trajectory.hh>
+
+namespace hpp {
+ namespace manipulation {
+ namespace pathPlanner {
+ typedef manipulation::steeringMethod::EndEffectorTrajectory SM_t;
+ typedef manipulation::steeringMethod::EndEffectorTrajectoryPtr_t SMPtr_t;
+
+ EndEffectorTrajectoryPtr_t EndEffectorTrajectory::create (const core::Problem& problem)
+ {
+ EndEffectorTrajectoryPtr_t ptr (new EndEffectorTrajectory(problem));
+ ptr->init(ptr);
+ return ptr;
+ }
+
+ EndEffectorTrajectoryPtr_t EndEffectorTrajectory::createWithRoadmap (
+ const core::Problem& problem, const core::RoadmapPtr_t& roadmap)
+ {
+ EndEffectorTrajectoryPtr_t ptr (new EndEffectorTrajectory(problem, roadmap));
+ ptr->init(ptr);
+ return ptr;
+ }
+
+ void EndEffectorTrajectory::tryConnectInitAndGoals ()
+ {}
+
+ void EndEffectorTrajectory::startSolve ()
+ {
+ //core::PathPlanner::startSolve();
+ //problem().checkProblem ();
+ if (!problem().robot ()) {
+ std::string msg ("No device in problem.");
+ hppDout (error, msg);
+ throw std::runtime_error (msg);
+ }
+
+ if (!problem().initConfig ()) {
+ std::string msg ("No init config in problem.");
+ hppDout (error, msg);
+ throw std::runtime_error (msg);
+ }
+
+ // Tag init and goal configurations in the roadmap
+ roadmap()->resetGoalNodes ();
+
+ SMPtr_t sm (HPP_DYNAMIC_PTR_CAST (SM_t, problem().steeringMethod()));
+ if (!sm)
+ throw std::invalid_argument ("Steering method must be of type hpp::manipulation::steeringMethod::EndEffectorTrajectory");
+
+ if (!sm->constraints() || !sm->constraints()->configProjector())
+ throw std::invalid_argument ("Steering method constraint has no ConfigProjector.");
+ core::ConfigProjectorPtr_t constraints (sm->constraints()->configProjector());
+
+ const constraints::ImplicitPtr_t& trajConstraint = sm->trajectoryConstraint ();
+ if (!trajConstraint)
+ throw std::invalid_argument ("EndEffectorTrajectory has no trajectory constraint.");
+ if (!sm->trajectory())
+ throw std::invalid_argument ("EndEffectorTrajectory has no trajectory.");
+
+ const core::NumericalConstraints_t& ncs = constraints->numericalConstraints();
+ bool ok = false;
+ for (std::size_t i = 0; i < ncs.size(); ++i) {
+ if (ncs[i] == trajConstraint) {
+ ok = true;
+ break; // Same pointer
+ }
+ // Here, we do not check the right hand side on purpose.
+ // if (*ncs[i] == *trajConstraint) {
+ if (ncs[i]->functionPtr() == trajConstraint->functionPtr()
+ && ncs[i]->comparisonType() == trajConstraint->comparisonType()) {
+ ok = true;
+ // TODO We should only modify the path constraint.
+ // However, only the pointers to implicit constraints are copied
+ // while we would like the implicit constraints to be copied as well.
+ ncs[i]->rightHandSideFunction (sm->trajectory());
+ break; // logically identical
+ }
+ }
+ if (!ok) {
+ HPP_THROW (std::logic_error, "EndEffectorTrajectory could not find "
+ "constraint " << trajConstraint->function());
+ }
+ }
+
+ void EndEffectorTrajectory::oneStep ()
+ {
+ SMPtr_t sm (HPP_DYNAMIC_PTR_CAST (SM_t, problem().steeringMethod()));
+ if (!sm)
+ throw std::invalid_argument ("Steering method must be of type hpp::manipulation::steeringMethod::EndEffectorTrajectory");
+ if (!sm->trajectoryConstraint ())
+ throw std::invalid_argument ("EndEffectorTrajectory has no trajectory constraint.");
+ if (!sm->trajectory())
+ throw std::invalid_argument ("EndEffectorTrajectory has no trajectory.");
+
+ if (!sm->constraints() || !sm->constraints()->configProjector())
+ throw std::invalid_argument ("Steering method constraint has no ConfigProjector.");
+ core::ConfigProjectorPtr_t constraints (sm->constraints()->configProjector());
+
+ core::ConfigValidationPtr_t cfgValidation (problem().configValidations());
+ core:: PathValidationPtr_t pathValidation (problem().pathValidation());
+ core:: ValidationReportPtr_t cfgReport;
+ core::PathValidationReportPtr_t pathReport;
+
+ core::interval_t timeRange (sm->timeRange());
+
+ std::vector<core::Configuration_t> qs (configurations(*problem().initConfig()));
+ if (qs.empty()) {
+ hppDout (info, "Failed to generate initial configs.");
+ return;
+ }
+
+ // Generate a valid initial configuration.
+ bool success = false;
+ bool resetRightHandSide = true;
+ std::size_t i;
+ Configuration_t q1;
+ for (i = 0; i < qs.size(); ++i)
+ {
+ if (resetRightHandSide) {
+ constraints->rightHandSideAt (timeRange.first);
+ resetRightHandSide = false;
+ }
+ Configuration_t& q (qs[i]);
+ if (!constraints->apply (q)) continue;
+ if (!cfgValidation->validate (q, cfgReport)) continue;
+ resetRightHandSide = true;
+
+ q1 = q;
+ success = true;
+ for (int j = 1; j <= nDiscreteSteps_; ++j) {
+ value_type t = timeRange.first + j * (timeRange.second - timeRange.first) / nDiscreteSteps_;
+ constraints->rightHandSideAt (t);
+ hppDout (info, "RHS: " << setpyformat << constraints->rightHandSide().transpose());
+ if (!constraints->apply (q)) {
+ hppDout (info, "Failed to generate destination config.\n" << setpyformat
+ << *constraints
+ << "\nq=" << one_line (q));
+ success = false;
+ break;
+ }
+ }
+ if (!success) continue;
+ success = false;
+
+ if (!cfgValidation->validate (q, cfgReport)) {
+ hppDout (info, "Destination config is in collision.");
+ continue;
+ }
+
+ core::PathPtr_t path = (*sm) (q1, q);
+ if (!path) {
+ hppDout (info, "Steering method failed.\n" << setpyformat
+ << one_line(q1) << '\n'
+ << one_line(q ) << '\n'
+ );
+ continue;
+ }
+
+ core::PathPtr_t validPart;
+ if (!pathValidation->validate (path, false, validPart, pathReport)) {
+ hppDout (info, "Path is in collision.");
+ continue;
+ }
+
+ roadmap()->initNode (boost::make_shared<Configuration_t>(q1));
+ core::NodePtr_t init = roadmap()-> initNode ();
+ core::NodePtr_t goal = roadmap()->addGoalNode (boost::make_shared<Configuration_t>(q ));
+ roadmap()->addEdge (init, goal, path);
+ success = true;
+ if (feasibilityOnly_) break;
+ }
+ }
+
+ std::vector<core::Configuration_t> EndEffectorTrajectory::configurations(const core::Configuration_t& q_init)
+ {
+ if (!ikSolverInit_) {
+ std::vector<core::Configuration_t> configs(nRandomConfig_ + 1);
+ configs[0] = q_init;
+ for (int i = 1; i < nRandomConfig_ + 1; ++i)
+ problem().configurationShooter()->shoot(configs[i]);
+ return configs;
+ }
+
+ // TODO Compute the target and call ikSolverInit_
+ // See https://gepgitlab.laas.fr/airbus-xtct/hpp_airbus_xtct for an
+ // example using IKFast.
+ throw std::runtime_error ("Using an IkSolverInitialization is not implemented yet");
+ }
+
+ EndEffectorTrajectory::EndEffectorTrajectory (const core::Problem& problem)
+ : core::PathPlanner (problem)
+ {}
+
+ EndEffectorTrajectory::EndEffectorTrajectory (const core::Problem& problem, const core::RoadmapPtr_t& roadmap)
+ : core::PathPlanner (problem, roadmap)
+ {}
+
+ void EndEffectorTrajectory::checkFeasibilityOnly (bool enable)
+ {
+ feasibilityOnly_ = enable;
+ }
+
+ void EndEffectorTrajectory::init (const EndEffectorTrajectoryWkPtr_t& weak)
+ {
+ core::PathPlanner::init (weak);
+ weak_ = weak;
+ nRandomConfig_ = 10;
+ nDiscreteSteps_ = 1;
+ feasibilityOnly_ = true;
+ }
+ } // namespace pathPlanner
+ } // namespace manipulation
+} // namespace hpp
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index a75cd757a8eb622b65eb626ab7fcdf6c5ef6e948..92eede11847f4842373c0a733e8c1eac6fa38b7f 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -57,6 +57,7 @@
#include "hpp/manipulation/graph-node-optimizer.hh"
#include "hpp/manipulation/path-optimization/random-shortcut.hh"
#include "hpp/manipulation/path-optimization/enforce-transition-semantic.hh"
+#include "hpp/manipulation/path-planner/end-effector-trajectory.hh"
#include "hpp/manipulation/problem-target/state.hh"
#include "hpp/manipulation/steering-method/cross-state-optimization.hh"
#include "hpp/manipulation/steering-method/graph.hh"
@@ -121,6 +122,7 @@ namespace hpp {
robotType ("hpp::manipulation::Device");
pathPlanners.add ("M-RRT", ManipulationPlanner::create);
+ pathPlanners.add ("EndEffectorTrajectory", pathPlanner::EndEffectorTrajectory::createWithRoadmap);
pathValidations.add ("Graph-Discretized" , createDiscretizedCollisionGraphPathValidation);
pathValidations.add ("Graph-DiscretizedCollision" , createDiscretizedCollisionGraphPathValidation);