diff --git a/CMakeLists.txt b/CMakeLists.txt
index a3f43fcc2b5f4d47e614e27012c40cf837b555c0..22ba1cd0e64f9e701b4055e42691680015d4acbc 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -48,16 +48,19 @@ ADD_DOC_DEPENDENCY("hpp-fcl")
ADD_REQUIRED_DEPENDENCY("hpp-core >= 3.0.0")
ADD_REQUIRED_DEPENDENCY("hpp-constraints >= 3.0.0")
ADD_REQUIRED_DEPENDENCY("hpp-statistics >= 0.1")
+ADD_OPTIONAL_DEPENDENCY("hpp-wholebody-step")
IF (TEST_UR5)
ADD_REQUIRED_DEPENDENCY(hpp-model-urdf >= 3.0.0)
ENDIF ()
-CONFIGURE_FILE (${CMAKE_SOURCE_DIR}/doc/main.hh.in
- ${CMAKE_BINARY_DIR}/doc/main.hh
- @ONLY
- )
+SET(HPP_MANIPULATION_HAS_WHOLEBODY_STEP ${HPP_WHOLEBODY_STEP_FOUND}
+ CACHE BOOL "Compile with dependency to hpp-wholebody-step")
+
+CONFIG_FILES (doc/main.hh
+ include/hpp/manipulation/package-config.hh)
SET (${PROJECT_NAME}_HEADERS
+ ${CMAKE_BINARY_DIR}/include/hpp/manipulation/package-config.hh
include/hpp/manipulation/fwd.hh
include/hpp/manipulation/axial-handle.hh
include/hpp/manipulation/handle.hh
@@ -84,6 +87,7 @@ SET (${PROJECT_NAME}_HEADERS
include/hpp/manipulation/graph/dot.hh
include/hpp/manipulation/graph/helper.hh
+ include/hpp/manipulation/path-optimization/small-steps.hh
include/hpp/manipulation/path-optimization/config-optimization.hh
)
diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index 2a1c6aeed31cbe439285a35f8999c666176e7853..f8d2297f12d9e9f0a2aaa71ff047a4fd3a0bc4a6 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -123,6 +123,11 @@ namespace hpp {
typedef HPP_MANIPULATION_DEPRECATED Shape_t Triangle;
typedef HPP_MANIPULATION_DEPRECATED JointAndShape_t JointAndTriangle_t;
typedef HPP_MANIPULATION_DEPRECATED JointAndShapes_t JointAndTriangles_t;
+
+ namespace pathOptimization {
+ HPP_PREDEF_CLASS (SmallSteps);
+ typedef boost::shared_ptr < SmallSteps > SmallStepsPtr_t;
+ } // namespace pathOptimization
} // namespace manipulation
} // namespace hpp
diff --git a/include/hpp/manipulation/package-config.hh.in b/include/hpp/manipulation/package-config.hh.in
new file mode 100644
index 0000000000000000000000000000000000000000..9814fac372a544a67ecb8654117c4bb088d09cd4
--- /dev/null
+++ b/include/hpp/manipulation/package-config.hh.in
@@ -0,0 +1,22 @@
+// Copyright (c) 2016, Joseph Mirabel
+// 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_PACKAGECONFIG_HH
+#define HPP_MANIPULATION_PACKAGECONFIG_HH
+
+#cmakedefine01 HPP_MANIPULATION_HAS_WHOLEBODY_STEP
+
+#endif // HPP_MANIPULATION_PACKAGECONFIG_HH
diff --git a/include/hpp/manipulation/path-optimization/small-steps.hh b/include/hpp/manipulation/path-optimization/small-steps.hh
new file mode 100644
index 0000000000000000000000000000000000000000..65950deca892323bbd3306915bcb7de0f462329b
--- /dev/null
+++ b/include/hpp/manipulation/path-optimization/small-steps.hh
@@ -0,0 +1,62 @@
+// Copyright (c) 2016, Joseph Mirabel
+// 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_PATHOPTIMIZATION_SMALLSTEPS_HH
+#define HPP_MANIPULATION_PATHOPTIMIZATION_SMALLSTEPS_HH
+
+#include <hpp/manipulation/fwd.hh>
+#include <hpp/manipulation/config.hh>
+
+#include <hpp/core/path-optimizer.hh>
+
+namespace hpp {
+ namespace manipulation {
+ namespace pathOptimization {
+ using hpp::core::Path;
+ using hpp::core::PathPtr_t;
+ using hpp::core::PathVector;
+ using hpp::core::PathVectorPtr_t;
+
+ /// \addtogroup path_optimization
+ /// \{
+
+ /// Walking trajectory generator for paths created with the constraint graph
+ ///
+ /// This class encapsulates hpp::wholebodyStep::SmallSteps.
+ class HPP_MANIPULATION_DLLAPI SmallSteps : public PathOptimizer
+ {
+ public:
+ static SmallStepsPtr_t create (const core::Problem& problem)
+ {
+ SmallSteps* ptr (new SmallSteps (problem));
+ return SmallStepsPtr_t (ptr);
+ }
+
+ PathVectorPtr_t optimize (const PathVectorPtr_t& path);
+
+ protected:
+ /// Constructor
+ SmallSteps (const core::Problem& problem) :
+ PathOptimizer (problem)
+ {}
+ };
+ /// \}
+
+ } // namespace pathOptimization
+ } // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_PATHOPTIMIZATION_SMALLSTEPS_HH
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 327ddcf6f662a180d29ef1b7a6f79aee4d84b99e..b9edbd26044ec15463b1a7e84dfa5a0030003975 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -19,7 +19,7 @@
SET(LIBRARY_NAME ${PROJECT_NAME})
-ADD_LIBRARY(${LIBRARY_NAME} SHARED
+SET(SOURCES
axial-handle.cc
handle.cc
manipulation-planner.cc
@@ -47,10 +47,22 @@ ADD_LIBRARY(${LIBRARY_NAME} SHARED
graph/dot.cc
path-optimization/config-optimization.cc
-)
+ )
+
+IF(HPP_MANIPULATION_HAS_WHOLEBODY_STEP)
+ SET(SOURCES
+ ${SOURCES}
+ path-optimization/small-steps.cc
+ )
+ENDIF(HPP_MANIPULATION_HAS_WHOLEBODY_STEP)
+
+ADD_LIBRARY(${LIBRARY_NAME} SHARED ${SOURCES})
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} hpp-core)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} hpp-statistics)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} hpp-constraints)
+IF(HPP_MANIPULATION_HAS_WHOLEBODY_STEP)
+ PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} hpp-wholebody-step)
+ENDIF(HPP_MANIPULATION_HAS_WHOLEBODY_STEP)
INSTALL(TARGETS ${LIBRARY_NAME} DESTINATION lib)
diff --git a/src/path-optimization/small-steps.cc b/src/path-optimization/small-steps.cc
new file mode 100644
index 0000000000000000000000000000000000000000..6f9c8d86994370dfe51d15aaa42858a621d456e8
--- /dev/null
+++ b/src/path-optimization/small-steps.cc
@@ -0,0 +1,76 @@
+// Copyright (c) 2016, Joseph Mirabel
+// 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/path-optimization/small-steps.hh>
+
+#include <hpp/core/problem.hh>
+
+#include <hpp/wholebody-step/small-steps.hh>
+
+#include <hpp/manipulation/graph/edge.hh>
+#include <hpp/manipulation/constraint-set.hh>
+#include <hpp/manipulation/graph-path-validation.hh>
+
+namespace hpp {
+ namespace manipulation {
+ namespace pathOptimization {
+ PathVectorPtr_t SmallSteps::optimize (const PathVectorPtr_t& path)
+ {
+ PathVectorPtr_t
+ opted = PathVector::create (path->outputSize(),
+ path->outputDerivativeSize()),
+ flat = PathVector::create (path->outputSize(),
+ path->outputDerivativeSize()),
+ toConcat;
+ path->flatten (flat);
+
+ GraphPathValidationPtr_t gpv = HPP_DYNAMIC_PTR_CAST (GraphPathValidation,
+ this->problem().pathValidation ());
+ const_cast <core::Problem&>(this->problem ()).pathValidation (gpv->innerValidation());
+
+ wholebodyStep::SmallStepsPtr_t stepPtr
+ (wholebodyStep::SmallSteps::create(problem()));
+ wholebodyStep::SmallSteps& step (*stepPtr);
+ step.leftHand_.active = true;
+
+ ConstraintSetPtr_t c;
+ for (std::size_t i_s = 0; i_s < flat->numberPaths ();) {
+ PathVectorPtr_t toOpt = PathVector::create (
+ path->outputSize(), path->outputDerivativeSize());
+ PathPtr_t current = flat->pathAtRank (i_s);
+ toOpt->appendPath (current);
+ graph::EdgePtr_t edge;
+ c = HPP_DYNAMIC_PTR_CAST (ConstraintSet, current->constraints ());
+ if (c) edge = c->edge ();
+ std::size_t i_e = i_s + 1;
+ for (; i_e < flat->numberPaths (); ++i_e) {
+ current = flat->pathAtRank (i_e);
+ c = HPP_DYNAMIC_PTR_CAST (ConstraintSet, current->constraints ());
+ if (!c && edge) break;
+ if (c && edge->node() != c->edge ()->node()) break;
+ toOpt->appendPath (current);
+ }
+ toConcat = step.optimize (toOpt);
+ i_s = i_e;
+ opted->concatenate (*toConcat);
+ }
+
+ const_cast <core::Problem&>(this->problem ()).pathValidation (gpv);
+ return opted;
+ }
+ } // namespace pathOptimization
+ } // namespace manipulation
+} // namespace hpp
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index 8f590ae90fba4293eb4cfd90d0f7f9de5ba31e3a..fbc1a9e4628f528e45ffd970282bf2a2c37a6262 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -34,6 +34,8 @@
#include <hpp/core/steering-method-straight.hh>
#include <hpp/core/comparison-type.hh>
+#include "hpp/manipulation/package-config.hh" // HPP_MANIPULATION_HAS_WHOLEBODY_STEP
+
#include "hpp/manipulation/problem.hh"
#include "hpp/manipulation/device.hh"
#include "hpp/manipulation/handle.hh"
@@ -47,6 +49,11 @@
#include "hpp/manipulation/graph-steering-method.hh"
#include "hpp/manipulation/path-optimization/config-optimization.hh"
+#ifdef HPP_MANIPULATION_HAS_WHOLEBODY_STEP
+#include <hpp/wholebody-step/small-steps.hh>
+#include "hpp/manipulation/path-optimization/small-steps.hh"
+#endif
+
namespace hpp {
namespace manipulation {
namespace {
@@ -100,6 +107,13 @@ namespace hpp {
parent_t::add <SteeringMethodBuilder_t> ("Graph-SteeringMethodStraight",
GraphSteeringMethod::create <core::SteeringMethodStraight>);
+#ifdef HPP_MANIPULATION_HAS_WHOLEBODY_STEP
+ parent_t::add <PathOptimizerBuilder_t>
+ ("Walkgen", wholebodyStep::SmallSteps::create);
+ parent_t::add <PathOptimizerBuilder_t>
+ ("Graph-Walkgen", pathOptimization::SmallSteps::create);
+#endif
+
pathPlannerType ("M-RRT");
pathValidationType ("Graph-Discretized", 0.05);
steeringMethodType ("Graph-SteeringMethodStraight");