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