diff --git a/CMakeLists.txt b/CMakeLists.txt index edfe0750adfd6516e1115effbb2f90ba1cbb2563..2504d9e9654119a3a35c0109a3afbd5dbc952a58 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -85,7 +85,6 @@ SET (${PROJECT_NAME}_HEADERS include/hpp/manipulation/symbolic-planner.hh include/hpp/manipulation/manipulation-planner.hh include/hpp/manipulation/graph-path-validation.hh - include/hpp/manipulation/graph-steering-method.hh include/hpp/manipulation/graph-optimizer.hh include/hpp/manipulation/graph/node.hh include/hpp/manipulation/graph/state.hh @@ -110,6 +109,7 @@ SET (${PROJECT_NAME}_HEADERS include/hpp/manipulation/steering-method/cross-state-optimization.hh include/hpp/manipulation/steering-method/fwd.hh + include/hpp/manipulation/steering-method/graph.hh ) ADD_SUBDIRECTORY(src) diff --git a/doc/Doxyfile.extra.in b/doc/Doxyfile.extra.in index 7a322b67fac91eea6d4d175f147a6e33ae41f0d3..f36a30b8d16c3f7bfd34667bc6b07ef39cbac903 100644 --- a/doc/Doxyfile.extra.in +++ b/doc/Doxyfile.extra.in @@ -1,5 +1,6 @@ INPUT = @CMAKE_SOURCE_DIR@/doc \ @CMAKE_SOURCE_DIR@/include \ + @CMAKE_SOURCE_DIR@/src/steering-method/cross-state-optimization/function.cc \ @CMAKE_BINARY_DIR@/doc HTML_EXTRA_FILES = @CMAKE_SOURCE_DIR@/doc/ObjectManipulation_MasterThesis_JosephMirabel.pdf diff --git a/include/hpp/manipulation/device.hh b/include/hpp/manipulation/device.hh index a2cd02a19b867888a00994a075c9d87f8f6067ab..8834f25b2c923da2d6823e6cbd862d2baf4c869c 100644 --- a/include/hpp/manipulation/device.hh +++ b/include/hpp/manipulation/device.hh @@ -35,23 +35,11 @@ namespace hpp { /// it is compatible with hpp::pinocchio::urdf::loadHumanoidRobot /// /// This class also contains pinocchio::Gripper, Handle and \ref JointAndShapes_t - class HPP_MANIPULATION_DLLAPI Device : - public pinocchio::HumanoidRobot, - public core::Containers< - boost::mpl::vector < HandlePtr_t, - GripperPtr_t, - JointAndShapes_t, - FrameIndices_t> > + class HPP_MANIPULATION_DLLAPI Device : public pinocchio::HumanoidRobot { public: typedef pinocchio::HumanoidRobot Parent_t; - typedef core::Containers< - boost::mpl::vector < HandlePtr_t, - pinocchio::GripperPtr_t, - JointAndShapes_t, - FrameIndices_t> > Containers_t; - /// Constructor /// \param name of the new instance, static DevicePtr_t create (const std::string& name) @@ -82,6 +70,11 @@ namespace hpp { /// \} + core::Container <HandlePtr_t> handles; + core::Container <GripperPtr_t> grippers; + core::Container <JointAndShapes_t> jointAndShapes; + core::Container <FrameIndices_t> frameIndices; + protected: /// Constructor /// \param name of the new instance, diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh index 360ce8e9ae01d5d5e96810237cf04e6f59e9fcd1..16cb9a31c44c3f8532cfa8e03fa2367a98e1dd44 100644 --- a/include/hpp/manipulation/fwd.hh +++ b/include/hpp/manipulation/fwd.hh @@ -87,9 +87,7 @@ namespace hpp { HPP_PREDEF_CLASS (GraphPathValidation); typedef boost::shared_ptr < GraphPathValidation > GraphPathValidationPtr_t; HPP_PREDEF_CLASS (SteeringMethod); - HPP_PREDEF_CLASS (GraphSteeringMethod); typedef boost::shared_ptr < SteeringMethod > SteeringMethodPtr_t; - typedef boost::shared_ptr < GraphSteeringMethod > GraphSteeringMethodPtr_t; typedef core::PathOptimizer PathOptimizer; typedef core::PathOptimizerPtr_t PathOptimizerPtr_t; HPP_PREDEF_CLASS (GraphOptimizer); diff --git a/include/hpp/manipulation/problem.hh b/include/hpp/manipulation/problem.hh index 34bc9e48476fa46b12655512255de19d889ad403..59d76e409a96e0f05bc27490dc8e5d29ef4ea64b 100644 --- a/include/hpp/manipulation/problem.hh +++ b/include/hpp/manipulation/problem.hh @@ -55,7 +55,7 @@ namespace hpp { if (!pathValidation ()) throw std::runtime_error ("No GraphPathValidation in the problem."); if (!steeringMethod ()) - throw std::runtime_error ("No GraphSteeringMethod in the problem."); + throw std::runtime_error ("No SteeringMethod in the problem."); } /// Get the path validation as a GraphPathValidation @@ -63,7 +63,7 @@ namespace hpp { void pathValidation (const PathValidationPtr_t& pathValidation); - /// Get the steering method as a GraphSteeringMethod + /// Get the steering method as a SteeringMethod SteeringMethodPtr_t steeringMethod () const; /// Build a new path validation diff --git a/include/hpp/manipulation/steering-method/cross-state-optimization.hh b/include/hpp/manipulation/steering-method/cross-state-optimization.hh index 14293f6128a0730d85c4a18fe4381c709a94aa81..8b555508fab9dcdbaa1025881e7a490043fd38b3 100644 --- a/include/hpp/manipulation/steering-method/cross-state-optimization.hh +++ b/include/hpp/manipulation/steering-method/cross-state-optimization.hh @@ -23,11 +23,36 @@ # include <hpp/manipulation/fwd.hh> # include <hpp/manipulation/problem.hh> # include <hpp/manipulation/steering-method/fwd.hh> -# include <hpp/manipulation/graph-steering-method.hh> +# include <hpp/manipulation/steering-method/graph.hh> namespace hpp { namespace manipulation { namespace steeringMethod { + /// \addtogroup steering_method + /// \{ + + /// Optimization-based steering method. + /// + /// #### Methodology + /// + /// Given two configuration \f$ (q_1,q_2) \f$, this class formulates and + /// solves the problem as follows. + /// - Compute the corresponding states \f$ (s_1, s_2) \f$. + /// - For a each path \f$ (e_0, ... e_n) \f$ between \f$ (s_1, s_2) \f$ + /// in the constraint graph, do: + /// - define \f$ n-1 \f$ intermediate configuration \f$ p_i \f$, + /// - initialize the optimization problem, as explained below, + /// - solve the optimization problem, which gives \f$ p^*_i \f$, + /// - in case of failure, continue the loop. + /// - call the Edge::build of each \f$ e_j \f$ for each consecutive + /// \f$ (p^*_i, p^*_{i+1}) \f$. + /// + /// #### Problem formulation + /// Find \f$ (p_i) \f$ such that: + /// - \f$ p_0 = q_1 \f$, + /// - \f$ p_{n+1} = q_2 \f$, + /// - \f$ p_i \f$ is in state between \f$ (e_{i-1}, e_i) \f$, (\ref StateFunction) + /// - \f$ (p_i, p_{i+1}) \f$ are reachable with transition \f$ e_i \f$ (\ref EdgeFunction). class HPP_MANIPULATION_DLLAPI CrossStateOptimization : public SteeringMethod { @@ -35,7 +60,11 @@ namespace hpp { static CrossStateOptimizationPtr_t create (const Problem& problem); /// \warning core::Problem will be casted to Problem - static CrossStateOptimizationPtr_t createFromCore + static CrossStateOptimizationPtr_t create + (const core::Problem& problem); + + template <typename T> + static CrossStateOptimizationPtr_t create (const core::Problem& problem); core::SteeringMethodPtr_t copy () const; @@ -77,6 +106,16 @@ namespace hpp { /// Weak pointer to itself CrossStateOptimizationWkPtr_t weak_; }; // class CrossStateOptimization + /// \} + + template <typename T> + CrossStateOptimizationPtr_t CrossStateOptimization::create + (const core::Problem& problem) + { + CrossStateOptimizationPtr_t gsm = CrossStateOptimization::create (problem); + gsm->innerSteeringMethod (T::create (problem)); + return gsm; + } } // namespace steeringMethod } // namespace manipulation } // namespace hpp diff --git a/include/hpp/manipulation/steering-method/fwd.hh b/include/hpp/manipulation/steering-method/fwd.hh index 3b245f6bf4d74a71de57110f81f8078aaba51dd2..467557d9d37ee8684783ecdeab090c12ee5b19bc 100644 --- a/include/hpp/manipulation/steering-method/fwd.hh +++ b/include/hpp/manipulation/steering-method/fwd.hh @@ -26,6 +26,8 @@ namespace hpp { namespace manipulation { namespace steeringMethod { + HPP_PREDEF_CLASS (Graph); + typedef boost::shared_ptr < Graph > GraphPtr_t; HPP_PREDEF_CLASS (CrossStateOptimization); typedef boost::shared_ptr < CrossStateOptimization > CrossStateOptimizationPtr_t; } // namespace steeringMethod diff --git a/include/hpp/manipulation/graph-steering-method.hh b/include/hpp/manipulation/steering-method/graph.hh similarity index 51% rename from include/hpp/manipulation/graph-steering-method.hh rename to include/hpp/manipulation/steering-method/graph.hh index d21d7a46184d737c5c77a5ede6e303c35754a6c7..b3145b7109500354416b1fab673554c6e0b9707b 100644 --- a/include/hpp/manipulation/graph-steering-method.hh +++ b/include/hpp/manipulation/steering-method/graph.hh @@ -15,8 +15,8 @@ // hpp-manipulation. If not, see <http://www.gnu.org/licenses/>. -#ifndef HPP_MANIPULATION_GRAPH_STEERING_METHOD_HH -# define HPP_MANIPULATION_GRAPH_STEERING_METHOD_HH +#ifndef HPP_MANIPULATION_STEERING_METHOD_GRAPH_HH +# define HPP_MANIPULATION_STEERING_METHOD_GRAPH_HH # include <hpp/core/problem-solver.hh> // SteeringMethodBuilder_t # include <hpp/core/steering-method.hh> @@ -24,13 +24,12 @@ # include <hpp/manipulation/config.hh> # include <hpp/manipulation/fwd.hh> # include <hpp/manipulation/graph/fwd.hh> +# include <hpp/manipulation/steering-method/fwd.hh> namespace hpp { namespace manipulation { - using core::PathPtr_t; /// \addtogroup steering_method /// \{ - class HPP_MANIPULATION_DLLAPI SteeringMethod : public core::SteeringMethod { public: @@ -62,63 +61,67 @@ namespace hpp { core::SteeringMethodPtr_t steeringMethod_; }; - class HPP_MANIPULATION_DLLAPI GraphSteeringMethod : public SteeringMethod - { - typedef core::SteeringMethodBuilder_t SteeringMethodBuilder_t; - - public: - /// Create instance and return shared pointer - /// \warning core::Problem will be casted to Problem - static GraphSteeringMethodPtr_t create - (const core::Problem& problem); - - template <typename T> - static GraphSteeringMethodPtr_t create - (const core::Problem& problem); - - /// Create instance and return shared pointer - static GraphSteeringMethodPtr_t create (const Problem& problem); - - /// Create copy and return shared pointer - static GraphSteeringMethodPtr_t createCopy - (const GraphSteeringMethodPtr_t& other); - - /// Copy instance and return shared pointer - virtual core::SteeringMethodPtr_t copy () const - { - return createCopy (weak_.lock ()); - } - - protected: - /// Constructor - GraphSteeringMethod (const Problem& problem); - - /// Copy constructor - GraphSteeringMethod (const GraphSteeringMethod&); - - virtual PathPtr_t impl_compute (ConfigurationIn_t q1, ConfigurationIn_t q2) const; - - void init (GraphSteeringMethodWkPtr_t weak) - { - SteeringMethod::init (weak); - weak_ = weak; - } - - private: - /// Weak pointer to itself - GraphSteeringMethodWkPtr_t weak_; - }; - - template <typename T> - GraphSteeringMethodPtr_t GraphSteeringMethod::create - (const core::Problem& problem) - { - GraphSteeringMethodPtr_t gsm = GraphSteeringMethod::create (problem); - gsm->innerSteeringMethod (T::create (problem)); - return gsm; - } + namespace steeringMethod { + using core::PathPtr_t; + + class HPP_MANIPULATION_DLLAPI Graph : public SteeringMethod + { + typedef core::SteeringMethodBuilder_t SteeringMethodBuilder_t; + + public: + /// Create instance and return shared pointer + /// \warning core::Problem will be casted to Problem + static GraphPtr_t create + (const core::Problem& problem); + + template <typename T> + static GraphPtr_t create + (const core::Problem& problem); + + /// Create instance and return shared pointer + static GraphPtr_t create (const Problem& problem); + + /// Create copy and return shared pointer + static GraphPtr_t createCopy + (const GraphPtr_t& other); + + /// Copy instance and return shared pointer + virtual core::SteeringMethodPtr_t copy () const + { + return createCopy (weak_.lock ()); + } + + protected: + /// Constructor + Graph (const Problem& problem); + + /// Copy constructor + Graph (const Graph&); + + virtual PathPtr_t impl_compute (ConfigurationIn_t q1, ConfigurationIn_t q2) const; + + void init (GraphWkPtr_t weak) + { + SteeringMethod::init (weak); + weak_ = weak; + } + + private: + /// Weak pointer to itself + GraphWkPtr_t weak_; + }; + + template <typename T> + GraphPtr_t Graph::create + (const core::Problem& problem) + { + GraphPtr_t gsm = Graph::create (problem); + gsm->innerSteeringMethod (T::create (problem)); + return gsm; + } + } // namespace steeringMethod /// \} } // namespace manipulation } // namespace hpp -#endif // HPP_MANIPULATION_GRAPH_STEERING_METHOD_HH +#endif // HPP_MANIPULATION_STEERING_METHOD_GRAPH_HH diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 66d6b9eed3168cbd25eb1d8faac2a71c5e1eece2..ecb8644bfc75c921001c9f865d9beb7ffdcc2381 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -34,7 +34,6 @@ SET(SOURCES weighed-distance.cc problem.cc graph-path-validation.cc - graph-steering-method.cc graph-optimizer.cc graph/state.cc @@ -55,6 +54,7 @@ SET(SOURCES problem-target/state.cc steering-method/cross-state-optimization.cc + steering-method/graph.cc ) IF(HPP_WHOLEBODY_STEP_FOUND) diff --git a/src/device.cc b/src/device.cc index b641f991bae504f0dd36f052f3537e3a12ebeb0a..d4f01097a99d17ae8b4814dca13fe3f719d6e73d 100644 --- a/src/device.cc +++ b/src/device.cc @@ -43,7 +43,7 @@ namespace hpp { void Device::setRobotRootPosition (const std::string& rn, const Transform3f& t) { - FrameIndices_t idxs = get<JointIndices_t> (rn); + const FrameIndices_t& idxs = frameIndices.get (rn); pinocchio::Model& m = model(); pinocchio::GeomModel& gm = geomModel(); // The root frame should be the first frame. @@ -93,11 +93,11 @@ namespace hpp { } frameCacheSize_ = model().frames.size(); - if (has<FrameIndices_t>(name)) { - const FrameIndices_t& old = get<FrameIndices_t>(name); + if (frameIndices.has(name)) { + const FrameIndices_t& old = frameIndices.get(name); newF.insert(newF.begin(), old.begin(), old.end()); } - add (name, newF); + frameIndices.add (name, newF); createData(); createGeomData(); } @@ -107,10 +107,10 @@ namespace hpp { Parent_t::print (os); // print handles os << "Handles:" << std::endl; - Containers_t::print <HandlePtr_t> (os); + handles.print (os); // print grippers os << "Grippers:" << std::endl; - Containers_t::print <GripperPtr_t> (os); + grippers.print (os); return os; } } // namespace manipulation diff --git a/src/graph-steering-method.cc b/src/graph-steering-method.cc deleted file mode 100644 index 446388814d27f4f33ca2ac072b2dd4d8cef5c01e..0000000000000000000000000000000000000000 --- a/src/graph-steering-method.cc +++ /dev/null @@ -1,102 +0,0 @@ -// Copyright (c) 2014, LAAS-CNRS -// Authors: Joseph Mirabel (joseph.mirabel@laas.fr) -// -// 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/graph-steering-method.hh" - -#include <hpp/util/pointer.hh> - -#include <hpp/core/straight-path.hh> -#include <hpp/core/steering-method/straight.hh> - -#include <hpp/manipulation/problem.hh> -#include <hpp/manipulation/graph/graph.hh> -#include <hpp/manipulation/graph/edge.hh> - -namespace hpp { - namespace manipulation { - SteeringMethod::SteeringMethod (const Problem& problem) : - core::SteeringMethod (problem), problem_ (problem), - steeringMethod_ (core::SteeringMethodStraight::create (problem)) - { - } - - SteeringMethod::SteeringMethod (const SteeringMethod& other): - core::SteeringMethod (other), problem_ (other.problem_), steeringMethod_ - (other.steeringMethod_) - { - } - - GraphSteeringMethodPtr_t GraphSteeringMethod::create - (const core::Problem& problem) - { - HPP_STATIC_CAST_REF_CHECK (const Problem, problem); - const Problem& p = static_cast <const Problem&> (problem); - return create (p); - } - - GraphSteeringMethodPtr_t GraphSteeringMethod::create - (const Problem& problem) - { - GraphSteeringMethod* ptr = new GraphSteeringMethod (problem); - GraphSteeringMethodPtr_t shPtr (ptr); - ptr->init (shPtr); - return shPtr; - } - - GraphSteeringMethodPtr_t GraphSteeringMethod::createCopy - (const GraphSteeringMethodPtr_t& other) - { - GraphSteeringMethod* ptr = new GraphSteeringMethod (*other); - GraphSteeringMethodPtr_t shPtr (ptr); - ptr->init (shPtr); - return shPtr; - } - - GraphSteeringMethod::GraphSteeringMethod (const Problem& problem) : - SteeringMethod (problem), weak_ () - { - } - - GraphSteeringMethod::GraphSteeringMethod (const GraphSteeringMethod& other): - SteeringMethod (other) - { - } - - PathPtr_t GraphSteeringMethod::impl_compute (ConfigurationIn_t q1, ConfigurationIn_t q2) const - { - graph::Edges_t possibleEdges; - const graph::Graph& graph = *problem_.constraintGraph (); - try { - possibleEdges = graph.getEdges - (graph.getState (q1), graph.getState (q2)); - } catch (const std::logic_error& e) { - hppDout (error, e.what ()); - return PathPtr_t (); - } - PathPtr_t path; - if (possibleEdges.empty()) { - hppDout (info, "No edge found."); - } - while (!possibleEdges.empty()) { - if (possibleEdges.back ()->build (path, q1, q2)) { - return path; - } - possibleEdges.pop_back (); - } - return PathPtr_t (); - } - } // namespace manipulation -} // namespace hpp diff --git a/src/graph/edge.cc b/src/graph/edge.cc index 5c30efacb34c22d02160553f75547432a454bd73..537fd7e80259f17aff19bd50f258e40823be8c65 100644 --- a/src/graph/edge.cc +++ b/src/graph/edge.cc @@ -29,7 +29,7 @@ #include "hpp/manipulation/device.hh" #include "hpp/manipulation/problem.hh" -#include "hpp/manipulation/graph-steering-method.hh" +#include "hpp/manipulation/steering-method/graph.hh" #include "hpp/manipulation/graph/statistics.hh" #include "hpp/manipulation/constraint-set.hh" diff --git a/src/graph/helper.cc b/src/graph/helper.cc index a1ff2a520a16dbb2dabc6238ee2e05eab4191a12..224c71d802ecf9ad3f0f0f231f6a2c181e56ef24 100644 --- a/src/graph/helper.cc +++ b/src/graph/helper.cc @@ -663,8 +663,6 @@ namespace hpp { inline boost::array<NumericalConstraintPtr_t,3>& graspConstraint ( const index_t& iG, const index_t& iOH) { - typedef core::ProblemSolver CPS_t; - boost::array<NumericalConstraintPtr_t,3>& gcs = graspCs [iG * nOH + iOH]; if (!gcs[0]) { @@ -673,16 +671,16 @@ namespace hpp { const GripperPtr_t& g (gs[iG]); const HandlePtr_t& h (handle (iOH)); const std::string& grasp = g->name() + " grasps " + h->name(); - if (!ps->CPS_t::has<NumericalConstraintPtr_t>(grasp)) { + if (!ps->numericalConstraints.has(grasp)) { ps->createGraspConstraint (grasp, g->name(), h->name()); } - gcs[0] = ps->CPS_t::get<NumericalConstraintPtr_t>(grasp); - gcs[1] = ps->CPS_t::get<NumericalConstraintPtr_t>(grasp + "/complement"); + gcs[0] = ps->numericalConstraints.get(grasp); + gcs[1] = ps->numericalConstraints.get(grasp + "/complement"); const std::string& pregrasp = g->name() + " pregrasps " + h->name(); - if (!ps->CPS_t::has<NumericalConstraintPtr_t>(pregrasp)) { + if (!ps->numericalConstraints.has(pregrasp)) { ps->createPreGraspConstraint (pregrasp, g->name(), h->name()); } - gcs[2] = ps->CPS_t::get<NumericalConstraintPtr_t>(pregrasp); + gcs[2] = ps->numericalConstraints.get(pregrasp); } return gcs; } @@ -1050,7 +1048,7 @@ namespace hpp { Grippers_t grippers (griNames.size()); index_t i = 0; BOOST_FOREACH (const std::string& gn, griNames) { - grippers[i] = robot.get <GripperPtr_t> (gn); + grippers[i] = robot.grippers.get (gn); ++i; } Objects_t objects (objs.size()); @@ -1063,7 +1061,7 @@ namespace hpp { objects[i].get<1> ().resize (od.handles.size()); Handles_t::iterator it = objects[i].get<1> ().begin(); BOOST_FOREACH (const std::string hn, od.handles) { - *it = robot.get <HandlePtr_t> (hn); + *it = robot.handles.get (hn); ++it; } // Create placement @@ -1076,30 +1074,30 @@ namespace hpp { // else if contact surfaces are defined and selected // create default placement constraint using the ProblemSolver // methods createPlacementConstraint and createPrePlacementConstraint - if (ps->core::ProblemSolver::has<NumericalConstraintPtr_t>(placeN)) { + if (ps->numericalConstraints.has(placeN)) { objects[i].get<0> ().get<0> () = - ps->core::ProblemSolver::get <NumericalConstraintPtr_t> (placeN); - if (ps->core::ProblemSolver::has<NumericalConstraintPtr_t>(preplaceN)) { + ps->numericalConstraints.get (placeN); + if (ps->numericalConstraints.has(preplaceN)) { objects[i].get<0> ().get<1> () = - ps->core::ProblemSolver::get <NumericalConstraintPtr_t> (preplaceN); + ps->numericalConstraints.get (preplaceN); } } else if (!envNames.empty() && !od.shapes.empty ()) { ps->createPlacementConstraint (placeN, od.shapes, envNames, margin); objects[i].get<0> ().get<0> () = - ps->core::ProblemSolver::get <NumericalConstraintPtr_t> (placeN); + ps->numericalConstraints.get (placeN); if (prePlace) { ps->createPrePlacementConstraint (preplaceN, od.shapes, envNames, margin, prePlaceWidth); objects[i].get<0> ().get<1> () = - ps->core::ProblemSolver::get <NumericalConstraintPtr_t> (preplaceN); + ps->numericalConstraints.get (preplaceN); } } // Create object lock // Loop over all frames of object, detect joint and create locked // joint. - assert (robot.has <FrameIndices_t> (od.name)); - BOOST_FOREACH (const se3::FrameIndex& f, robot.get<FrameIndices_t> (od.name)) { + assert (robot.frameIndices.has (od.name)); + BOOST_FOREACH (const se3::FrameIndex& f, robot.frameIndices.get (od.name)) { if (model.frames[f].type != se3::JOINT) continue; const JointIndex j = model.frames[f].parent; JointPtr_t oj (new Joint (ps->robot(), j)); @@ -1108,7 +1106,7 @@ namespace hpp { .segment (oj->rankInConfiguration (), oj->configSize ()), space); LockedJointPtr_t lj = core::LockedJoint::create (oj, lge); - ps->add <LockedJointPtr_t> ("lock_" + oj->name (), lj); + ps->lockedJoints.add ("lock_" + oj->name (), lj); objects[i].get<0> ().get<2> ().push_back (lj); } ++i; diff --git a/src/path-optimization/keypoints.cc b/src/path-optimization/keypoints.cc index 384750c16389c4c7d43ea9d96e117a7bbefca9f2..fd802d0ef62e54127df5aded609f6685a245fdfe 100644 --- a/src/path-optimization/keypoints.cc +++ b/src/path-optimization/keypoints.cc @@ -24,7 +24,7 @@ #include <hpp/manipulation/graph/edge.hh> #include <hpp/manipulation/graph/graph.hh> #include <hpp/manipulation/constraint-set.hh> -#include <hpp/manipulation/graph-steering-method.hh> +#include <hpp/manipulation/steering-method/graph.hh> namespace hpp { namespace manipulation { diff --git a/src/problem-solver.cc b/src/problem-solver.cc index 543de2fe8068390290c06954db7a0cfae1a09a3f..6016b9f50e4e2db209e34505850c606418b3b048 100644 --- a/src/problem-solver.cc +++ b/src/problem-solver.cc @@ -33,7 +33,10 @@ #include <hpp/core/path-projector/global.hh> #include <hpp/core/path-projector/recursive-hermite.hh> #include <hpp/core/roadmap.hh> +#include <hpp/core/steering-method/dubins.hh> #include <hpp/core/steering-method/hermite.hh> +#include <hpp/core/steering-method/reeds-shepp.hh> +#include <hpp/core/steering-method/snibud.hh> #include <hpp/core/steering-method/straight.hh> #include "hpp/manipulation/package-config.hh" // HPP_MANIPULATION_HAS_WHOLEBODY_STEP @@ -49,12 +52,12 @@ #include "hpp/manipulation/graph-optimizer.hh" #include "hpp/manipulation/graph-path-validation.hh" #include "hpp/manipulation/graph-node-optimizer.hh" -#include "hpp/manipulation/graph-steering-method.hh" #include "hpp/manipulation/path-optimization/config-optimization.hh" #include "hpp/manipulation/path-optimization/keypoints.hh" #include "hpp/manipulation/path-optimization/spline-gradient-based.hh" #include "hpp/manipulation/problem-target/state.hh" #include "hpp/manipulation/steering-method/cross-state-optimization.hh" +#include "hpp/manipulation/steering-method/graph.hh" #if HPP_MANIPULATION_HAS_WHOLEBODY_STEP #include <hpp/wholebody-step/small-steps.hh> @@ -78,14 +81,23 @@ namespace hpp { } }; + template <typename ParentSM_t, typename ChildSM_t> + core::SteeringMethodPtr_t createSMWithGuess + (const core::Problem& problem) + { + boost::shared_ptr<ParentSM_t> sm = ParentSM_t::create (problem); + sm->innerSteeringMethod (ChildSM_t::createWithGuess (problem)); + return sm; + } + template <typename PathProjectorType> core::PathProjectorPtr_t createPathProjector (const core::Problem& problem, const value_type& step) { - GraphSteeringMethodPtr_t gsm = - HPP_DYNAMIC_PTR_CAST (GraphSteeringMethod, problem.steeringMethod()); + steeringMethod::GraphPtr_t gsm = + HPP_DYNAMIC_PTR_CAST (steeringMethod::Graph, problem.steeringMethod()); if (!gsm) throw std::logic_error ("The steering method should be of type" - " GraphSteeringMethod"); + " steeringMethod::Graph"); return PathProjectorType::create (problem.distance(), gsm->innerSteeringMethod(), step); } @@ -99,63 +111,70 @@ namespace hpp { ProblemSolver::ProblemSolver () : core::ProblemSolver (), robot_ (), problem_ (0x0) { - parent_t::add<core::RobotBuilder_t> ("hpp::manipulation::Device", - hpp::manipulation::Device::create); - parent_t::robotType ("hpp::manipulation::Device"); - parent_t::add<core::PathPlannerBuilder_t> - ("M-RRT", ManipulationPlanner::create); - parent_t::add<core::PathPlannerBuilder_t> - ("SymbolicPlanner", SymbolicPlanner::create); - using core::PathOptimizerBuilder_t; - parent_t::add <PathOptimizerBuilder_t> ("Graph-RandomShortcut", + robots.add ("hpp::manipulation::Device", manipulation::Device::create); + robotType ("hpp::manipulation::Device"); + + pathPlanners.add ("M-RRT", ManipulationPlanner::create); + pathPlanners.add ("SymbolicPlanner", SymbolicPlanner::create); + + pathOptimizers.add ("Graph-RandomShortcut", GraphOptimizer::create <core::RandomShortcut>); - parent_t::add <PathOptimizerBuilder_t> ("PartialShortcut", core::pathOptimization:: + pathOptimizers.add ("PartialShortcut", core::pathOptimization:: PartialShortcut::createWithTraits <PartialShortcutTraits>); - parent_t::add <PathOptimizerBuilder_t> ("Graph-PartialShortcut", + pathOptimizers.add ("Graph-PartialShortcut", GraphOptimizer::create <core::pathOptimization::PartialShortcut>); - parent_t::add <PathOptimizerBuilder_t> ("ConfigOptimization", + pathOptimizers.add ("ConfigOptimization", core::pathOptimization::ConfigOptimization::createWithTraits <pathOptimization::ConfigOptimizationTraits>); - parent_t::add <PathOptimizerBuilder_t> ("Graph-ConfigOptimization", + pathOptimizers.add ("Graph-ConfigOptimization", GraphOptimizer::create < GraphConfigOptimizationTraits <pathOptimization::ConfigOptimizationTraits> >); - parent_t::add <core::PathProjectorBuilder_t> ("Progressive", + pathProjectors.add ("Progressive", createPathProjector <core::pathProjector::Progressive>); - parent_t::add <core::PathProjectorBuilder_t> ("Dichotomy", + pathProjectors.add ("Dichotomy", createPathProjector <core::pathProjector::Dichotomy>); - parent_t::add <core::PathProjectorBuilder_t> ("Global", + pathProjectors.add ("Global", createPathProjector <core::pathProjector::Global>); - parent_t::add <core::PathProjectorBuilder_t> ("RecursiveHermite", + pathProjectors.add ("RecursiveHermite", createPathProjector <core::pathProjector::RecursiveHermite>); - // parent_t::add <PathOptimizerBuilder_t> ("SplineGradientBased_cannonical1",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 1>::createFromCore); - // parent_t::add <PathOptimizerBuilder_t> ("SplineGradientBased_cannonical2",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 2>::createFromCore); - // parent_t::add <PathOptimizerBuilder_t> ("SplineGradientBased_cannonical3",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 3>::createFromCore); - parent_t::add <PathOptimizerBuilder_t> ("SplineGradientBased_bezier1",pathOptimization::SplineGradientBased<core::path::BernsteinBasis, 1>::createFromCore); - // parent_t::add <PathOptimizerBuilder_t> ("SplineGradientBased_bezier2",pathOptimization::SplineGradientBased<core::path::BernsteinBasis, 2>::createFromCore); - parent_t::add <PathOptimizerBuilder_t> ("SplineGradientBased_bezier3",pathOptimization::SplineGradientBased<core::path::BernsteinBasis, 3>::createFromCore); - - using core::SteeringMethodBuilder_t; - parent_t::add <SteeringMethodBuilder_t> ("Graph-SteeringMethodStraight", - GraphSteeringMethod::create <core::SteeringMethodStraight>); - parent_t::add <SteeringMethodBuilder_t> ("Graph-Straight", - GraphSteeringMethod::create <core::steeringMethod::Straight>); - parent_t::add <SteeringMethodBuilder_t> ("Graph-Hermite", - GraphSteeringMethod::create <core::steeringMethod::Hermite>); - parent_t::add <SteeringMethodBuilder_t> ("CrossStateOptimization", - steeringMethod::CrossStateOptimization::createFromCore); - - parent_t::add <PathOptimizerBuilder_t> ("KeypointsShortcut", + // pathOptimizers.add ("SplineGradientBased_cannonical1",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 1>::createFromCore); + // pathOptimizers.add ("SplineGradientBased_cannonical2",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 2>::createFromCore); + // pathOptimizers.add ("SplineGradientBased_cannonical3",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 3>::createFromCore); + pathOptimizers.add ("SplineGradientBased_bezier1",pathOptimization::SplineGradientBased<core::path::BernsteinBasis, 1>::createFromCore); + // pathOptimizers.add ("SplineGradientBased_bezier2",pathOptimization::SplineGradientBased<core::path::BernsteinBasis, 2>::createFromCore); + pathOptimizers.add ("SplineGradientBased_bezier3",pathOptimization::SplineGradientBased<core::path::BernsteinBasis, 3>::createFromCore); + + steeringMethods.add ("Graph-SteeringMethodStraight", + steeringMethod::Graph::create <core::SteeringMethodStraight>); + steeringMethods.add ("Graph-Straight", + steeringMethod::Graph::create <core::steeringMethod::Straight>); + steeringMethods.add ("Graph-Hermite", + steeringMethod::Graph::create <core::steeringMethod::Hermite>); + steeringMethods.add ("Graph-ReedsShepp", + createSMWithGuess <steeringMethod::Graph, core::steeringMethod::ReedsShepp>); + steeringMethods.add ("Graph-Dubins", + createSMWithGuess <steeringMethod::Graph, core::steeringMethod::Dubins>); + steeringMethods.add ("Graph-Snibud", + createSMWithGuess <steeringMethod::Graph, core::steeringMethod::Snibud>); + steeringMethods.add ("CrossStateOptimization-Straight", + steeringMethod::CrossStateOptimization::create<core::steeringMethod::Straight>); + steeringMethods.add ("CrossStateOptimization-ReedsShepp", + createSMWithGuess <steeringMethod::CrossStateOptimization, core::steeringMethod::ReedsShepp>); + steeringMethods.add ("CrossStateOptimization-Dubins", + createSMWithGuess <steeringMethod::CrossStateOptimization, core::steeringMethod::Dubins>); + steeringMethods.add ("CrossStateOptimization-Snibud", + createSMWithGuess <steeringMethod::CrossStateOptimization, core::steeringMethod::Snibud>); + + pathOptimizers.add ("KeypointsShortcut", pathOptimization::Keypoints::create); #if HPP_MANIPULATION_HAS_WHOLEBODY_STEP - parent_t::add <PathOptimizerBuilder_t> - ("Walkgen", wholebodyStep::SmallSteps::create); - parent_t::add <PathOptimizerBuilder_t> - ("Graph-Walkgen", pathOptimization::SmallSteps::create); + pathOptimizers.add ("Walkgen", wholebodyStep::SmallSteps::create); + pathOptimizers.add ("Graph-Walkgen", pathOptimization::SmallSteps::create); #endif pathPlannerType ("M-RRT"); @@ -220,9 +239,9 @@ namespace hpp { JointAndShapes_t l; for (StringList_t::const_iterator it1 = surface1.begin (); it1 != surface1.end(); ++it1) { - if (!robot_->has <JointAndShapes_t> (*it1)) + if (!robot_->jointAndShapes.has (*it1)) throw std::runtime_error ("First list of triangles not found."); - l = robot_->get <JointAndShapes_t> (*it1); + l = robot_->jointAndShapes.get (*it1); for (JointAndShapes_t::const_iterator it = l.begin (); it != l.end(); ++it) { constraints.first->addObject (ConvexShape (it->second, it->first)); @@ -232,11 +251,11 @@ namespace hpp { for (StringList_t::const_iterator it2 = surface2.begin (); it2 != surface2.end(); ++it2) { // Search first robot triangles - if (robot_->has <JointAndShapes_t> (*it2)) - l = robot_->get <JointAndShapes_t> (*it2); + if (robot_->jointAndShapes.has (*it2)) + l = robot_->jointAndShapes.get (*it2); // and then environment triangles. - else if (core::ProblemSolver::has <JointAndShapes_t> (*it2)) - l = core::ProblemSolver::get <JointAndShapes_t> (*it2); + else if (jointAndShapes.has (*it2)) + l = jointAndShapes.get (*it2); else throw std::runtime_error ("Second list of triangles not found."); for (JointAndShapes_t::const_iterator it = l.begin (); it != l.end(); ++it) { @@ -271,9 +290,9 @@ namespace hpp { JointAndShapes_t l; for (StringList_t::const_iterator it1 = surface1.begin (); it1 != surface1.end(); ++it1) { - if (!robot_->has <JointAndShapes_t> (*it1)) + if (!robot_->jointAndShapes.has (*it1)) throw std::runtime_error ("First list of triangles not found."); - l = robot_->get <JointAndShapes_t> (*it1); + l = robot_->jointAndShapes.get (*it1); for (JointAndShapes_t::const_iterator it = l.begin (); it != l.end(); ++it) { @@ -284,11 +303,11 @@ namespace hpp { for (StringList_t::const_iterator it2 = surface2.begin (); it2 != surface2.end(); ++it2) { // Search first robot triangles - if (robot_->has <JointAndShapes_t> (*it2)) - l = robot_->get <JointAndShapes_t> (*it2); + if (robot_->jointAndShapes.has (*it2)) + l = robot_->jointAndShapes.get (*it2); // and then environment triangles. - else if (has <JointAndShapes_t> (*it2)) - l = get <JointAndShapes_t> (*it2); + else if (jointAndShapes.has (*it2)) + l = jointAndShapes.get (*it2); else throw std::runtime_error ("Second list of triangles not found."); for (JointAndShapes_t::const_iterator it = l.begin (); @@ -309,9 +328,9 @@ namespace hpp { if (!constraintGraph ()) { throw std::runtime_error ("The graph is not defined."); } - GripperPtr_t g = robot_->get <GripperPtr_t> (gripper); + GripperPtr_t g = robot_->grippers.get (gripper, GripperPtr_t()); if (!g) throw std::runtime_error ("No gripper with name " + gripper + "."); - HandlePtr_t h = robot_->get <HandlePtr_t> (handle); + HandlePtr_t h = robot_->handles.get (handle, HandlePtr_t()); if (!h) throw std::runtime_error ("No handle with name " + handle + "."); const std::string cname = name + "/complement"; const std::string bname = name + "/hold"; @@ -328,9 +347,9 @@ namespace hpp { (const std::string& name, const std::string& gripper, const std::string& handle) { - GripperPtr_t g = robot_->get <GripperPtr_t> (gripper); + GripperPtr_t g = robot_->grippers.get (gripper, GripperPtr_t()); if (!g) throw std::runtime_error ("No gripper with name " + gripper + "."); - HandlePtr_t h = robot_->get <HandlePtr_t> (handle); + HandlePtr_t h = robot_->handles.get (handle, HandlePtr_t()); if (!h) throw std::runtime_error ("No handle with name " + handle + "."); value_type c = h->clearance () + g->clearance (); @@ -344,7 +363,7 @@ namespace hpp { parent_t::pathValidationType(type, tolerance); assert (problem_); problem_->setPathValidationFactory ( - parent_t::get<core::PathValidationBuilder_t>(type), + pathValidations.get(type), tolerance); } diff --git a/src/problem.cc b/src/problem.cc index 69a00b26049d662dddde3edd8560482f5503a589..883f4a9d7b12e5bf0e6f59adb4222193ee506aea 100644 --- a/src/problem.cc +++ b/src/problem.cc @@ -19,7 +19,7 @@ #include <hpp/core/discretized-collision-checking.hh> #include <hpp/manipulation/weighed-distance.hh> -#include <hpp/manipulation/graph-steering-method.hh> +#include <hpp/manipulation/steering-method/graph.hh> #include <hpp/manipulation/graph-path-validation.hh> #include <hpp/manipulation/graph/graph.hh> @@ -28,7 +28,7 @@ namespace hpp { Problem::Problem (DevicePtr_t robot) : Parent (robot), graph_() { - Parent::steeringMethod (GraphSteeringMethod::create (*this)); + Parent::steeringMethod (steeringMethod::Graph::create (*this)); distance (WeighedDistance::create (robot, graph_)); setPathValidationFactory(core::DiscretizedCollisionChecking::create, 0.05); diff --git a/src/steering-method/cross-state-optimization.cc b/src/steering-method/cross-state-optimization.cc index 8d0929af461da143f0ef7308efe0920b9b26ad5d..83bef2bb41a234f11902394442e538961b9bc2d1 100644 --- a/src/steering-method/cross-state-optimization.cc +++ b/src/steering-method/cross-state-optimization.cc @@ -24,6 +24,8 @@ #include <hpp/util/exception-factory.hh> +#include <pinocchio/multibody/model.hpp> + #include <hpp/pinocchio/configuration.hh> #include <hpp/constraints/hybrid-solver.hh> @@ -39,10 +41,18 @@ namespace hpp { namespace manipulation { namespace steeringMethod { - using namespace graph; using Eigen::RowBlockIndices; using Eigen::ColBlockIndices; + using graph::StatePtr_t; + using graph::States_t; + using graph::EdgePtr_t; + using graph::Edges_t; + using graph::Neighbors_t; + using graph::NumericalConstraints_t; + using graph::LockedJoints_t; + using graph::segments_t; + CrossStateOptimizationPtr_t CrossStateOptimization::create ( const Problem& problem) { @@ -51,7 +61,7 @@ namespace hpp { return shPtr; } - CrossStateOptimizationPtr_t CrossStateOptimization::createFromCore ( + CrossStateOptimizationPtr_t CrossStateOptimization::create ( const core::Problem& problem) { HPP_STATIC_CAST_REF_CHECK (const Problem, problem); @@ -194,18 +204,25 @@ namespace hpp { typedef std::vector<States_t> StatesPerConf_t; StatesPerConf_t statesPerConf_; struct RightHandSideSetter { - DifferentiableFunctionPtr_t impF, expF; + DifferentiableFunctionPtr_t impF; + size_type expFidx; Configuration_t* qrhs; vector_t rhs; RightHandSideSetter () : qrhs (NULL) {} - RightHandSideSetter (DifferentiableFunctionPtr_t _impF, DifferentiableFunctionPtr_t _expF, Configuration_t* _qrhs) - : impF(_impF), expF(_expF), qrhs (_qrhs) {} - RightHandSideSetter (DifferentiableFunctionPtr_t _impF, DifferentiableFunctionPtr_t _expF, vector_t _rhs) - : impF(_impF), expF(_expF), qrhs (NULL), rhs (_rhs) {} + // TODO delete this constructor + RightHandSideSetter (DifferentiableFunctionPtr_t _impF, size_type _expFidx, Configuration_t* _qrhs) + : impF(_impF), expFidx(_expFidx), qrhs (_qrhs) {} + RightHandSideSetter (DifferentiableFunctionPtr_t _impF, size_type _expFidx, vector_t _rhs) + : impF(_impF), expFidx(_expFidx), qrhs (NULL), rhs (_rhs) {} void apply(constraints::HybridSolver& s) { - if (qrhs != NULL) s.rightHandSideFromInput (impF, expF, *qrhs); - else s.rightHandSide (impF, expF, rhs); + if (expFidx >= 0) { + if (qrhs != NULL) s.explicitSolver().rightHandSideFromInput (expFidx, *qrhs); + else s.explicitSolver().rightHandSide (expFidx, rhs); + } else { + if (qrhs != NULL) s.rightHandSideFromInput (impF, DifferentiableFunctionPtr_t(), *qrhs); + else s.rightHandSide (impF, DifferentiableFunctionPtr_t(), rhs); + } } }; typedef std::vector<RightHandSideSetter> RightHandSideSetters_t; @@ -216,9 +233,10 @@ namespace hpp { solver (N * nq, N * nv), robot (_robot), statesPerConf_ (N) { solver.integration (boost::bind(&OptimizationData::_integrate, this, _1, _2, _3)); + solver.saturation (boost::bind(&OptimizationData::_saturate , this, _1, _2)); } - void addGraphConstraints (const GraphPtr_t& graph) + void addGraphConstraints (const graph::GraphPtr_t& graph) { for (std::size_t i = 0; i < N; ++i) { _add (graph->lockedJoints(), i); @@ -245,29 +263,39 @@ namespace hpp { LockedJointPtr_t lj (*_lj); std::ostringstream os; os << lj->jointName() << " | " << i << " -> " << (i+1); - DifferentiableFunctionPtr_t f; + DifferentiableFunctionPtr_t f, f_implicit; // i = Input, o = Output, // c = Config, v = Velocity RowBlockIndices ic, oc, ov; ColBlockIndices iv; ComparisonTypes_t cts; - Configuration_t* qrhs; + vector_t rhs; if (i == 0) { - f = lj->function(); + f = lj->explicitFunction(); ic = _row(lj->inputConf() , 0); oc = _row(lj->outputConf() , 0); iv = _col(lj->inputVelocity() , 0); ov = _row(lj->outputVelocity(), 0); - cts = ComparisonTypes_t (lj->numberDof(), constraints::Equality); - qrhs = &q1; + cts = lj->comparisonType(); + lj->rightHandSideFromConfig (q1); + rhs = lj->rightHandSide(); } else if (i == N) { - f = lj->function(); + f = lj->explicitFunction(); + // Currently, this function is mandatory because if the same + // joint is locked all along the path, then, one of the LockedJoint + // has to be treated implicitely. + // TODO it would be smarter to detect this case beforehand. If the + // chain in broken in the middle, an explicit formulation exists + // (by inverting the equality in the next else section) and we + // miss it. + f_implicit = _stateFunction (lj->functionPtr(), N-1); ic = _row(lj->inputConf() , 0); oc = _row(lj->outputConf() , (N-1) * nq); iv = _col(lj->inputVelocity() , 0); ov = _row(lj->outputVelocity(), (N-1) * nv); - cts = ComparisonTypes_t (lj->numberDof(), constraints::Equality); - qrhs = &q2; + cts = lj->comparisonType(); + lj->rightHandSideFromConfig (q2); + rhs = lj->rightHandSide(); } else { f = Identity::Ptr_t (new Identity (lj->configSpace(), os.str())); ic = _row(lj->outputConf() , (i - 1) * nq); @@ -275,21 +303,26 @@ namespace hpp { iv = _col(lj->outputVelocity(), (i - 1) * nv); ov = _row(lj->outputVelocity(), i * nv); cts = ComparisonTypes_t (lj->numberDof(), constraints::EqualToZero); - qrhs = NULL; } - bool added = solver.explicitSolver().add (f, ic, oc, iv, ov, cts); - if (!added) { - HPP_THROW (std::invalid_argument, - "Could not add locked joint " << lj->jointName() << - " of transition " << trans->name() << " at id " << i); + // It is important to use the index of the function since the same + // function may be added several times on different part. + size_type expFidx = solver.explicitSolver().add (f, ic, oc, iv, ov, cts); + if (expFidx < 0) { + if (f_implicit) { + solver.add (f_implicit, 0, cts); + } else { + HPP_THROW (std::invalid_argument, + "Could not add locked joint " << lj->jointName() << + " of transition " << trans->name() << " at id " << i); + } } - // This must be done later - if (qrhs != NULL) + // Setting the right hand side must be done later + if (rhs.size() > 0) rhsSetters_.push_back (RightHandSideSetter( - DifferentiableFunctionPtr_t(), f, qrhs)); - // solver.rightHandSideFromInput (DifferentiableFunctionPtr_t(), f, *qrhs); + f_implicit, expFidx, rhs)); + f_implicit.reset(); } // TODO handle numerical constraints @@ -335,15 +368,15 @@ namespace hpp { // qrhs = NULL; } - bool added = false; - if (ef) added = solver.explicitSolver().add (ef, ic, oc, iv, ov, cts); - if (!added) solver.add (f, 0, cts); + size_type expFidx = -1; + if (ef) expFidx = solver.explicitSolver().add (ef, ic, oc, iv, ov, cts); + if (expFidx < 0) solver.add (f, 0, cts); // TODO This must be done later... // if (qrhs != NULL) { if (rhs.size() > 0) { // solver.rightHandSideFromInput (f, ef, rhs); - rhsSetters_.push_back (RightHandSideSetter(f, ef, rhs)); + rhsSetters_.push_back (RightHandSideSetter(f, expFidx, rhs)); } } } @@ -393,23 +426,23 @@ namespace hpp { for (LockedJoints_t::const_iterator _lj = ljs.begin(); _lj != ljs.end(); ++_lj) { LockedJointPtr_t lj (*_lj); - bool added = solver.explicitSolver().add ( - lj->function(), + size_type expFidx = solver.explicitSolver().add ( + lj->explicitFunction(), _row(lj->inputConf() , i * nq), _row(lj->outputConf() , i * nq), _col(lj->inputVelocity() , i * nv), _row(lj->outputVelocity(), i * nv), lj->comparisonType ()); - if (!added) + if (expFidx < 0) throw std::invalid_argument ("Could not add locked joint " + lj->jointName()); // This must be done later rhsSetters_.push_back (RightHandSideSetter( DifferentiableFunctionPtr_t(), - lj->function(), + expFidx, lj->rightHandSide())); // solver.rightHandSide (DifferentiableFunctionPtr_t(), - // lj->function(), + // lj->explicitFunction(), // lj->rightHandSide()); } } @@ -447,6 +480,53 @@ namespace hpp { v.segment(iv,nv), qout.segment(iq,nq)); } + bool _saturate (vectorIn_t q, Eigen::VectorXi& sat) + { + bool ret = false; + const se3::Model& model = robot->model(); + + for (std::size_t i = 1; i < model.joints.size(); ++i) { + const size_type jnq = model.joints[i].nq(); + const size_type jnv = model.joints[i].nv(); + const size_type jiq = model.joints[i].idx_q(); + const size_type jiv = model.joints[i].idx_v(); + for (std::size_t k = 0; k < N; ++k) { + const size_type idx_q = k * nq + jiq; + const size_type idx_v = k * nv + jiv; + for (size_type j = 0; j < jnq; ++j) { + const size_type iq = idx_q + j; + const size_type iv = idx_v + std::min(j,jnv-1); + if (q[iq] >= model.upperPositionLimit[jiq + j]) { + sat[iv] = 1; + ret = true; + } else if (q[iq] <= model.lowerPositionLimit[jiq + j]) { + sat[iv] = -1; + ret = true; + } else + sat[iv] = 0; + } + } + } + + const hpp::pinocchio::ExtraConfigSpace& ecs = robot->extraConfigSpace(); + const size_type& d = ecs.dimension(); + + for (size_type k = 0; k < d; ++k) { + for (std::size_t j = 0; j < N; ++j) { + const size_type iq = j * nq + model.nq + k; + const size_type iv = j * nv + model.nv + k; + if (q[iq] >= ecs.upper(k)) { + sat[iv] = 1; + ret = true; + } else if (q[iq] <= ecs.lower(k)) { + sat[iv] = -1; + ret = true; + } else + sat[iv] = 0; + } + } + return ret; + } }; void CrossStateOptimization::buildOptimizationProblem ( @@ -541,7 +621,7 @@ namespace hpp { core::PathPtr_t CrossStateOptimization::impl_compute ( ConfigurationIn_t q1, ConfigurationIn_t q2) const { - const Graph& graph = *problem_.constraintGraph (); + const graph::Graph& graph = *problem_.constraintGraph (); GraphSearchData d; d.s1 = graph.getState (q1); d.s2 = graph.getState (q2); diff --git a/src/steering-method/cross-state-optimization/function.cc b/src/steering-method/cross-state-optimization/function.cc index a536f0e1db50ac42af581291d1c08d791ba389ae..3ba0c3d79aefbf8eafacf286723ea8ec674ef6ea 100644 --- a/src/steering-method/cross-state-optimization/function.cc +++ b/src/steering-method/cross-state-optimization/function.cc @@ -31,6 +31,8 @@ namespace hpp { } } + /// Apply the constraint on a subspace of the input space. + /// i.e.: \f$ f (q_0, ... , q_n) = f_{inner} (q_k) \f$ class HPP_MANIPULATION_LOCAL StateFunction : public constraints::DifferentiableFunction { @@ -74,6 +76,8 @@ namespace hpp { const segment_t sa_, sd_; }; // class Function + /// \f$ q_{out} = q_{in} \f$ + /// \todo Make this derive from constraints::AffineFunction class HPP_MANIPULATION_LOCAL Identity : public constraints::DifferentiableFunction { @@ -95,6 +99,8 @@ namespace hpp { } }; // class Function + /// Compute the difference between the value of the function in two points. + /// i.e.: \f$ f (q_0, ... , q_n) = f_{inner} (q_{left}) - f_{inner} (q_{right}) \f$ class HPP_MANIPULATION_LOCAL EdgeFunction : public constraints::DifferentiableFunction { diff --git a/src/steering-method/graph.cc b/src/steering-method/graph.cc new file mode 100644 index 0000000000000000000000000000000000000000..107baac9bfc62da4668d1bcd758e5509fa2c1cd7 --- /dev/null +++ b/src/steering-method/graph.cc @@ -0,0 +1,107 @@ +// Copyright (c) 2014, LAAS-CNRS +// Authors: Joseph Mirabel (joseph.mirabel@laas.fr) +// +// 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/steering-method/graph.hh" + +#include <hpp/util/pointer.hh> + +#include <hpp/core/straight-path.hh> +#include <hpp/core/steering-method/straight.hh> + +#include <hpp/manipulation/problem.hh> +#include <hpp/manipulation/graph/graph.hh> +#include <hpp/manipulation/graph/edge.hh> + +namespace hpp { + namespace manipulation { + SteeringMethod::SteeringMethod (const Problem& problem) : + core::SteeringMethod (problem), problem_ (problem), + steeringMethod_ (core::SteeringMethodStraight::create (problem)) + { + } + + SteeringMethod::SteeringMethod (const SteeringMethod& other): + core::SteeringMethod (other), problem_ (other.problem_), steeringMethod_ + (other.steeringMethod_) + { + } + + namespace steeringMethod { + + GraphPtr_t Graph::create + (const core::Problem& problem) + { + HPP_STATIC_CAST_REF_CHECK (const Problem, problem); + const Problem& p = static_cast <const Problem&> (problem); + return create (p); + } + + GraphPtr_t Graph::create + (const Problem& problem) + { + Graph* ptr = new Graph (problem); + GraphPtr_t shPtr (ptr); + ptr->init (shPtr); + return shPtr; + } + + GraphPtr_t Graph::createCopy + (const GraphPtr_t& other) + { + Graph* ptr = new Graph (*other); + GraphPtr_t shPtr (ptr); + ptr->init (shPtr); + return shPtr; + } + + Graph::Graph (const Problem& problem) : + SteeringMethod (problem), weak_ () + { + } + + Graph::Graph (const Graph& other): + SteeringMethod (other) + { + } + + PathPtr_t Graph::impl_compute (ConfigurationIn_t q1, ConfigurationIn_t q2) const + { + graph::Edges_t possibleEdges; + if (!problem_.constraintGraph()) + throw std::invalid_argument ("The constraint graph should be set to use the steeringMethod::Graph"); + const graph::Graph& graph = *problem_.constraintGraph (); + try { + possibleEdges = graph.getEdges + (graph.getState (q1), graph.getState (q2)); + } catch (const std::logic_error& e) { + hppDout (error, e.what ()); + return PathPtr_t (); + } + PathPtr_t path; + if (possibleEdges.empty()) { + hppDout (info, "No edge found."); + } + while (!possibleEdges.empty()) { + if (possibleEdges.back ()->build (path, q1, q2)) { + return path; + } + possibleEdges.pop_back (); + } + return PathPtr_t (); + } + } // namespace steeringMethod + } // namespace manipulation +} // namespace hpp