Merge branch 'Joseph'

CMakeLists.txt

@@ -45,6 +45,12 @@ SET (${PROJECT_NAME}_HEADERS
   include/hpp/manipulation/object.hh
   include/hpp/manipulation/problem-solver.hh
   include/hpp/manipulation/robot.hh
+  include/hpp/manipulation/manipulation-planner.hh
+  include/hpp/manipulation/graph/node.hh
+  include/hpp/manipulation/graph/edge.hh
+  include/hpp/manipulation/graph/node-selector.hh
+  include/hpp/manipulation/graph/graph.hh
+  include/hpp/manipulation/graph/fwd.hh
   )
 
 # Add dependency toward hpp-model library in pkg-config file.

include/hpp/manipulation/fwd.hh

@@ -61,6 +61,8 @@ namespace hpp {
     typedef model::vector_t vector_t;
     typedef model::vectorIn_t vectorIn_t;
     typedef model::vectorOut_t vectorOut_t;
+    HPP_PREDEF_CLASS (ManipulationPlanner);
+    typedef boost::shared_ptr < ManipulationPlanner > ManipulationPlannerPtr_t;
 
     typedef std::vector <DevicePtr_t> Devices_t;
     typedef std::vector <ObjectPtr_t> Objects_t;

include/hpp/manipulation/graph/edge.hh

+// 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/>.
+
+#ifndef HPP_MANIPULATION_GRAPH_EDGE_HH
+# define HPP_MANIPULATION_GRAPH_EDGE_HH
+
+#include <hpp/core/constraints-set.hh>
+
+#include "hpp/manipulation/fwd.hh"
+#include "hpp/manipulation/graph/gripper-state.hh"
+
+namespace hpp {
+  namespace manipulation {
+    namespace graph {
+      /// Transition between states of a end-effector.
+      ///
+      /// Vertices of the graph of constraints.
+      class HPP_MANIPULATION_DLLAPI Edge
+      {
+        public:
+          /// Projector to project onto the same leaf as config.
+          /// \return The initialized projector.
+          /// \param config Configuration that will initialize the projector.
+          ConfigProjectorPtr_t configurationProjector(const Configuration_t config);
+
+          /// Projector to project a path.
+          /// \return The initialized projector.
+          /// \param config Configuration that will initialize the projector.
+          ConfigProjectorPtr_t pathProjector(const Configuration_t config);
+
+        private:
+          /// The two ends of the transition.
+          NodeWkPtr_t startState, endState;
+
+          /// Set of constraints to be statisfied.
+          ConstraintSetPtr_t constraints_;
+
+          /// Projectors ensuring that a path between q_near and q_proj is
+          /// valid regarding the manipulation rules.
+          ConfigProjectorPtr_t configurationProjector_;
+
+          /// Projectors ensuring that a path between two configurations in
+          /// the same leaf lies in the leaf.
+          ConfigProjectorPtr_t pathProjector_;
+      }; // class Edge
+    } // namespace graph
+  } // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_GRAPH_EDGE_HH

include/hpp/manipulation/graph/fwd.hh

+// 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/>.
+
+#ifndef HPP_MANIPULATION_GRAPH_FWD_HH
+# define HPP_MANIPULATION_GRAPH_FWD_HH
+
+#include <hpp/util/pointer.hh>
+
+namespace hpp {
+  namespace manipulation {
+    namespace graph {
+      HPP_PREDEF_CLASS (Node);
+      HPP_PREDEF_CLASS (Edge);
+      HPP_PREDEF_CLASS (NodeSelector);
+      typedef boost::shared_ptr < Edge > EdgePtr_t;
+      typedef boost::shared_ptr < NodeSelector > NodeSelectorPtr_t;
+
+      typedef hpp::core::ConstraintSet ConstraintSet_t;
+      typedef hpp::core::ConstraintSetPtr_t ConstraintSetPtr_t;
+      typedef hpp::core::ConfigProjectorPtr_t ConfigProjectorPtr_t;
+    } // namespace graph
+  } // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_GRAPH_FWD_HH

include/hpp/manipulation/graph/graph.hh

+// 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/>.
+
+
+#ifndef HPP_MANIPULATION_GRAPHGRAPH_HH
+# define HPP_MANIPULATION_GRAPHGRAPH_HH
+
+#include "hpp/manipulation/fwd.hh"
+#include "hpp/manipulation/graph/fwd.hh"
+
+namespace hpp {
+  namespace manipulation {
+    namespace graph {
+      /// Description of the constraint graph
+      /// This class contains a graph representing a robot with several
+      /// end-effectors.
+      class HPP_MANIPULATION_DLLAPI Graph
+      {
+        public:
+
+        private:
+          /// This list contains a node selector for each end-effector.
+          set::list < NodeSelectorPtr_t > nodeSelectors_;
+
+          /// A set of constraints that will always be used, for example
+          /// stability constraints.
+          ConstraintSetPtr_t constraints_;
+      }
+    } // namespace graph
+  } // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_GRAPHGRAPH_HH

include/hpp/manipulation/graph/node-selector.hh

+// 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/>.
+
+#ifndef HPP_MANIPULATION_GRAPH_NODE_SELECTOR_HH
+# define HPP_MANIPULATION_GRAPH_NODE_SELECTOR_HH
+
+#include "hpp/manipulation/fwd.hh"
+#include "hpp/manipulation/graph/gripper-state.hh"
+
+namespace hpp {
+  namespace manipulation {
+    namespace graph {
+      /// This class is used to get the state of a configuration. States have to
+      /// be ordered because a configuration can be in several states.
+      class HPP_MANIPULATION_DLLAPI NodeSelector
+      {
+        public:
+          /// Returns the state of a configuration.
+          virtual Node getState(const Configuration_t config) const;
+
+        private:
+          /// List of the states of one end-effector, ordered by priority.
+          std::vector< NodeWkPtr_t > orderedStates_;
+      }; //
+    } // namespace graph
+  } // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_GRAPH_NODE_SELECTOR_HH

include/hpp/manipulation/graph/node.hh

+// 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/>.
+
+#ifndef HPP_MANIPULATION_GRAPH_NODE_HH
+# define HPP_MANIPULATION_GRAPH_NODE_HH
+
+#include <hpp/core/constraints-set.hh>
+
+#include "hpp/manipulation/fwd.hh"
+#include "hpp/manipulation/graph/fwd.hh"
+#include "hpp/manipulation/graph/edge.hh"
+
+namespace hpp {
+  namespace manipulation {
+    namespace graph {
+      /// State of an end-effector.
+      ///
+      /// Nodes of the graph of constraints. There is one
+      /// graph for each end-effector.
+      class HPP_MANIPULATION_DLLAPI Node
+      {
+        public:
+          /// Check whether the configuration is in this state.
+          /// \return True if this state contains this configuration
+          /// \param config The configuration to be tested.
+          /// \note You should note use that to know in which states a
+          /// configuration is. This only checks if the configuration satisfies
+          /// the constraints. Instead, use the class NodeSelector.
+          virtual bool contains(const Configuration_t config) const;
+
+        private:
+          /// List of possible motions from this state (i.e. the outgoing
+          /// vertices).
+          std::vector < EdgePtr_t > neighbors_;
+
+          /// Set of constraints to be statisfied.
+          ConstraintSetPtr_t constraints_;
+
+          /// A selector that will implement the selection of the next state.
+          NodeSelectorPtr_t selector_;
+      }; // class Node
+    } // namespace graph
+  } // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_GRAPH_NODE_HH

include/hpp/manipulation/manipulation-planner.hh

+// 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/>.
+
+#ifndef HPP_MANIPULATION_MANIPULATION_PLANNER_HH
+# define HPP_MANIPULATION_MANIPULATION_PLANNER_HH
+
+#include <hpp/core/basic-configuration-shooter.hh>
+#include <hpp/core/path-planner.hh>
+#include <hpp/core/problem.hh>
+#include <hpp/core/roadmap.hh>
+
+#include "hpp/manipulation/config.hh"
+#include "hpp/manipulation/graph/fwd.hh"
+#include "hpp/manipulation/fwd.hh"
+
+namespace hpp {
+  namespace manipulation {
+    class HPP_MANIPULATION_DLLAPI ManipulationPlanner : public hpp::core::PathPlanner
+    {
+      public:
+        /// Create an instance and return a shared pointer to the instance
+        static ManipulationPlannerPtr_t create (const core::Problem& problem,
+            const core::RoadmapPtr_t& roadmap);
+
+        /// One step of extension.
+        /// 
+        /// A set of constraints is choosen using the graph of constraints.
+        /// A constraint extension is done using a choosen set.
+        ///
+        virtual void oneStep ();
+
+      protected:
+        /// Protected constructor
+        ManipulationPlanner (const core::Problem& problem,
+            const core::RoadmapPtr_t& roadmap);
+
+        /// Store weak pointer to itself
+        void init (const ManipulationPlannerWkPtr_t& weak);
+
+      private:
+        /// Configuration shooter to uniformly shoot random configurations
+        core::BasicConfigurationShooter shooter_;
+        /// weak pointer to itself
+        ManipulationPlannerWkPtr_t weakPtr_;
+    };
+  } // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_MANIPULATION_PLANNER_HH

src/CMakeLists.txt

@@ -24,6 +24,7 @@ ADD_LIBRARY(${LIBRARY_NAME} SHARED
   handle.cc
   problem-solver.cc
   robot.cc
+  manipulation-planner.cc
 )
 
 PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} hpp-core)

src/manipulation-planner.cc

+// 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/manipulation-planner.hh"
+
+namespace hpp {
+  namespace manipulation {
+    ManipulationPlannerPtr_t ManipulationPlanner::create (const core::Problem& problem,
+        const core::RoadmapPtr_t& roadmap)
+    {
+      ManipulationPlanner* ptr = new ManipulationPlanner (problem, roadmap);
+      ManipulationPlannerPtr_t shPtr (ptr);
+      ptr->init (shPtr);
+      return shPtr;
+    }
+
+    void ManipulationPlanner::oneStep ()
+    {
+    }
+
+    ManipulationPlanner::ManipulationPlanner (const core::Problem& problem,
+        const core::RoadmapPtr_t& roadmap) :
+      core::PathPlanner (problem, roadmap), shooter_ (problem.robot ())
+    {}
+
+    void ManipulationPlanner::init (const ManipulationPlannerWkPtr_t& weak)
+    {
+      core::PathPlanner::init (weak);
+      weakPtr_ = weak;
+    }
+  } // namespace manipulation
+} // namespace hpp