Add pathPlanner::EndEffectorTrajectory

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

include/hpp/manipulation/path-planner/end-effector-trajectory.hh

+// 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

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

src/path-planner/end-effector-trajectory.cc

+// 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

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);