Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found
Select Git revision

Target

Select target project
  • gsaurel/hpp-manipulation
  • humanoid-path-planner/hpp-manipulation
2 results
Select Git revision
Show changes
Hide whitespace changes
Inline Side-by-side
Showing
with 2055 additions and 1654 deletions
include/hpp/manipulation/steering-method/fwd.hh 0 → 100644
View file @ 539aab52
//
// Copyright (c) 2017 CNRS
// Authors: Joseph Mirabel
//
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
#ifndef HPP_MANIPULATION_STEERING_METHOD_FWD_HH
#define HPP_MANIPULATION_STEERING_METHOD_FWD_HH
#include <hpp/core/fwd.hh>
#include <map>
namespace hpp {
namespace manipulation {
namespace steeringMethod {
HPP_PREDEF_CLASS(Graph);
typedef shared_ptr<Graph> GraphPtr_t;
HPP_PREDEF_CLASS(CrossStateOptimization);
typedef shared_ptr<CrossStateOptimization> CrossStateOptimizationPtr_t;
} // namespace steeringMethod
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_STEERING_METHOD_FWD_HH
include/hpp/manipulation/steering-method/graph.hh 0 → 100644
View file @ 539aab52
// Copyright (c) 2014, LAAS-CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
#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>
#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 {
/// \addtogroup steering_method
/// \{
class HPP_MANIPULATION_DLLAPI SteeringMethod : public core::SteeringMethod {
public:
const core::SteeringMethodPtr_t& innerSteeringMethod() const {
return steeringMethod_;
}
void innerSteeringMethod(const core::SteeringMethodPtr_t& sm) {
steeringMethod_ = sm;
}
protected:
/// Constructor
SteeringMethod(const ProblemConstPtr_t& problem);
/// Copy constructor
SteeringMethod(const SteeringMethod& other);
void init(SteeringMethodWkPtr_t weak) { core::SteeringMethod::init(weak); }
/// A pointer to the manipulation problem
ProblemConstPtr_t problem_;
/// The encapsulated steering method
core::SteeringMethodPtr_t steeringMethod_;
};
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::ProblemConstPtr_t& problem);
template <typename T>
static GraphPtr_t create(const core::ProblemConstPtr_t& 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 ProblemConstPtr_t& 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::ProblemConstPtr_t& problem) {
GraphPtr_t gsm = Graph::create(problem);
gsm->innerSteeringMethod(T::create(problem));
return gsm;
}
} // namespace steeringMethod
/// \}
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_STEERING_METHOD_GRAPH_HH
include/hpp/manipulation/weighed-distance.hh
View file @ 539aab52
// Copyright (c) 2015 CNRS // Copyright (c) 2015 CNRS
// Authors: Joseph Mirabel // Authors: Joseph Mirabel
// //
// This file is part of hpp-manipulation
// hpp-manipulation is free software: you can redistribute it // Redistribution and use in source and binary forms, with or without
// and/or modify it under the terms of the GNU Lesser General Public // modification, are permitted provided that the following conditions are
// License as published by the Free Software Foundation, either version // met:
// 3 of the License, or (at your option) any later version. //
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// //
// hpp-manipulation is distributed in the hope that it will be // 2. Redistributions in binary form must reproduce the above copyright
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty // notice, this list of conditions and the following disclaimer in the
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // documentation and/or other materials provided with the distribution.
// General Lesser Public License for more details. You should have //
// received a copy of the GNU Lesser General Public License along with // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// hpp-manipulation If not, see // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// <http://www.gnu.org/licenses/>. // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
#ifndef HPP_MANIPULATION_DISTANCE_HH #ifndef HPP_MANIPULATION_DISTANCE_HH
# define HPP_MANIPULATION_DISTANCE_HH #define HPP_MANIPULATION_DISTANCE_HH
# include <hpp/core/weighed-distance.hh> #include <hpp/core/weighed-distance.hh>
# include <hpp/manipulation/fwd.hh> #include <hpp/manipulation/config.hh>
# include <hpp/manipulation/graph/fwd.hh> #include <hpp/manipulation/fwd.hh>
# include <hpp/manipulation/config.hh> #include <hpp/manipulation/graph/fwd.hh>
namespace hpp { namespace hpp {
namespace manipulation { namespace manipulation {
/// \addtogroup steering_method /// \addtogroup steering_method
/// \{ /// \{
/// Class for distance between configurations
class HPP_MANIPULATION_DLLAPI WeighedDistance : public core::WeighedDistance {
public:
static WeighedDistancePtr_t create(const DevicePtr_t& robot,
const graph::GraphPtr_t& graph);
/// Class for distance between configurations static WeighedDistancePtr_t createCopy(const WeighedDistancePtr_t& distance);
class HPP_MANIPULATION_DLLAPI WeighedDistance : public core::WeighedDistance
{
public:
static WeighedDistancePtr_t create (const DevicePtr_t& robot,
const graph::GraphPtr_t& graph);
static WeighedDistancePtr_t createCopy virtual core::DistancePtr_t clone() const;
(const WeighedDistancePtr_t& distance);
virtual core::DistancePtr_t clone () const; /// Set the graph of constraints
void constraintGraph(const graph::GraphPtr_t& graph) { graph_ = graph; }
/// Set the graph of constraints /// Get the graph of constraints
void constraintGraph (const graph::GraphPtr_t& graph) graph::GraphPtr_t constraintGraph() const { return graph_; }
{
graph_ = graph;
}
/// Get the graph of constraints protected:
graph::GraphPtr_t constraintGraph () const WeighedDistance(const DevicePtr_t& robot, const graph::GraphPtr_t graph);
{
return graph_;
}
protected: WeighedDistance(const WeighedDistance& distance);
WeighedDistance (const DevicePtr_t& robot, const graph::GraphPtr_t graph);
WeighedDistance (const WeighedDistance& distance); /// Derived class should implement this function
virtual value_type impl_distance(ConfigurationIn_t q1,
ConfigurationIn_t q2) const;
virtual value_type impl_distance(core::NodePtr_t n1,
core::NodePtr_t n2) const;
/// Derived class should implement this function void init(WeighedDistanceWkPtr_t self);
virtual value_type impl_distance (
ConfigurationIn_t q1, ConfigurationIn_t q2) const;
virtual value_type impl_distance (
core::NodePtr_t n1, core::NodePtr_t n2) const;
void init (WeighedDistanceWkPtr_t self); private:
graph::GraphPtr_t graph_;
WeighedDistanceWkPtr_t weak_;
private: WeighedDistance() {}
graph::GraphPtr_t graph_; HPP_SERIALIZABLE();
WeighedDistanceWkPtr_t weak_; }; // class Distance
}; // class Distance /// \}
/// \} } // namespace manipulation
} // namespace manipulation } // namespace hpp
} // namespace hpp #endif // HPP_MANIPULATION_DISTANCE_HH
#endif // HPP_MANIPULATION_DISTANCE_HH
package.xml 0 → 100644
View file @ 539aab52
<?xml version='1.0'?>
<package format='2'>
<name>hpp-manipulation</name>
<version>6.0.0</version>
<description>Classes for manipulation planning. </description>
<maintainer email='hpp@laas.fr'>Joseph Mirabel</maintainer>
<license>LGPL</license>
<!-- <url type='website'>https://github.com/humanoid-path-planner/hpp-manipulation </url> -->
<author>Joseph Mirabel</author>
<author>Florent Lamiraux et al.</author>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>hpp-core</build_depend>
<exec_depend>hpp-core</exec_depend>
<build_export_depend>hpp-core</build_export_depend>
<build_depend>hpp-wholebody-step</build_depend>
<exec_depend>hpp-wholebody-step</exec_depend>
<build_export_depend>hpp-wholebody-step</build_export_depend>
<test_depend>example-robot-data</test_depend>
<build_depend>eigen3</build_depend>
<exec_depend>eigen3</exec_depend>
<build_export_depend>eigen3</build_export_depend>
<build_depend>urdfdom</build_depend>
<exec_depend>urdfdom</exec_depend>
<build_export_depend>urdfdom</build_export_depend>
<build_depend>coal</build_depend>
<exec_depend>coal</exec_depend>
<build_export_depend>coal</build_export_depend>
<test_depend>romeo_description</test_depend>
</package>
plugins/CMakeLists.txt 0 → 100644
View file @ 539aab52
# Copyright (c) 2019, 2020, CNRS Authors: Joseph Mirabel
# (joseph.mirabel@laas.fr) Authors: Guilhem Saurel (guilhem.saurel@laas.fr)
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# 1. Redistributions of source code must retain the above copyright notice, this
# list of conditions and the following disclaimer.
#
# 1. Redistributions in binary form must reproduce the above copyright notice,
# this list of conditions and the following disclaimer in the documentation
# and/or other materials provided with the distribution.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
include(${HPP_CORE_CMAKE_PLUGIN})
hpp_add_plugin(manipulation-spline-gradient-based SOURCES
spline-gradient-based.cc LINK_DEPENDENCIES ${PROJECT_NAME}-gpl)
hpp_add_plugin(end-effector-trajectory SOURCES end-effector-trajectory.cc
LINK_DEPENDENCIES ${PROJECT_NAME})
plugins/end-effector-trajectory.cc 0 → 100644
View file @ 539aab52
// Copyright (c) 2019, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
#include <hpp/core/plugin.hh>
#include <hpp/core/problem-solver.hh>
#include <hpp/manipulation/path-planner/end-effector-trajectory.hh>
#include <hpp/manipulation/steering-method/end-effector-trajectory.hh>
namespace hpp {
namespace manipulation {
class EndEffectorTrajectoryPlugin : public core::ProblemSolverPlugin {
public:
EndEffectorTrajectoryPlugin()
: ProblemSolverPlugin("EndEffectorTrajectoryPlugin", "0.0") {}
protected:
virtual bool impl_initialize(core::ProblemSolverPtr_t ps) {
ps->pathPlanners.add("EndEffectorTrajectory",
pathPlanner::EndEffectorTrajectory::createWithRoadmap);
ps->steeringMethods.add("EndEffectorTrajectory",
steeringMethod::EndEffectorTrajectory::create);
return true;
}
};
} // namespace manipulation
} // namespace hpp
HPP_CORE_DEFINE_PLUGIN(hpp::manipulation::EndEffectorTrajectoryPlugin)
plugins/spline-gradient-based.cc 0 → 100644
View file @ 539aab52
// Copyright (c) 2019, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
#include <hpp/core/plugin.hh>
#include <hpp/core/problem-solver.hh>
#include <hpp/manipulation/path-optimization/spline-gradient-based.hh>
namespace hpp {
namespace manipulation {
class SplineGradientBasedPlugin : public core::ProblemSolverPlugin {
public:
SplineGradientBasedPlugin()
: ProblemSolverPlugin("SplineGradientBasedPlugin", "0.0") {}
protected:
virtual bool impl_initialize(core::ProblemSolverPtr_t ps) {
// ps->pathOptimizers.add
// ("SplineGradientBased_cannonical1",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis,
// 1>::createFromCore); ps->pathOptimizers.add
// ("SplineGradientBased_cannonical2",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis,
// 2>::createFromCore); ps->pathOptimizers.add
// ("SplineGradientBased_cannonical3",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis,
// 3>::createFromCore);
ps->pathOptimizers.add(
"SplineGradientBased_bezier1",
pathOptimization::SplineGradientBased<core::path::BernsteinBasis,
1>::createFromCore);
// ps->pathOptimizers.add
// ("SplineGradientBased_bezier2",pathOptimization::SplineGradientBased<core::path::BernsteinBasis,
// 2>::createFromCore);
ps->pathOptimizers.add(
"SplineGradientBased_bezier3",
pathOptimization::SplineGradientBased<core::path::BernsteinBasis,
3>::createFromCore);
return true;
}
};
} // namespace manipulation
} // namespace hpp
HPP_CORE_DEFINE_PLUGIN(hpp::manipulation::SplineGradientBasedPlugin)
pyproject.toml 0 → 100644
View file @ 539aab52
[build-system]
build-backend = "cmeel.build"
requires = [
"cmeel-boost ~= 1.83.0",
"cmeel[build]",
"hpp-core[build]"
]
[project]
dependencies = [
"cmeel-boost ~= 1.83.0",
"hpp-core"
]
description = "Classes for manipulation planning."
license = "BSD-2-Clause"
name = "hpp-manipulation"
version = "6.0.0"
[tool.ruff]
extend-exclude = ["cmake"]
[tool.ruff.lint]
extend-select = ["I", "NPY", "RUF", "UP", "W"]
[tool.tomlsort]
all = true
src/CMakeLists.txt deleted 100644 → 0
View file @ 0e448947
#
# Copyright (c) 2014 CNRS
# Authors: Florent Lamiraux
#
#
# 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/>.
SET(LIBRARY_NAME ${PROJECT_NAME})
SET(SOURCES
axial-handle.cc
handle.cc
manipulation-planner.cc
problem-solver.cc
roadmap.cc
connected-component.cc
constraint-set.cc
roadmap-node.cc
device.cc
weighed-distance.cc
problem.cc
graph-path-validation.cc
graph-steering-method.cc
graph-optimizer.cc
graph/node.cc
graph/edge.cc
graph/graph.cc
graph/graph-component.cc
graph/node-selector.cc
graph/guided-node-selector.cc
graph/statistics.cc
graph/helper.cc
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)
src/astar.hh
View file @ 539aab52
...@@ -2,167 +2,167 @@ ...@@ -2,167 +2,167 @@
// Copyright (c) 2015 CNRS // Copyright (c) 2015 CNRS
// Authors: Florent Lamiraux, Joseph Mirabel // Authors: Florent Lamiraux, Joseph Mirabel
// //
// This file is part of hpp-manipulation.gc
// hpp-manipulation.gc is free software: you can redistribute it // Redistribution and use in source and binary forms, with or without
// and/or modify it under the terms of the GNU Lesser General Public // modification, are permitted provided that the following conditions are
// License as published by the Free Software Foundation, either version // met:
// 3 of the License, or (at your option) any later version. //
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// //
// hpp-manipulation.gc is distributed in the hope that it will be // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// General Lesser Public License for more details. You should have // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// received a copy of the GNU Lesser General Public License along with // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// hpp-manipulation.gc If not, see // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// <http://www.gnu.org/licenses/>. // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
#ifndef HPP_MANIPULATION_ASTAR_HH #ifndef HPP_MANIPULATION_ASTAR_HH
# define HPP_MANIPULATION_ASTAR_HH #define HPP_MANIPULATION_ASTAR_HH
# include <limits> #include <hpp/core/distance.hh>
# include <hpp/core/distance.hh> #include <hpp/core/edge.hh>
# include <hpp/core/node.hh> #include <hpp/core/node.hh>
# include <hpp/core/edge.hh> #include <hpp/manipulation/fwd.hh>
#include <hpp/manipulation/graph/state-selector.hh>
# include <hpp/manipulation/fwd.hh> #include <hpp/manipulation/roadmap-node.hh>
# include <hpp/manipulation/roadmap-node.hh> #include <limits>
# include <hpp/manipulation/graph/node-selector.hh> // # include <hpp/core/path-vector.hh>
//# include <hpp/core/path-vector.hh>
namespace hpp { namespace hpp {
namespace manipulation { namespace manipulation {
class Astar class Astar {
{ public:
public: typedef std::map<RoadmapNodePtr_t, value_type> CostMap_t;
typedef std::map <RoadmapNodePtr_t, value_type> CostMap_t; struct CostMapCompFunctor {
struct CostMapCompFunctor { CostMap_t& cost_;
CostMap_t& cost_; CostMapCompFunctor(CostMap_t& cost) : cost_(cost) {}
CostMapCompFunctor (CostMap_t& cost) : cost_ (cost) {} bool operator()(const RoadmapNodePtr_t& n1, const RoadmapNodePtr_t& n2) {
bool operator () (const RoadmapNodePtr_t& n1, const RoadmapNodePtr_t& n2) return cost_[n1] < cost_[n2];
{ return cost_ [n1] < cost_ [n2]; } }
bool operator () (const RoadmapNodePtr_t& n1, const value_type& val) bool operator()(const RoadmapNodePtr_t& n1, const value_type& val) {
{ return cost_ [n1] < val; } return cost_[n1] < val;
}; // struc CostMapCompFunctor }
}; // struc CostMapCompFunctor
typedef std::list <graph::NodePtr_t> Nodes_t;
typedef std::list <RoadmapNodePtr_t> RoadmapNodes_t; typedef std::list<graph::StatePtr_t> States_t;
typedef std::list <core::EdgePtr_t> RoadmapEdges_t; typedef std::list<RoadmapNodePtr_t> RoadmapNodes_t;
typedef std::map <RoadmapNodePtr_t, core::EdgePtr_t> Parent_t; typedef std::list<core::EdgePtr_t> RoadmapEdges_t;
typedef std::map<RoadmapNodePtr_t, core::EdgePtr_t> Parent_t;
Astar (const core::DistancePtr_t distance,
const graph::NodeSelectorPtr_t& nodeSelector, RoadmapNodePtr_t from) : Astar(const core::DistancePtr_t distance,
distance_ (distance), selector_ (nodeSelector), const graph::StateSelectorPtr_t& stateSelector, RoadmapNodePtr_t from)
from_ (from) : distance_(distance), selector_(stateSelector), from_(from) {
{ open_.push_back(from);
open_.push_back (from); costFromStart_[from] = 0;
costFromStart_ [from] = 0; }
States_t solution(RoadmapNodePtr_t to) {
if (parent_.find(to) != parent_.end() || findPath(to)) {
RoadmapNodePtr_t node = to;
States_t states;
states.push_front(selector_->getState(to));
while (node) {
Parent_t::const_iterator itNode = parent_.find(node);
if (itNode != parent_.end()) {
node = static_cast<RoadmapNodePtr_t>(itNode->second->from());
states.push_front(selector_->getState(node));
} else
node = RoadmapNodePtr_t(0);
} }
// We may want to clean it a little
Nodes_t solution (RoadmapNodePtr_t to) // std::unique (states.begin(), states.end ());
{
if (parent_.find (to) != parent_.end () || states.push_front(selector_->getState(from_));
findPath (to)) return states;
{ }
RoadmapNodePtr_t node = to; return States_t();
Nodes_t nodes; }
nodes.push_front (selector_->getNode (to)); private:
while (node) { bool findPath(RoadmapNodePtr_t to) {
Parent_t::const_iterator itNode = parent_.find (node); // Recompute the estimated cost to goal
if (itNode != parent_.end ()) { for (CostMap_t::iterator it = estimatedCostToGoal_.begin();
node = static_cast <RoadmapNodePtr_t> (itNode->second->from ()); it != estimatedCostToGoal_.end(); ++it) {
nodes.push_front (selector_->getNode (node)); it->second = getCostFromStart(it->first) + heuristic(it->first, to);
}
open_.sort(CostMapCompFunctor(estimatedCostToGoal_));
while (!open_.empty()) {
RoadmapNodes_t::iterator itv = open_.begin();
RoadmapNodePtr_t current(*itv);
if (current == to) {
return true;
}
open_.erase(itv);
closed_.push_back(current);
for (RoadmapEdges_t::const_iterator itEdge = current->outEdges().begin();
itEdge != current->outEdges().end(); ++itEdge) {
RoadmapNodePtr_t child = static_cast<RoadmapNodePtr_t>((*itEdge)->to());
if (std::find(closed_.begin(), closed_.end(), child) == closed_.end()) {
// node is not in closed set
value_type transitionCost = edgeCost(*itEdge);
value_type tmpCost = getCostFromStart(current) + transitionCost;
bool childNotInOpenSet =
(std::find(open_.begin(), open_.end(), child) == open_.end());
if ((childNotInOpenSet) || (tmpCost < getCostFromStart(child))) {
parent_[child] = *itEdge;
costFromStart_[child] = tmpCost;
value_type estimatedCost = tmpCost + heuristic(child, to);
estimatedCostToGoal_[child] = estimatedCost;
if (childNotInOpenSet) {
// Find the first element not strictly smaller than child
RoadmapNodes_t::iterator pos =
std::lower_bound(open_.begin(), open_.end(), estimatedCost,
CostMapCompFunctor(estimatedCostToGoal_));
open_.insert(pos, child);
} }
else node = RoadmapNodePtr_t (0);
} }
// We may want to clean it a little
// std::unique (nodes.begin(), nodes.end ());
nodes.push_front (selector_->getNode (from_));
return nodes;
}
return Nodes_t();
}
private:
bool findPath (RoadmapNodePtr_t to)
{
// Recompute the estimated cost to goal
for (CostMap_t::iterator it = estimatedCostToGoal_.begin ();
it != estimatedCostToGoal_.end (); ++it) {
it->second = getCostFromStart (it->first) + heuristic (it->first, to);
} }
open_.sort (CostMapCompFunctor (estimatedCostToGoal_));
while (!open_.empty ()) {
RoadmapNodes_t::iterator itv = open_.begin ();
RoadmapNodePtr_t current (*itv);
if (current == to) {
return true;
}
open_.erase (itv);
closed_.push_back (current);
for (RoadmapEdges_t::const_iterator itEdge = current->outEdges ().begin ();
itEdge != current->outEdges ().end (); ++itEdge) {
RoadmapNodePtr_t child = static_cast <RoadmapNodePtr_t> ((*itEdge)->to ());
if (std::find (closed_.begin(), closed_.end(),
child) == closed_.end ()) {
// node is not in closed set
value_type transitionCost = edgeCost (*itEdge);
value_type tmpCost = getCostFromStart (current) + transitionCost;
bool childNotInOpenSet = (std::find (open_.begin (),
open_.end (),
child) == open_.end ());
if ((childNotInOpenSet) || (tmpCost < getCostFromStart (child))) {
parent_ [child] = *itEdge;
costFromStart_ [child] = tmpCost;
value_type estimatedCost = tmpCost + heuristic (child, to);
estimatedCostToGoal_ [child] = estimatedCost;
if (childNotInOpenSet) {
// Find the first element not strictly smaller than child
RoadmapNodes_t::iterator pos =
std::lower_bound (open_.begin (), open_.end (),
estimatedCost, CostMapCompFunctor (estimatedCostToGoal_));
open_.insert (pos, child);
}
}
}
}
}
return false;
} }
}
inline value_type heuristic (RoadmapNodePtr_t node, RoadmapNodePtr_t to) const return false;
{ }
const ConfigurationPtr_t& config = node->configuration ();
return (*distance_) (*config, *to->configuration ()); inline value_type heuristic(RoadmapNodePtr_t node,
} RoadmapNodePtr_t to) const {
const Configuration_t& config = node->configuration();
inline value_type edgeCost (const core::EdgePtr_t& edge) const return (*distance_)(config, to->configuration());
{ }
return edge->path ()->length ();
} inline value_type edgeCost(const core::EdgePtr_t& edge) const {
return edge->path()->length();
value_type getCostFromStart (RoadmapNodePtr_t to) const }
{
CostMap_t::const_iterator it = costFromStart_.find (to); value_type getCostFromStart(RoadmapNodePtr_t to) const {
if (it == costFromStart_.end()) CostMap_t::const_iterator it = costFromStart_.find(to);
return std::numeric_limits <value_type>::max(); if (it == costFromStart_.end())
return it->second; return std::numeric_limits<value_type>::max();
} return it->second;
}
RoadmapNodes_t closed_;
RoadmapNodes_t open_; RoadmapNodes_t closed_;
std::map <RoadmapNodePtr_t, value_type> costFromStart_; RoadmapNodes_t open_;
std::map <RoadmapNodePtr_t, value_type> estimatedCostToGoal_; std::map<RoadmapNodePtr_t, value_type> costFromStart_;
Parent_t parent_; std::map<RoadmapNodePtr_t, value_type> estimatedCostToGoal_;
core::DistancePtr_t distance_; Parent_t parent_;
graph::NodeSelectorPtr_t selector_; core::DistancePtr_t distance_;
RoadmapNodePtr_t from_; graph::StateSelectorPtr_t selector_;
RoadmapNodePtr_t from_;
}; // class Astar
} // namespace manipulation }; // class Astar
} // namespace hpp } // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_ASTAR_HH #endif // HPP_MANIPULATION_ASTAR_HH
src/axial-handle.cc deleted 100644 → 0
View file @ 0e448947
///
/// Copyright (c) 2014 CNRS
/// Authors: Florent Lamiraux
///
///
// 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/axial-handle.hh>
#include <boost/assign/list_of.hpp>
#include <hpp/fcl/math/transform.h>
#include <hpp/model/joint.hh>
#include <hpp/model/gripper.hh>
#include <hpp/constraints/relative-transformation.hh>
#include <hpp/core/numerical-constraint.hh>
namespace hpp {
namespace manipulation {
NumericalConstraintPtr_t AxialHandle::createGrasp
(const GripperPtr_t& gripper) const
{
using boost::assign::list_of;
std::vector <bool> mask = list_of (true)(true)(true)(true)(true)(false);
return NumericalConstraintPtr_t
(NumericalConstraint::create (RelativeTransformation::create
("Transformation_(1,1,1,1,1,0)_" + name ()
+ "_" + gripper->name (),
gripper->joint()->robot(),
gripper->joint (), joint (),
gripper->objectPositionInJoint (),
localPosition(), mask)));
}
NumericalConstraintPtr_t AxialHandle::createGraspComplement
(const GripperPtr_t& gripper) const
{
using boost::assign::list_of;
std::vector <bool> mask = list_of (false)(false)(false)(false)(false)
(true);
return NumericalConstraintPtr_t
(NumericalConstraint::create (RelativeTransformation::create
("Transformation_(0,0,0,0,0,1)_" + name ()
+ "_" + gripper->name (),
gripper->joint()->robot(),
gripper->joint (), joint (),
gripper->objectPositionInJoint (),
localPosition(), mask)));
}
NumericalConstraintPtr_t AxialHandle::createPreGrasp
(const GripperPtr_t& gripper) const
{
using boost::assign::list_of;
std::vector <bool> mask = list_of (false)(true)(true)(true)(true)(false);
return NumericalConstraintPtr_t
(NumericalConstraint::create (RelativeTransformation::create
("Transformation_(0,1,1,1,1,0)_" + name ()
+ "_" + gripper->name (),
gripper->joint()->robot(),
gripper->joint (), joint (),
gripper->objectPositionInJoint (),
localPosition(), mask)));
}
NumericalConstraintPtr_t AxialHandle::createPreGraspComplement
(const GripperPtr_t& gripper, const value_type& shift,
const value_type& width) const
{
using boost::assign::list_of;
using core::DoubleInequality;
std::vector <bool> mask = list_of (true)(false)(false)(false)(false)
(false);
Transform3f transform = gripper->objectPositionInJoint ()
* Transform3f (fcl::Vec3f (shift,0,0));
return NumericalConstraintPtr_t
(NumericalConstraint::create (RelativeTransformation::create
("Transformation_(1,0,0,0,0,0)_" + name ()
+ "_" + gripper->name (),
gripper->joint()->robot(),
gripper->joint (), joint (),
transform, localPosition(), mask),
DoubleInequality::create (width)));
}
HandlePtr_t AxialHandle::clone () const
{
AxialHandlePtr_t self = weakPtr_.lock ();
return AxialHandle::create (self->name (), self->localPosition (),
self->joint ());
}
std::ostream& AxialHandle::print (std::ostream& os) const
{
os << "name :" << name () << " (axial)" << std::endl;
os << "local position :" << localPosition () << std::endl;
os << "joint :" << joint ()->name () << std::endl;
return os;
}
} // namespace manipulation
} // namespace hpp
src/connected-component.cc
View file @ 539aab52
...@@ -2,89 +2,111 @@ ...@@ -2,89 +2,111 @@
// Copyright (c) 2015 CNRS // Copyright (c) 2015 CNRS
// Authors: Anna Seppala (seppala@laas.fr) // Authors: Anna Seppala (seppala@laas.fr)
// //
// This file is part of hpp-core
// hpp-core is free software: you can redistribute it // Redistribution and use in source and binary forms, with or without
// and/or modify it under the terms of the GNU Lesser General Public // modification, are permitted provided that the following conditions are
// License as published by the Free Software Foundation, either version // met:
// 3 of the License, or (at your option) any later version. //
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// //
// hpp-core is distributed in the hope that it will be // 2. Redistributions in binary form must reproduce the above copyright
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty // notice, this list of conditions and the following disclaimer in the
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // documentation and/or other materials provided with the distribution.
// General Lesser Public License for more details. You should have //
// received a copy of the GNU Lesser General Public License along with // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// hpp-core If not, see // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// <http://www.gnu.org/licenses/>. // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
#include <hpp/manipulation/connected-component.hh> #include <hpp/manipulation/connected-component.hh>
#include "hpp/manipulation/roadmap.hh"
#include "hpp/manipulation/roadmap-node.hh" #include "hpp/manipulation/roadmap-node.hh"
#include "hpp/manipulation/roadmap.hh"
namespace hpp { namespace hpp {
namespace manipulation { namespace manipulation {
RoadmapNodes_t ConnectedComponent::empty_ = RoadmapNodes_t();
ConnectedComponentPtr_t ConnectedComponent::create(const RoadmapWkPtr_t& roadmap) bool ConnectedComponent::check() const {
{ std::set<core::NodePtr_t> s1;
ConnectedComponent* ptr = new ConnectedComponent (); for (core::NodeVector_t::const_iterator it = nodes().begin();
ConnectedComponentPtr_t shPtr (ptr); it != nodes().end(); ++it) {
// calls init function in core::ConnectedComponent that saves s1.insert(*it);
// shPtr into the class variable weak_ (weak pointer). Reimplement? }
ptr->init (shPtr); std::set<core::NodePtr_t> s2;
shPtr->roadmap_ = roadmap.lock(); for (GraphStates_t::const_iterator it = graphStateMap_.begin();
return shPtr; it != graphStateMap_.end(); ++it) {
for (RoadmapNodes_t::const_iterator itNodes = it->second.begin();
itNodes != it->second.end(); ++itNodes) {
s2.insert(*itNodes);
} }
}
if (s1.size() == 0) return false;
if (s1 == s2) return true;
return false;
}
void ConnectedComponent::merge (const core::ConnectedComponentPtr_t& otherCC) ConnectedComponentPtr_t ConnectedComponent::create(
{ const RoadmapWkPtr_t& roadmap) {
core::ConnectedComponent::merge(otherCC); ConnectedComponent* ptr = new ConnectedComponent();
const ConnectedComponentPtr_t other = boost::static_pointer_cast <ConnectedComponent> (otherCC); ConnectedComponentPtr_t shPtr(ptr);
/// take all graph nodes in other->graphNodeMap_ and put them in this->graphNodeMap_ // calls init function in core::ConnectedComponent that saves
/// if they already exist in this->graphNodeMap_, append roadmap nodes from other graph node // shPtr into the class variable weak_ (weak pointer). Reimplement?
/// to graph node in this. ptr->init(shPtr);
for (GraphNodes_t::iterator otherIt = other->graphNodeMap_.begin(); shPtr->roadmap_ = roadmap.lock();
otherIt != other->graphNodeMap_.end(); otherIt++) return shPtr;
{ }
// find other graph node in this-graphNodeMap_ -> merge their roadmap nodes
GraphNodes_t::iterator mapIt = this->graphNodeMap_.find(otherIt->first);
if (mapIt != this->graphNodeMap_.end()) {
mapIt->second.insert(mapIt->second.end(), otherIt->second.begin(), otherIt->second.end());
} else {
this->graphNodeMap_.insert(*otherIt);
}
}
other->graphNodeMap_.clear();
}
void ConnectedComponent::addNode(const core::NodePtr_t& node)
{
core::ConnectedComponent::addNode(node);
// Find right graph node in map and add roadmap node to corresponding vector
const RoadmapNodePtr_t& n = static_cast <const RoadmapNodePtr_t> (node);
GraphNodes_t::iterator mapIt = graphNodeMap_.find(roadmap_->getNode(n));
if (mapIt != graphNodeMap_.end()) {
mapIt->second.push_back(n);
// if graph node not found, add new map element with one roadmap node
} else {
RoadmapNodes_t newRoadmapNodeVector;
newRoadmapNodeVector.push_back(n);
graphNodeMap_.insert(std::pair<graph::NodePtr_t, RoadmapNodes_t>
(roadmap_->getNode(n), newRoadmapNodeVector));
}
}
ConnectedComponent::RoadmapNodes_t ConnectedComponent::getRoadmapNodes (const graph::NodePtr_t graphNode) void ConnectedComponent::merge(const core::ConnectedComponentPtr_t& otherCC) {
{ core::ConnectedComponent::merge(otherCC);
RoadmapNodes_t res; const ConnectedComponentPtr_t other =
GraphNodes_t::iterator mapIt = graphNodeMap_.find(graphNode); static_pointer_cast<ConnectedComponent>(otherCC);
if (mapIt != graphNodeMap_.end()) { /// take all graph states in other->graphStateMap_ and put them in
res = mapIt->second; /// this->graphStateMap_ if they already exist in this->graphStateMap_, append
} /// roadmap nodes from other graph state to graph state in this.
return res; for (GraphStates_t::iterator otherIt = other->graphStateMap_.begin();
otherIt != other->graphStateMap_.end(); otherIt++) {
// find other graph state in this-graphStateMap_ -> merge their roadmap
// nodes
GraphStates_t::iterator mapIt = this->graphStateMap_.find(otherIt->first);
if (mapIt != this->graphStateMap_.end()) {
mapIt->second.insert(mapIt->second.end(), otherIt->second.begin(),
otherIt->second.end());
} else {
this->graphStateMap_.insert(*otherIt);
} }
}
other->graphStateMap_.clear();
assert(check());
}
void ConnectedComponent::addNode(const core::NodePtr_t& node) {
core::ConnectedComponent::addNode(node);
// Find right graph state in map and add roadmap node to corresponding vector
const RoadmapNodePtr_t& n = static_cast<RoadmapNodePtr_t>(node);
RoadmapPtr_t roadmap = roadmap_.lock();
if (!roadmap)
throw std::logic_error(
"The roadmap of this ConnectedComponent as been deleted.");
graphStateMap_[roadmap->getState(n)].push_back(n);
assert(check());
}
} // namespace manipulation const RoadmapNodes_t& ConnectedComponent::getRoadmapNodes(
} // namespace hpp const graph::StatePtr_t graphState) const {
GraphStates_t::const_iterator mapIt = graphStateMap_.find(graphState);
if (mapIt != graphStateMap_.end()) return mapIt->second;
return empty_;
}
} // namespace manipulation
} // namespace hpp
src/constraint-set.cc
View file @ 539aab52
// Copyright (c) 2015, Joseph Mirabel // Copyright (c) 2015, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr) // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
// //
// This file is part of hpp-manipulation.
// hpp-manipulation is free software: you can redistribute it // Redistribution and use in source and binary forms, with or without
// and/or modify it under the terms of the GNU Lesser General Public // modification, are permitted provided that the following conditions are
// License as published by the Free Software Foundation, either version // met:
// 3 of the License, or (at your option) any later version. //
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// //
// hpp-manipulation is distributed in the hope that it will be // 2. Redistributions in binary form must reproduce the above copyright
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty // notice, this list of conditions and the following disclaimer in the
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // documentation and/or other materials provided with the distribution.
// General Lesser Public License for more details. You should have //
// received a copy of the GNU Lesser General Public License along with // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>. // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
#include "hpp/manipulation/constraint-set.hh" #include "hpp/manipulation/constraint-set.hh"
#include <hpp/util/serialization.hh>
#include "hpp/manipulation/device.hh" #include "hpp/manipulation/device.hh"
#include "hpp/manipulation/graph/edge.hh" #include "hpp/manipulation/graph/edge.hh"
#include "hpp/manipulation/serialization.hh"
namespace hpp { namespace hpp {
namespace manipulation { namespace manipulation {
ConstraintSetPtr_t ConstraintSet::create ConstraintSetPtr_t ConstraintSet::create(const DevicePtr_t& robot,
(const DevicePtr_t& robot, const std::string& name) const std::string& name) {
{ ConstraintSet* ptr = new ConstraintSet(robot, name);
ConstraintSet* ptr = new ConstraintSet (robot, name); ConstraintSetPtr_t shPtr(ptr);
ConstraintSetPtr_t shPtr (ptr); ptr->init(shPtr);
ptr->init (shPtr); return shPtr;
return shPtr; }
}
ConstraintSetPtr_t ConstraintSet::createCopy(const ConstraintSetPtr_t& cs) {
ConstraintSetPtr_t ConstraintSet::createCopy (const ConstraintSetPtr_t& cs) ConstraintSet* ptr = new ConstraintSet(*cs);
{ ConstraintSetPtr_t shPtr(ptr);
ConstraintSet* ptr = new ConstraintSet (*cs); ptr->init(shPtr);
ConstraintSetPtr_t shPtr (ptr); return shPtr;
ptr->init (shPtr); }
return shPtr;
} ConstraintPtr_t ConstraintSet::copy() const { return createCopy(weak_.lock()); }
ConstraintPtr_t ConstraintSet::copy () const void ConstraintSet::edge(graph::EdgePtr_t edge) { edge_ = edge; }
{
return createCopy (weak_.lock ()); graph::EdgePtr_t ConstraintSet::edge() const { return edge_; }
}
ConstraintSet::ConstraintSet(const DevicePtr_t& robot, const std::string& name)
void ConstraintSet::edge (graph::EdgePtr_t edge) : Parent_t(robot, name), edge_() {}
{
edge_ = edge; ConstraintSet::ConstraintSet(const ConstraintSet& other)
} : Parent_t(other), edge_(other.edge()) {}
graph::EdgePtr_t ConstraintSet::edge () const void ConstraintSet::init(const ConstraintSetPtr_t& self) {
{ Parent_t::init(self);
return edge_; weak_ = self;
} }
ConstraintSet::ConstraintSet (const DevicePtr_t& robot, const std::string& name) : std::ostream& ConstraintSet::print(std::ostream& os) const {
Parent_t (robot, name), Parent_t::print(os);
edge_ () if (edge_) os << iendl << "Built by edge " << edge_->name();
{} return os;
}
ConstraintSet::ConstraintSet (const ConstraintSet& other) :
Parent_t (other), template <class Archive>
edge_ (other.edge ()) void ConstraintSet::serialize(Archive& ar, const unsigned int version) {
{} using namespace boost::serialization;
(void)version;
void ConstraintSet::init (const ConstraintSetPtr_t& self) ar& make_nvp("base", base_object<core::ConstraintSet>(*this));
{ ar& BOOST_SERIALIZATION_NVP(edge_);
Parent_t::init (self); ar& BOOST_SERIALIZATION_NVP(weak_);
weak_ = self; }
}
HPP_SERIALIZATION_IMPLEMENT(ConstraintSet);
std::ostream& ConstraintSet::print (std::ostream& os) const } // namespace manipulation
{ } // namespace hpp
Parent_t::print (os);
if (edge_) os << "Built by edge " << edge_->name() << std::endl; BOOST_CLASS_EXPORT_IMPLEMENT(hpp::manipulation::ConstraintSet)
return os;
}
} // namespace manipulation
} // namespace hpp
src/device.cc
View file @ 539aab52
...@@ -3,67 +3,173 @@ ...@@ -3,67 +3,173 @@
/// Authors: Joseph Mirabel /// Authors: Joseph Mirabel
/// ///
/// ///
// This file is part of hpp-manipulation.
// hpp-manipulation is free software: you can redistribute it // Redistribution and use in source and binary forms, with or without
// and/or modify it under the terms of the GNU Lesser General Public // modification, are permitted provided that the following conditions are
// License as published by the Free Software Foundation, either version // met:
// 3 of the License, or (at your option) any later version. //
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// //
// hpp-manipulation is distributed in the hope that it will be // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// General Lesser Public License for more details. You should have // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// received a copy of the GNU Lesser General Public License along with // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// hpp-manipulation. If not, see // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// <http://www.gnu.org/licenses/>. // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
#include <hpp/model/gripper.hh> // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
#include <boost/serialization/weak_ptr.hpp>
#include <hpp/manipulation/device.hh> #include <hpp/manipulation/device.hh>
#include <hpp/manipulation/handle.hh> #include <hpp/manipulation/handle.hh>
#include <hpp/manipulation/serialization.hh>
#include <hpp/model/joint.hh> #include <hpp/pinocchio/gripper.hh>
#include <hpp/pinocchio/joint-collection.hh>
#include <hpp/pinocchio/joint.hh>
#include <hpp/util/serialization.hh>
#include <pinocchio/multibody/geometry.hpp>
#include <pinocchio/multibody/model.hpp>
namespace hpp { namespace hpp {
namespace manipulation { namespace manipulation {
void Device::didInsertRobot (const std::string& name) using ::pinocchio::Frame;
{
if (!didPrepare_) { pinocchio::DevicePtr_t Device::clone() const {
hppDout (error, "You must call prepareInsertRobot before."); Device* ptr = new Device(*this);
} DevicePtr_t shPtr(ptr);
didPrepare_ = false; ptr->initCopy(shPtr, *this);
/// Build list of new joints return shPtr;
const model::JointVector_t jv = getJointVector (); }
model::JointVector_t newj;
newj.reserve (jv.size () - jointCache_.size ()); void Device::setRobotRootPosition(const std::string& rn, const Transform3s& t) {
model::JointVector_t::const_iterator retFind, it1, it2; FrameIndices_t idxs = robotFrames(rn);
for (it1 = jv.begin (); it1 != jv.end (); ++it1) { if (idxs.size() == 0)
retFind = find (jointCache_.begin (), jointCache_.end (), *it1); throw std::invalid_argument("No frame for robot name " + rn);
if (retFind == jointCache_.end ())
newj.push_back (*it1); pinocchio::Model& m = model();
} pinocchio::GeomModel& gm = geomModel();
/// Add collision between old joints and new ones. // The root frame should be the first frame.
for (it1 = newj.begin (); it1 != newj.end (); ++it1) { Frame& rootFrame = m.frames[idxs[0]];
if (!(*it1)->linkedBody ()) continue; if (rootFrame.type == ::pinocchio::JOINT) {
for (it2 = jointCache_.begin (); it2 != jointCache_.end (); ++it2) { JointIndex jid = m.getJointId(rootFrame.name);
if (!(*it2)->linkedBody ()) continue; m.jointPlacements[jid] = t;
addCollisionPairs (*it1, *it2, model::COLLISION); return;
addCollisionPairs (*it1, *it2, model::DISTANCE); }
}
} Transform3s shift(t * rootFrame.placement.inverse());
jointCache_.clear (); // Find all the frames that have the same parent joint.
add (name, newj); for (std::size_t i = 1; i < idxs.size(); ++i) {
Frame& frame = m.frames[idxs[i]];
if (frame.parent == rootFrame.parent) {
// frame is between rootFrame and next moving joints.
frame.placement = shift * frame.placement;
if (frame.type == ::pinocchio::BODY) {
// Update the geometry object placement.
for (std::size_t k = 0; k < gm.geometryObjects.size(); ++k) {
::pinocchio::GeometryObject& go = gm.geometryObjects[k];
if (go.parentFrame == idxs[i]) go.placement = shift * go.placement;
} }
std::ostream& Device::print (std::ostream& os) const }
{ } else if ((frame.type == ::pinocchio::JOINT) &&
Parent_t::print (os); (rootFrame.parent == m.parents[frame.parent])) {
// print handles // frame corresponds to a child joint of rootFrame.parent
os << "Handles:" << std::endl; m.jointPlacements[frame.parent] = shift * m.jointPlacements[frame.parent];
Containers_t::print <HandlePtr_t> (os); }
// print grippers }
os << "Grippers:" << std::endl; invalidate();
Containers_t::print <model::GripperPtr_t> (os); // Update the pool of device data.
return os; numberDeviceData(numberDeviceData());
}
std::vector<std::string> Device::robotNames() const {
const pinocchio::Model& model = this->model();
std::vector<std::string> names;
for (pinocchio::FrameIndex fi = 1; fi < model.frames.size(); ++fi) {
const Frame& frame = model.frames[fi];
std::size_t sep = frame.name.find('/');
if (sep == std::string::npos) {
hppDout(warning,
"Frame " << frame.name << " does not belong to any robots.");
continue;
} }
std::string name = frame.name.substr(0, sep);
if (std::find(names.rbegin(), names.rend(), name) != names.rend())
names.push_back(name);
}
return names;
}
FrameIndices_t Device::robotFrames(const std::string& robotName) const {
const pinocchio::Model& model = this->model();
FrameIndices_t frameIndices;
for (pinocchio::FrameIndex fi = 1; fi < model.frames.size(); ++fi) {
const std::string& name = model.frames[fi].name;
if (name.size() > robotName.size() &&
name.compare(0, robotName.size(), robotName) == 0 &&
name[robotName.size()] == '/') {
frameIndices.push_back(fi);
}
}
return frameIndices;
}
void Device::removeJoints(const std::vector<std::string>& jointNames,
Configuration_t referenceConfig) {
Parent_t::removeJoints(jointNames, referenceConfig);
for (auto& pair : grippers.map)
pair.second = pinocchio::Gripper::create(pair.second->name(), self_);
// TODO update handles and jointAndShapes
}
std::ostream& Device::print(std::ostream& os) const {
Parent_t::print(os);
// print handles
os << "Handles:" << std::endl;
handles.print(os);
// print grippers
os << "Grippers:" << std::endl;
grippers.print(os);
return os;
}
template <class Archive>
void Device::serialize(Archive& ar, const unsigned int version) {
using namespace boost::serialization;
(void)version;
auto* har = hpp::serialization::cast(&ar);
ar& make_nvp("base", base_object<pinocchio::HumanoidRobot>(*this));
// TODO we should throw if a pinocchio::Device instance with name name_
// and not of type manipulation::Device is found.
bool written =
(!har || har->template getChildClass<pinocchio::Device, Device>(
name_, false) != this);
ar& BOOST_SERIALIZATION_NVP(written);
if (written) {
ar& BOOST_SERIALIZATION_NVP(self_);
// TODO (easy) add serialization of core::Container ?
// ar & BOOST_SERIALIZATION_NVP(handles);
// ar & BOOST_SERIALIZATION_NVP(grippers);
// ar & BOOST_SERIALIZATION_NVP(jointAndShapes);
}
}
HPP_SERIALIZATION_IMPLEMENT(Device);
} // namespace manipulation
} // namespace hpp
} // namespace manipulation BOOST_CLASS_EXPORT_IMPLEMENT(hpp::manipulation::Device)
} // namespace hpp
src/graph-node-optimizer.cc
View file @ 539aab52
// Copyright (c) 2015, Joseph Mirabel // Copyright (c) 2015, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr) // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
// //
// This file is part of hpp-manipulation.
// hpp-manipulation is free software: you can redistribute it // Redistribution and use in source and binary forms, with or without
// and/or modify it under the terms of the GNU Lesser General Public // modification, are permitted provided that the following conditions are
// License as published by the Free Software Foundation, either version // met:
// 3 of the License, or (at your option) any later version. //
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// //
// hpp-manipulation is distributed in the hope that it will be // 2. Redistributions in binary form must reproduce the above copyright
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty // notice, this list of conditions and the following disclaimer in the
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // documentation and/or other materials provided with the distribution.
// General Lesser Public License for more details. You should have //
// received a copy of the GNU Lesser General Public License along with // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>. // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
#include <hpp/core/steering-method/straight.hh>
#include <hpp/manipulation/graph-node-optimizer.hh> #include <hpp/manipulation/graph-node-optimizer.hh>
#include <hpp/core/steering-method-straight.hh>
namespace hpp { namespace hpp {
namespace manipulation { namespace manipulation {
GraphNodeOptimizerPtr_t GraphNodeOptimizer::create GraphNodeOptimizerPtr_t GraphNodeOptimizer::create(
(const core::Problem& problem) const core::ProblemConstPtr_t& problem) {
{ GraphNodeOptimizer* ptr = new GraphNodeOptimizer(problem);
GraphNodeOptimizer* ptr = new GraphNodeOptimizer (problem); return GraphNodeOptimizerPtr_t(ptr);
return GraphNodeOptimizerPtr_t (ptr); }
}
PathVectorPtr_t GraphNodeOptimizer::optimize (const PathVectorPtr_t& path) PathVectorPtr_t GraphNodeOptimizer::optimize(const PathVectorPtr_t& path) {
{ core::ProblemPtr_t p = const_cast<core::ProblemPtr_t>(this->problem());
core::Problem& p = const_cast <core::Problem&> (this->problem ()); core::SteeringMethodPtr_t sm = p.steeringMethod();
core::SteeringMethodPtr_t sm = p.steeringMethod ();
/// Start by flattening the path /// Start by flattening the path
PathVectorPtr_t flat = PathVector::create PathVectorPtr_t flat = PathVector::create(path->outputSize(),
(path->outputSize(), path->outputDerivativeSize()), path->outputDerivativeSize()),
path->flatten (flat); path->flatten(flat);
PathVectorPtr_t opted = PathVector::create PathVectorPtr_t opted = PathVector::create(path->outputSize(),
(path->outputSize(), path->outputDerivativeSize()), path->outputDerivativeSize()),
toConcat; toConcat;
ConstraintSetPtr_t c; ConstraintSetPtr_t c;
for (std::size_t i_s = 0; i_s < flat->numberPaths ();) { for (std::size_t i_s = 0; i_s < flat->numberPaths();) {
PathPtr_t p = flat->pathAtRank (i_s); PathPtr_t p = flat->pathAtRank(i_s);
PathPtr_t newp = (*sm) (p->initial (), p->end ()); PathPtr_t newp = (*sm)(p->initial(), p->end());
if (!newp) if (!newp)
throw std::runtime_error ("It should not be a problem to recompute " throw std::runtime_error(
"a path..."); "It should not be a problem to recompute "
opted->appendPath (newp); "a path...");
} opted->appendPath(newp);
return opted; }
} return opted;
} // namespace manipulation }
} // namespace hpp } // namespace manipulation
} // namespace hpp
src/graph-optimizer.cc
View file @ 539aab52
// Copyright (c) 2015, Joseph Mirabel // Copyright (c) 2015, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr) // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
// //
// This file is part of hpp-manipulation.
// hpp-manipulation is free software: you can redistribute it // Redistribution and use in source and binary forms, with or without
// and/or modify it under the terms of the GNU Lesser General Public // modification, are permitted provided that the following conditions are
// License as published by the Free Software Foundation, either version // met:
// 3 of the License, or (at your option) any later version. //
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// //
// hpp-manipulation is distributed in the hope that it will be // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// General Lesser Public License for more details. You should have // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// received a copy of the GNU Lesser General Public License along with // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>. // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
#include <hpp/core/config-projector.hh>
#include <hpp/core/path-vector.hh>
#include <hpp/core/path.hh>
#include <hpp/core/problem.hh>
#include <hpp/manipulation/constraint-set.hh>
#include <hpp/manipulation/graph-optimizer.hh> #include <hpp/manipulation/graph-optimizer.hh>
#include <hpp/manipulation/graph/edge.hh>
#include <hpp/manipulation/graph-path-validation.hh> #include <hpp/manipulation/graph-path-validation.hh>
#include <hpp/manipulation/graph/edge.hh>
namespace hpp { namespace hpp {
namespace manipulation { namespace manipulation {
PathVectorPtr_t GraphOptimizer::optimize (const PathVectorPtr_t& path) PathVectorPtr_t GraphOptimizer::optimize(const PathVectorPtr_t& path) {
{ PathVectorPtr_t opted = PathVector::create(path->outputSize(),
PathVectorPtr_t opted = PathVector::create path->outputDerivativeSize()),
(path->outputSize(), path->outputDerivativeSize()), expanded = PathVector::create(path->outputSize(),
expanded = PathVector::create path->outputDerivativeSize()),
(path->outputSize(), path->outputDerivativeSize()), toConcat;
toConcat; GraphPathValidationPtr_t gpv = HPP_DYNAMIC_PTR_CAST(
GraphPathValidationPtr_t gpv = HPP_DYNAMIC_PTR_CAST (GraphPathValidation, GraphPathValidation, this->problem()->pathValidation());
this->problem().pathValidation ());
const_cast <core::Problem&>(this->problem ()).pathValidation (gpv->innerValidation()); path->flatten(expanded);
path->flatten (expanded); ConstraintSetPtr_t c;
ConstraintSetPtr_t c; for (std::size_t i_s = 0; i_s < expanded->numberPaths();) {
for (std::size_t i_s = 0; i_s < expanded->numberPaths ();) { PathVectorPtr_t toOpt =
PathVectorPtr_t toOpt = PathVector::create ( PathVector::create(path->outputSize(), path->outputDerivativeSize());
path->outputSize(), path->outputDerivativeSize()); PathPtr_t current = expanded->pathAtRank(i_s);
PathPtr_t current = expanded->pathAtRank (i_s); toOpt->appendPath(current);
toOpt->appendPath (current); graph::EdgePtr_t edge;
graph::EdgePtr_t edge; c = HPP_DYNAMIC_PTR_CAST(ConstraintSet, current->constraints());
c = HPP_DYNAMIC_PTR_CAST (ConstraintSet, current->constraints ()); if (c) edge = c->edge();
if (c) edge = c->edge (); bool isShort = edge && edge->isShort();
std::size_t i_e = i_s + 1; std::size_t i_e = i_s + 1;
for (; i_e < expanded->numberPaths (); ++i_e) { for (; i_e < expanded->numberPaths(); ++i_e) {
current = expanded->pathAtRank (i_e); current = expanded->pathAtRank(i_e);
c = HPP_DYNAMIC_PTR_CAST (ConstraintSet, current->constraints ()); c = HPP_DYNAMIC_PTR_CAST(ConstraintSet, current->constraints());
if (!c && edge) break; if (!c && edge) {
if (c && edge->node() != c->edge ()->node()) break; hppDout(info, "No manipulation::ConstraintSet");
toOpt->appendPath (current); break;
} }
pathOptimizer_ = factory_ (this->problem ()); if (c && edge->state() != c->edge()->state()) break;
toConcat = pathOptimizer_->optimize (toOpt); if (isShort !=
i_s = i_e; c->edge()->isShort()) // We do not optimize edges marked as short
opted->concatenate (*toConcat); break;
toOpt->appendPath(current);
}
hppDout(info, "Edge name: " << edge->name());
if (isShort)
toConcat = toOpt;
else {
core::ProblemPtr_t p = core::Problem::create(problem()->robot());
p->distance(problem()->distance());
// It should be ok to use the path validation of each edge because it
// corresponds to the global path validation minus the collision pairs
// disabled using the edge constraint.
// p.pathValidation(gpv->innerValidation());
p->pathProjector(problem()->pathProjector());
p->steeringMethod(edge->steeringMethod()->copy());
p->constraints(edge->pathConstraint());
if (p->constraints() && p->constraints()->configProjector()) {
p->constraints()->configProjector()->rightHandSideFromConfig(
toOpt->initial());
} }
pathOptimizer_.reset (); p->pathValidation(edge->pathValidation());
const_cast <core::Problem&>(this->problem ()).pathValidation (gpv); pathOptimizer_ = factory_(p);
return opted; toConcat = pathOptimizer_->optimize(toOpt);
} }
} // namespace manipulation i_s = i_e;
} // namespace hpp opted->concatenate(toConcat);
}
pathOptimizer_.reset();
return opted;
}
} // namespace manipulation
} // namespace hpp
src/graph-path-validation.cc
View file @ 539aab52
// Copyright (c) 2014, LAAS-CNRS // Copyright (c) 2014, LAAS-CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr) // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
// //
// This file is part of hpp-manipulation.
// hpp-manipulation is free software: you can redistribute it // Redistribution and use in source and binary forms, with or without
// and/or modify it under the terms of the GNU Lesser General Public // modification, are permitted provided that the following conditions are
// License as published by the Free Software Foundation, either version // met:
// 3 of the License, or (at your option) any later version. //
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// //
// hpp-manipulation is distributed in the hope that it will be // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// General Lesser Public License for more details. You should have // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// received a copy of the GNU Lesser General Public License along with // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>. // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
#include "hpp/manipulation/graph-path-validation.hh" #include "hpp/manipulation/graph-path-validation.hh"
#include <hpp/core/path-vector.hh>
#include <hpp/core/path.hh>
#include <hpp/manipulation/constraint-set.hh> #include <hpp/manipulation/constraint-set.hh>
#include <hpp/manipulation/graph/edge.hh>
#include <hpp/manipulation/graph/graph.hh>
#include <hpp/manipulation/graph/state.hh>
#include <hpp/pinocchio/configuration.hh>
#ifdef HPP_DEBUG
#include <hpp/manipulation/graph/state.hh>
#endif
namespace hpp { namespace hpp {
namespace manipulation { namespace manipulation {
GraphPathValidationPtr_t GraphPathValidation::create (const PathValidationPtr_t& pathValidation) GraphPathValidationPtr_t GraphPathValidation::create(
{ const PathValidationPtr_t& pathValidation) {
GraphPathValidation* p = new GraphPathValidation (pathValidation); GraphPathValidation* p = new GraphPathValidation(pathValidation);
return GraphPathValidationPtr_t (p); return GraphPathValidationPtr_t(p);
} }
GraphPathValidation::GraphPathValidation (const PathValidationPtr_t& pathValidation) : GraphPathValidation::GraphPathValidation(
pathValidation_ (pathValidation), constraintGraph_ () const PathValidationPtr_t& pathValidation)
{} : pathValidation_(pathValidation), constraintGraph_() {}
bool GraphPathValidation::validate (const PathPtr_t& path, bool reverse, bool GraphPathValidation::validate(const PathPtr_t& path, bool reverse,
PathPtr_t& validPart, PathPtr_t& validPart,
PathValidationReportPtr_t& report) PathValidationReportPtr_t& report) {
{ assert(path);
assert (path); bool success = impl_validate(path, reverse, validPart, report);
bool success = impl_validate (path, reverse, validPart, report); assert(constraintGraph_);
assert (constraintGraph_); assert(constraintGraph_->getState(validPart->initial()));
assert (constraintGraph_->getNode (validPart->initial ())); assert(constraintGraph_->getState(validPart->end()));
assert (constraintGraph_->getNode (validPart->end ())); return success;
return success; }
}
bool GraphPathValidation::impl_validate (const PathVectorPtr_t& path, bool GraphPathValidation::impl_validate(const PathVectorPtr_t& path,
bool reverse, PathPtr_t& validPart, PathValidationReportPtr_t& report) bool reverse, PathPtr_t& validPart,
{ PathValidationReportPtr_t& report) {
PathPtr_t validSubPart; PathPtr_t validSubPart;
if (reverse) { if (reverse) {
// TODO: This has never been tested. // TODO: This has never been tested.
assert (!reverse && "This has never been tested with reverse path"); assert(!reverse && "This has never been tested with reverse path");
for (long int i = path->numberPaths () - 1; i >= 0; i--) { for (long int i = path->numberPaths() - 1; i >= 0; i--) {
// We should stop at the first non valid subpath. // We should stop at the first non valid subpath.
if (!impl_validate (path->pathAtRank (i), true, validSubPart, report)) { if (!impl_validate(path->pathAtRank(i), true, validSubPart, report)) {
PathVectorPtr_t p = PathVector::create PathVectorPtr_t p = PathVector::create(path->outputSize(),
(path->outputSize(), path->outputDerivativeSize()); path->outputDerivativeSize());
for (long int v = path->numberPaths () - 1; v > i; v--) for (long int v = path->numberPaths() - 1; v > i; v--)
p->appendPath (path->pathAtRank(i)->copy()); p->appendPath(path->pathAtRank(i)->copy());
// TODO: Make sure this subpart is generated by the steering method. // TODO: Make sure this subpart is generated by the steering method.
p->appendPath (validSubPart); p->appendPath(validSubPart);
validPart = p; validPart = p;
return false; return false;
} }
} }
} else { } else {
for (size_t i = 0; i != path->numberPaths (); i++) { for (size_t i = 0; i != path->numberPaths(); i++) {
// We should stop at the first non valid subpath. // We should stop at the first non valid subpath.
if (!impl_validate (path->pathAtRank (i), false, validSubPart, report)) { if (!impl_validate(path->pathAtRank(i), false, validSubPart, report)) {
PathVectorPtr_t p = PathVector::create PathVectorPtr_t p = PathVector::create(path->outputSize(),
(path->outputSize(), path->outputDerivativeSize()); path->outputDerivativeSize());
for (size_t v = 0; v < i; v++) for (size_t v = 0; v < i; v++)
p->appendPath (path->pathAtRank(v)->copy()); p->appendPath(path->pathAtRank(v)->copy());
// TODO: Make sure this subpart is generated by the steering method. // TODO: Make sure this subpart is generated by the steering method.
p->appendPath (validSubPart); p->appendPath(validSubPart);
validPart = p; validPart = p;
return false; return false;
}
}
} }
// Here, every subpath is valid.
validPart = path;
return true;
} }
}
// Here, every subpath is valid.
validPart = path;
return true;
}
bool GraphPathValidation::impl_validate (const PathPtr_t& path, bool GraphPathValidation::impl_validate(const PathPtr_t& path, bool reverse,
bool reverse, PathPtr_t& validPart, PathValidationReportPtr_t& report) PathPtr_t& validPart,
{ PathValidationReportPtr_t& report) {
PathVectorPtr_t pathVector = HPP_DYNAMIC_PTR_CAST(PathVector, path); #ifndef NDEBUG
if (pathVector) bool success;
return impl_validate (pathVector, reverse, validPart, report); Configuration_t q0 = path->eval(path->timeRange().second, success);
assert(success);
assert(!path->constraints() || path->constraints()->isSatisfied(q0));
#endif
using pinocchio::displayConfig;
PathVectorPtr_t pathVector = HPP_DYNAMIC_PTR_CAST(PathVector, path);
if (pathVector) return impl_validate(pathVector, reverse, validPart, report);
PathPtr_t pathNoCollision; PathPtr_t pathNoCollision;
ConstraintSetPtr_t c = HPP_DYNAMIC_PTR_CAST(ConstraintSet, path->constraints()); ConstraintSetPtr_t c =
PathValidationPtr_t validation (c HPP_DYNAMIC_PTR_CAST(ConstraintSet, path->constraints());
? c->edge()->pathValidation() hppDout(info,
: pathValidation_); (c ? "Using edge path validation" : "Using default path validation"));
PathValidationPtr_t validation(c ? c->edge()->pathValidation()
: pathValidation_);
if (validation->validate (path, reverse, pathNoCollision, report)) { if (validation->validate(path, reverse, pathNoCollision, report)) {
validPart = path; validPart = path;
return true; return true;
} }
const Path& newPath (*pathNoCollision); const Path& newPath(*pathNoCollision);
const Path& oldPath (*path); const Path& oldPath(*path);
const core::interval_t& newTR = newPath.timeRange (), const core::interval_t &newTR = newPath.timeRange(),
oldTR = oldPath.timeRange (); oldTR = oldPath.timeRange();
Configuration_t q (newPath.outputSize()); Configuration_t q(newPath.outputSize());
if (!newPath (q, newTR.first)) if (!newPath.eval(q, newTR.first))
throw std::logic_error ("Initial configuration of the valid part cannot be projected."); throw std::logic_error(
const graph::NodePtr_t& origNode = constraintGraph_->getNode (q); "Initial configuration of the valid part cannot be projected.");
if (!newPath (q, newTR.second)) const graph::StatePtr_t& origState = constraintGraph_->getState(q);
throw std::logic_error ("End configuration of the valid part cannot be projected."); if (!newPath.eval(q, newTR.second))
// This may throw in the following case: throw std::logic_error(
// - node constraints: object_placement + other_function "End configuration of the valid part cannot be projected.");
// - path constraints: other_function, object_lock // This may throw in the following case:
// This is semantically correct but for a path going from q0 to q1, // - state constraints: object_placement + other_function
// we ensure that // - path constraints: other_function, object_lock
// - object_placement (q0) = eps_place0 // This is semantically correct but for a path going from q0 to q1,
// - other_function (q0) = eps_other0 // we ensure that
// - eps_place0 + eps_other0 < eps // - object_placement (q0) = eps_place0
// - other_function (q(s)) < eps // - other_function (q0) = eps_other0
// - object_placement (q(s)) = object_placement (q0) # thanks to the object_lock // - eps_place0 + eps_other0 < eps
// So we only have: // - other_function (q(s)) < eps
// - other_function (q(s)) + object_placement (q(s)) < eps + eps_place0 // - object_placement (q(s)) = object_placement (q0) # thanks to the
// And not: // object_lock So we only have:
// - other_function (q(s)) + object_placement (q(s)) < eps // - other_function (q(s)) + object_placement (q(s)) < eps + eps_place0
// In this case, there is no good way to recover. Just return failure. // And not:
graph::NodePtr_t destNode; // - other_function (q(s)) + object_placement (q(s)) < eps
try { // In this case, there is no good way to recover. Just return failure.
destNode = constraintGraph_->getNode (q); graph::StatePtr_t destState;
} catch (const std::logic_error& e) { try {
ConstraintSetPtr_t c = HPP_DYNAMIC_PTR_CAST(ConstraintSet, path->constraints()); destState = constraintGraph_->getState(q);
hppDout (error, "Edge " << c->edge()->name() } catch (const std::logic_error& e) {
<< " generated an error: " << e.what()); ConstraintSetPtr_t c =
hppDout (error, "Likely, the constraints for paths are relaxed. If " HPP_DYNAMIC_PTR_CAST(ConstraintSet, path->constraints());
hppDout(error, "Edge " << c->edge()->name()
<< " generated an error: " << e.what());
hppDout(error,
"Likely, the constraints for paths are relaxed. If "
"this problem occurs often, you may want to use the same " "this problem occurs often, you may want to use the same "
"constraints for node and paths in " << c->edge()->node()->name()); "constraints for state and paths in "
validPart = path->extract (std::make_pair (oldTR.first,oldTR.first)); << c->edge()->state()->name());
return false; validPart = path->extract(std::make_pair(oldTR.first, oldTR.first));
} return false;
if (!oldPath (q, oldTR.first)) }
throw std::logic_error ("Initial configuration of the path to be validated cannot be projected."); if (!oldPath.eval(q, oldTR.first)) {
const graph::NodePtr_t& oldOnode = constraintGraph_->getNode (q); std::stringstream oss;
if (!oldPath (q, oldTR.second)) oss << "Initial configuration of the path to be validated failed to"
throw std::logic_error ("End configuration of the path to be validated cannot be projected."); " be projected. After maximal number of iterations, q="
const graph::NodePtr_t& oldDnode = constraintGraph_->getNode (q); << displayConfig(q) << "; error=";
vector_t error;
oldPath.constraints()->isSatisfied(q, error);
oss << displayConfig(error) << ".";
throw std::logic_error(oss.str().c_str());
}
const graph::StatePtr_t& oldOstate = constraintGraph_->getState(q);
if (!oldPath.eval(q, oldTR.second)) {
std::stringstream oss;
oss << "End configuration of the path to be validated failed to"
" be projected. After maximal number of iterations, q="
<< displayConfig(q) << "; error=";
vector_t error;
oldPath.constraints()->isSatisfied(q, error);
oss << displayConfig(error) << ".";
throw std::logic_error(oss.str().c_str());
}
const graph::StatePtr_t& oldDstate = constraintGraph_->getState(q);
if (origNode == oldOnode && destNode == oldDnode) { if (origState == oldOstate && destState == oldDstate) {
validPart = pathNoCollision; validPart = pathNoCollision;
return false; return false;
} }
// Here, the full path is not valid. Moreover, it does not correspond to the same
// edge of the constraint graph. Two option are possible:
// - Use the steering method to create a new path and validate it.
// - Return a null path.
// validPart = path->extract (std::make_pair (oldTR.first,oldTR.first));
validPart = pathNoCollision;
return false;
}
void GraphPathValidation::addObstacle (const hpp::core::CollisionObjectPtr_t& collisionObject) // Here, the full path is not valid. Moreover, it does not correspond to the
{ // same edge of the constraint graph. Two option are possible:
pathValidation_->addObstacle (collisionObject); // - Use the steering method to create a new path and validate it.
} // - Return a null path.
} // namespace manipulation // validPart = path->extract (std::make_pair (oldTR.first,oldTR.first));
} // namespace hpp validPart = pathNoCollision;
return false;
}
} // namespace manipulation
} // namespace hpp
src/graph-steering-method.cc deleted 100644 → 0
View file @ 0e448947
// 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/distance.hh>
#include <hpp/core/straight-path.hh>
#include "hpp/manipulation/graph/graph.hh"
#include "hpp/manipulation/graph/edge.hh"
namespace hpp {
namespace manipulation {
GraphSteeringMethodPtr_t GraphSteeringMethod::create
(const core::ProblemPtr_t& problem)
{
assert (dynamic_cast <const ProblemPtr_t> (problem) != NULL
&& "Cast to const ProblemPtr_t failed");
const ProblemPtr_t& p = static_cast <const ProblemPtr_t> (problem);
return create (p);
}
GraphSteeringMethodPtr_t GraphSteeringMethod::create
(const ProblemPtr_t& 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 ProblemPtr_t& problem) :
SteeringMethod (problem), problem_ (problem), weak_ ()
{
}
GraphSteeringMethod::GraphSteeringMethod (const GraphSteeringMethod& other):
SteeringMethod (other), problem_ (other.problem_)
{
}
PathPtr_t GraphSteeringMethod::impl_compute (ConfigurationIn_t q1, ConfigurationIn_t q2) const
{
graph::Edges_t possibleEdges;
graph::Graph& graph = *problem_->constraintGraph ();
try {
possibleEdges = graph.getEdges (graph.getNode (q1), graph.getNode (q2));
} catch (const std::logic_error& e) {
hppDout (error, e.what ());
return PathPtr_t ();
}
PathPtr_t path;
while (!possibleEdges.empty()) {
if (possibleEdges.back ()->build (path, q1, q2)) {
return path;
}
possibleEdges.pop_back ();
}
return PathPtr_t ();
}
} // namespace manipulation
} // namespace hpp
src/graph/dot.cc
View file @ 539aab52
// Copyright (c) 2014, LAAS-CNRS // Copyright (c) 2014, LAAS-CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr) // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
// //
// This file is part of hpp-manipulation.
// hpp-manipulation is free software: you can redistribute it // Redistribution and use in source and binary forms, with or without
// and/or modify it under the terms of the GNU Lesser General Public // modification, are permitted provided that the following conditions are
// License as published by the Free Software Foundation, either version // met:
// 3 of the License, or (at your option) any later version. //
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// //
// hpp-manipulation is distributed in the hope that it will be // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// General Lesser Public License for more details. You should have // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// received a copy of the GNU Lesser General Public License along with // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>. // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
#include "hpp/manipulation/graph/dot.hh" #include "hpp/manipulation/graph/dot.hh"
namespace hpp { namespace hpp {
namespace manipulation { namespace manipulation {
namespace graph { namespace graph {
namespace dot { namespace dot {
const std::string Tooltip::tooltipendl = "&#10;"; const std::string Tooltip::tooltipendl = "&#10;";
std::ostream& operator<< (std::ostream& os, const DrawingAttributes& da) std::ostream& operator<<(std::ostream& os, const DrawingAttributes& da) {
{ if (da.attr.empty()) return os;
if (da.attr.empty ()) return os; os << da.openSection;
os << da.openSection; size_t i = da.attr.size();
size_t i = da.attr.size (); for (DrawingAttributes::Map::const_iterator it = da.attr.begin();
for (DrawingAttributes::Map::const_iterator it = da.attr.begin (); it != da.attr.end(); ++it) {
it != da.attr.end (); ++it) { os << it->first << "=" << it->second;
os << it->first << "=" << it->second; i--;
i--; if (i > 0) os << da.separator;
if (i > 0) os << da.separator; }
} return os << da.closeSection;
return os << da.closeSection; }
}
std::ostream& insertComments (std::ostream& os, const std::string& c) std::ostream& insertComments(std::ostream& os, const std::string& c) {
{ return os << "/*" << std::endl << c << std::endl << "*/";
return os << "/*" << std::endl << c << std::endl << "*/"; }
} } // namespace dot
} // namespace dot } // namespace graph
} // namespace graph } // namespace manipulation
} // namespace manipulation } // namespace hpp
} // namespace hpp
src/graph/edge.cc
View file @ 539aab52
// Copyright (c) 2014, LAAS-CNRS // Copyright (c) 2014, LAAS-CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr) // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
// //
// This file is part of hpp-manipulation.
// hpp-manipulation is free software: you can redistribute it // Redistribution and use in source and binary forms, with or without
// and/or modify it under the terms of the GNU Lesser General Public // modification, are permitted provided that the following conditions are
// License as published by the Free Software Foundation, either version // met:
// 3 of the License, or (at your option) any later version. //
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// //
// hpp-manipulation is distributed in the hope that it will be // 2. Redistributions in binary form must reproduce the above copyright
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty // notice, this list of conditions and the following disclaimer in the
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // documentation and/or other materials provided with the distribution.
// General Lesser Public License for more details. You should have //
// received a copy of the GNU Lesser General Public License along with // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>. // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
#include "hpp/manipulation/graph/edge.hh" #include "hpp/manipulation/graph/edge.hh"
#include <sstream>
#include <hpp/core/path-vector.hh>
#include <hpp/constraints/differentiable-function.hh> #include <hpp/constraints/differentiable-function.hh>
#include <hpp/constraints/locked-joint.hh>
#include <hpp/constraints/solver/by-substitution.hh>
#include <hpp/core/obstacle-user.hh>
#include <hpp/core/path-validation.hh>
#include <hpp/core/path-vector.hh>
#include <hpp/pinocchio/configuration.hh>
#include <hpp/util/exception-factory.hh>
#include <hpp/util/pointer.hh> #include <hpp/util/pointer.hh>
#include <sstream>
#include "hpp/manipulation/constraint-set.hh"
#include "hpp/manipulation/device.hh" #include "hpp/manipulation/device.hh"
#include "hpp/manipulation/problem.hh"
#include "hpp/manipulation/graph-steering-method.hh"
#include "hpp/manipulation/graph/statistics.hh" #include "hpp/manipulation/graph/statistics.hh"
#include "hpp/manipulation/constraint-set.hh" #include "hpp/manipulation/problem.hh"
#include "hpp/manipulation/steering-method/graph.hh"
namespace hpp { namespace hpp {
namespace manipulation { namespace manipulation {
namespace graph { namespace graph {
Edge::Edge (const std::string& name) : Edge::Edge(const std::string& name)
GraphComponent (name), isShort_ (false), : GraphComponent(name),
pathConstraints_ (new Constraint_t()), isShort_(false),
configConstraints_ (new Constraint_t()), pathConstraints_(),
steeringMethod_ (new SteeringMethod_t()), targetConstraints_(),
pathValidation_ (new PathValidation_t()) steeringMethod_(),
{} securityMargins_(),
pathValidation_() {}
Edge::~Edge ()
{ Edge::~Edge() {}
if (pathConstraints_ ) delete pathConstraints_;
if (configConstraints_) delete configConstraints_; StatePtr_t Edge::stateTo() const { return to_.lock(); }
if (steeringMethod_ ) delete steeringMethod_;
if (pathValidation_ ) delete pathValidation_; StatePtr_t Edge::stateFrom() const { return from_.lock(); }
}
void Edge::relativeMotion(const RelativeMotion::matrix_type& m) {
NodePtr_t Edge::to () const if (!isInit_)
{ throw std::logic_error(
return to_.lock(); "The graph must be initialized before changing the relative motion "
} "matrix.");
shared_ptr<core::ObstacleUserInterface> oui =
NodePtr_t Edge::from () const HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
{ if (oui) oui->filterCollisionPairs(m);
return from_.lock(); relMotion_ = m;
} }
bool Edge::direction (const core::PathPtr_t& path) const bool Edge::direction(const core::PathPtr_t& path) const {
{ Configuration_t q0 = path->initial(), q1 = path->end();
Configuration_t q0 = path->initial (), const bool src_contains_q0 = stateFrom()->contains(q0);
q1 = path->end (); const bool dst_contains_q0 = stateTo()->contains(q0);
const bool src_contains_q0 = from()->contains (q0); const bool src_contains_q1 = stateFrom()->contains(q1);
const bool dst_contains_q0 = to ()->contains (q0); const bool dst_contains_q1 = stateTo()->contains(q1);
const bool src_contains_q1 = from()->contains (q1); // Karnaugh table:
const bool dst_contains_q1 = to ()->contains (q1); // 1 = forward, 0 = reverse, ? = I don't know, * = 0 or 1
if (!( // s0s1 \ d0d1 | 00 | 01 | 11 | 10
(src_contains_q0 && dst_contains_q1) // 00 | ? | ? | ? | ?
|| (src_contains_q1 && dst_contains_q0) // 01 | ? | ? | 0 | 0
)) { // 11 | ? | 1 | * | 0
if (src_contains_q0) { // 10 | ? | 1 | 1 | 1
assert (node ()->contains (q1)); //
return false; /// true if reverse
} else if (src_contains_q1) { if ((!src_contains_q0 && !src_contains_q1) ||
assert (node ()->contains (q0)); (!dst_contains_q0 && !dst_contains_q1) ||
return true; (!src_contains_q0 && !dst_contains_q0))
} HPP_THROW(std::runtime_error,
throw std::runtime_error ("This path does not seem to have been " "Edge " << name() << " does not seem to have generated path from"
"generated by this edge."); << pinocchio::displayConfig(q0) << " to "
} << pinocchio::displayConfig(q1));
// Karnaugh table: return !(src_contains_q0 && (!src_contains_q1 || dst_contains_q1));
// 1 = forward, 0 = reverse, ? = I don't know, * = 0 or 1 }
// s0s1 \ d0d1 | 00 | 01 | 11 | 10
// 00 | ? | ? | ? | ? void Edge::securityMarginForPair(const size_type& row, const size_type& col,
// 01 | ? | ? | 0 | 0 const value_type& margin) {
// 11 | ? | 1 | * | 0 if ((row < 0) || (row >= securityMargins_.rows())) {
// 10 | ? | 1 | 1 | 1 std::ostringstream os;
// os << "Row index should be between 0 and " << securityMargins_.rows() + 1
/// true if reverse << ", got " << row << ".";
return !(src_contains_q0 && (!src_contains_q1 || dst_contains_q1)); throw std::runtime_error(os.str().c_str());
} }
if ((col < 0) || (col >= securityMargins_.cols())) {
bool WaypointEdge::direction (const core::PathPtr_t& path) const std::ostringstream os;
{ os << "Column index should be between 0 and " << securityMargins_.cols() + 1
Configuration_t q0 = path->initial (), << ", got " << col << ".";
q1 = path->end (); throw std::runtime_error(os.str().c_str());
const bool src_contains_q0 = waypoints_.back().second->contains (q0); }
const bool dst_contains_q0 = to ()->contains (q0); if (securityMargins_(row, col) != margin) {
const bool src_contains_q1 = waypoints_.back().second->contains (q1); securityMargins_(row, col) = margin;
const bool dst_contains_q1 = to ()->contains (q1); securityMargins_(col, row) = margin;
if (!( invalidate();
(src_contains_q0 && dst_contains_q1) }
|| (src_contains_q1 && dst_contains_q0) }
)) {
if (src_contains_q0) { bool Edge::intersectionConstraint(const EdgePtr_t& other,
assert (node ()->contains (q1)); ConfigProjectorPtr_t proj) const {
return false; GraphPtr_t g = graph_.lock();
} else if (src_contains_q1) {
assert (node ()->contains (q0)); g->insertNumericalConstraints(proj);
return true; insertNumericalConstraints(proj);
} state()->insertNumericalConstraints(proj);
throw std::runtime_error ("This path does not seem to have been "
"generated by this edge."); if (wkPtr_.lock() == other) // No intersection to be computed.
} return false;
/// See Edge::direction for Karnaugh table bool stateB_Eq_stateA = (state() == other->state());
/// true if reverse
return !(src_contains_q0 && (!src_contains_q1 || dst_contains_q1)); other->insertNumericalConstraints(proj);
} if (!stateB_Eq_stateA) other->state()->insertNumericalConstraints(proj);
return true;
bool Edge::intersectionConstraint (const EdgePtr_t& other, }
ConfigProjectorPtr_t proj) const
{ EdgePtr_t Edge::create(const std::string& name, const GraphWkPtr_t& graph,
GraphPtr_t g = graph_.lock (); const StateWkPtr_t& from, const StateWkPtr_t& to) {
Edge* ptr = new Edge(name);
g->insertNumericalConstraints (proj); EdgePtr_t shPtr(ptr);
insertNumericalConstraints (proj); ptr->init(shPtr, graph, from, to);
node ()->insertNumericalConstraints (proj); return shPtr;
}
g->insertLockedJoints (proj);
insertLockedJoints (proj); void Edge::init(const EdgeWkPtr_t& weak, const GraphWkPtr_t& graph,
node ()->insertLockedJoints (proj); const StateWkPtr_t& from, const StateWkPtr_t& to) {
GraphComponent::init(weak);
if (wkPtr_.lock() == other) // No intersection to be computed. parentGraph(graph);
return false; wkPtr_ = weak;
from_ = from;
bool nodeB_Eq_nodeA = (node() == other->node()); to_ = to;
state_ = to;
other->insertNumericalConstraints (proj); // add 1 joint for the environment
if (!nodeB_Eq_nodeA) other->node()->insertNumericalConstraints (proj); size_type n(graph.lock()->robot()->nbJoints() + 1);
other->insertLockedJoints (proj); securityMargins_.resize(n, n);
if (!nodeB_Eq_nodeA) other->node()->insertLockedJoints (proj); securityMargins_.setZero();
}
return true;
} void Edge::initialize() {
if (!isInit_) {
EdgePtr_t Edge::create (const std::string& name, targetConstraints_ = buildTargetConstraint();
const GraphWkPtr_t& graph, pathConstraints_ = buildPathConstraint();
const NodeWkPtr_t& from, const NodeWkPtr_t& to) }
{ isInit_ = true;
Edge* ptr = new Edge (name); }
EdgePtr_t shPtr (ptr);
ptr->init(shPtr, graph, from, to); std::ostream& Edge::print(std::ostream& os) const {
return shPtr; os << "| | |-- ";
} GraphComponent::print(os) << " --> " << to_.lock()->name();
return os;
void Edge::init (const EdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const NodeWkPtr_t& from, }
const NodeWkPtr_t& to)
{ std::ostream& Edge::dotPrint(std::ostream& os,
GraphComponent::init (weak); dot::DrawingAttributes da) const {
parentGraph (graph); da.insertWithQuote("label", name());
wkPtr_ = weak; da.insert("shape", "onormal");
from_ = from; dot::Tooltip tp;
to_ = to; tp.addLine("Edge constains:");
node_ = to; populateTooltip(tp);
} da.insertWithQuote("tooltip", tp.toStr());
da.insertWithQuote("labeltooltip", tp.toStr());
std::ostream& Edge::print (std::ostream& os) const os << stateFrom()->id() << " -> " << stateTo()->id() << " " << da << ";";
{ return os;
os << "| | |-- "; }
GraphComponent::print (os)
<< " --> " << to_.lock ()->name (); ConstraintSetPtr_t Edge::targetConstraint() const {
return os; throwIfNotInitialized();
} return targetConstraints_;
}
std::ostream& Edge::dotPrint (std::ostream& os, dot::DrawingAttributes da) const
{ // Merge constraints of several graph components into a config projector
da.insertWithQuote ("label", name ()); // Replace constraints and complement by combination of both when
da.insert ("shape", "onormal"); // necessary.
dot::Tooltip tp; tp.addLine ("Edge constains:"); static void mergeConstraintsIntoConfigProjector(
populateTooltip (tp); const ConfigProjectorPtr_t& proj,
da.insertWithQuote ("tooltip", tp.toStr()); const std::vector<GraphComponentPtr_t>& components,
da.insertWithQuote ("labeltooltip", tp.toStr()); const GraphPtr_t& graph) {
os << from()->id () << " -> " << to()->id () << " " << da << ";"; NumericalConstraints_t nc;
return os; for (const auto& gc : components)
} nc.insert(nc.end(), gc->numericalConstraints().begin(),
gc->numericalConstraints().end());
ConstraintSetPtr_t Edge::configConstraint() const
{ // Remove duplicate constraints
if (!*configConstraints_) { auto end = nc.end();
configConstraints_->set (buildConfigConstraint ()); for (auto it = nc.begin(); it != end; ++it)
} end = std::remove(std::next(it), end, *it);
return configConstraints_->get (); nc.erase(end, nc.end());
}
NumericalConstraints_t::iterator itnc1, itnc2;
ConstraintSetPtr_t Edge::buildConfigConstraint() const
{ // Look for complement
std::string n = "(" + name () + ")"; for (itnc1 = nc.begin(); itnc1 != nc.end(); ++itnc1) {
GraphPtr_t g = graph_.lock (); const auto& c1 = *itnc1;
itnc2 = std::next(itnc1);
ConstraintSetPtr_t constraint = ConstraintSet::create (g->robot (), "Set " + n); constraints::ImplicitPtr_t combination;
itnc2 = std::find_if(std::next(itnc1), nc.end(),
ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations()); [&c1, &combination, &graph](const auto& c2) -> bool {
g->insertNumericalConstraints (proj); assert(c1 != c2);
insertNumericalConstraints (proj); return graph->isComplement(c1, c2, combination) ||
to ()->insertNumericalConstraints (proj); graph->isComplement(c2, c1, combination);
constraint->addConstraint (proj); });
if (itnc2 != nc.end()) {
g->insertLockedJoints (proj); assert(*itnc1 != *itnc2);
insertLockedJoints (proj); *itnc1 = combination;
to ()->insertLockedJoints (proj); nc.erase(itnc2);
}
constraint->edge (wkPtr_.lock ()); }
return constraint;
} for (const auto& _nc : nc) proj->add(_nc);
// Insert numerical costs
ConstraintSetPtr_t Edge::pathConstraint() const nc.clear();
{ for (const auto& gc : components)
if (!*pathConstraints_) { nc.insert(nc.end(), gc->numericalCosts().begin(),
ConstraintSetPtr_t pathConstraints (buildPathConstraint ()); gc->numericalCosts().end());
pathConstraints_->set (pathConstraints); for (const auto& _nc : nc) proj->add(_nc, 1);
} }
return pathConstraints_->get ();
} ConstraintSetPtr_t Edge::buildTargetConstraint() {
std::string n = "(" + name() + ")";
ConstraintSetPtr_t Edge::buildPathConstraint() const GraphPtr_t g = graph_.lock();
{
std::string n = "(" + name () + ")"; ConstraintSetPtr_t constraint = ConstraintSet::create(g->robot(), "Set " + n);
GraphPtr_t g = graph_.lock ();
ConfigProjectorPtr_t proj = ConfigProjector::create(
ConstraintSetPtr_t constraint = ConstraintSet::create (g->robot (), "Set " + n); g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations());
proj->solver().solveLevelByLevel(this->solveLevelByLevel());
ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations()); std::vector<GraphComponentPtr_t> components;
g->insertNumericalConstraints (proj); components.push_back(g);
insertNumericalConstraints (proj); components.push_back(wkPtr_.lock());
node ()->insertNumericalConstraintsForPath (proj); components.push_back(stateTo());
constraint->addConstraint (proj); if (state() != stateTo()) {
components.push_back(state());
g->insertLockedJoints (proj); }
insertLockedJoints (proj); // Copy constraints from
node ()->insertLockedJoints (proj); // - graph,
// - this edge,
constraint->edge (wkPtr_.lock ()); // - the destination state,
// - the state in which the transition lies if different
// Build steering method mergeConstraintsIntoConfigProjector(proj, components, parentGraph());
const ProblemPtr_t& problem (g->problem());
steeringMethod_->set(problem->steeringMethod() constraint->addConstraint(proj);
->innerSteeringMethod()->copy()); constraint->edge(wkPtr_.lock());
steeringMethod_->get()->constraints (constraint); return constraint;
// Build path validation and relative motion matrix }
// TODO this path validation will not contain obstacles added after
// its creation. ConstraintSetPtr_t Edge::pathConstraint() const {
pathValidation_->set(problem->pathValidationFactory ()); throwIfNotInitialized();
using core::RelativeMotion; return pathConstraints_;
RelativeMotion::matrix_type matrix (RelativeMotion::matrix (g->robot())); }
RelativeMotion::fromConstraint (matrix, g->robot(), constraint);
pathValidation_->get()->filterCollisionPairs (matrix); ConstraintSetPtr_t Edge::buildPathConstraint() {
return constraint; std::string n = "(" + name() + ")";
} GraphPtr_t g = graph_.lock();
bool Edge::canConnect (ConfigurationIn_t q1, ConfigurationIn_t q2) ConstraintSetPtr_t constraint = ConstraintSet::create(g->robot(), "Set " + n);
const
{ ConfigProjectorPtr_t proj = ConfigProjector::create(
ConstraintSetPtr_t constraints = pathConstraint (); g->robot(), "proj_" + n, .5 * g->errorThreshold(), g->maxIterations());
constraints->configProjector ()->rightHandSideFromConfig(q1); proj->solver().solveLevelByLevel(this->solveLevelByLevel());
if (!constraints->isSatisfied (q1) || !constraints->isSatisfied (q2)) { std::vector<GraphComponentPtr_t> components;
return false; components.push_back(g);
} components.push_back(wkPtr_.lock());
return true; components.push_back(state());
} mergeConstraintsIntoConfigProjector(proj, components, parentGraph());
bool Edge::build (core::PathPtr_t& path, ConfigurationIn_t q1, constraint->addConstraint(proj);
ConfigurationIn_t q2) constraint->edge(wkPtr_.lock());
const
{ // Build steering method
ConstraintSetPtr_t constraints = pathConstraint (); const ProblemPtr_t& problem(g->problem());
constraints->configProjector ()->rightHandSideFromConfig(q1); steeringMethod_ =
if (!constraints->isSatisfied (q1) || !constraints->isSatisfied (q2)) { problem->manipulationSteeringMethod()->innerSteeringMethod()->copy();
return false; steeringMethod_->constraints(constraint);
} // Build path validation and relative motion matrix
path = (*steeringMethod_->get()) (q1, q2); // TODO this path validation will not contain obstacles added after
return true; // its creation.
} pathValidation_ = problem->pathValidationFactory();
shared_ptr<core::ObstacleUserInterface> oui =
bool Edge::applyConstraints (core::NodePtr_t nnear, ConfigurationOut_t q) const HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
{ if (oui) {
return applyConstraints (*(nnear->configuration ()), q); relMotion_ = RelativeMotion::matrix(g->robot());
} RelativeMotion::fromConstraint(relMotion_, g->robot(), constraint);
oui->filterCollisionPairs(relMotion_);
bool Edge::applyConstraints (ConfigurationIn_t qoffset, oui->setSecurityMargins(securityMargins_);
ConfigurationOut_t q) const }
{ return constraint;
ConstraintSetPtr_t c = configConstraint (); }
ConfigProjectorPtr_t proj = c->configProjector ();
proj->rightHandSideFromConfig (qoffset); bool Edge::canConnect(ConfigurationIn_t q1, ConfigurationIn_t q2) const {
if (isShort_) q = qoffset; ConstraintSetPtr_t constraints = pathConstraint();
if (c->apply (q)) return true; constraints->configProjector()->rightHandSideFromConfig(q1);
const ::hpp::statistics::SuccessStatistics& ss = proj->statistics (); if (!constraints->isSatisfied(q1) || !constraints->isSatisfied(q2)) {
if (ss.nbFailure () > ss.nbSuccess ()) { return false;
hppDout (warning, c->name () << " fails often.\n" << ss); }
} else { return true;
hppDout (warning, c->name () << " succeeds at rate " }
<< (value_type)(ss.nbSuccess ()) /
(value_type) ss.numberOfObservations () bool Edge::build(core::PathPtr_t& path, ConfigurationIn_t q1,
<< "."); ConfigurationIn_t q2) const {
} using pinocchio::displayConfig;
if (!steeringMethod_) {
std::ostringstream oss;
oss << "No steering method set in edge " << name() << ".";
throw std::runtime_error(oss.str().c_str());
}
ConstraintSetPtr_t constraints = pathConstraint();
constraints->configProjector()->rightHandSideFromConfig(q1);
if (constraints->isSatisfied(q1)) {
if (constraints->isSatisfied(q2)) {
path = (*steeringMethod_)(q1, q2);
return (bool)path;
} else {
hppDout(info, "q2 = " << displayConfig(q2)
<< " does not satisfy the constraints of edge "
<< name());
hppDout(info, "q1 = " << displayConfig(q1));
return false;
}
} else {
value_type th(constraints->configProjector()->errorThreshold());
if (!constraints->configProjector()->isSatisfied(q1, 2 * th)) {
std::ostringstream oss;
oss << "The initial configuration " << displayConfig(q1)
<< " does not satisfy the constraints of"
" edge "
<< name() << "." << std::endl;
oss << "The graph is probably malformed";
throw std::runtime_error(oss.str().c_str());
}
// q1 may slightly violate the edge constraint eventhough the graph
// is well constructed.
return false;
}
}
bool Edge::generateTargetConfig(core::NodePtr_t nStart,
ConfigurationOut_t q) const {
return generateTargetConfig(nStart->configuration(), q);
}
bool Edge::generateTargetConfig(ConfigurationIn_t qStart,
ConfigurationOut_t q) const {
ConstraintSetPtr_t c = targetConstraint();
ConfigProjectorPtr_t proj = c->configProjector();
proj->rightHandSideFromConfig(qStart);
if (isShort_) q = qStart;
if (c->apply(q)) return true;
const ::hpp::statistics::SuccessStatistics& ss = proj->statistics();
if (ss.nbFailure() > ss.nbSuccess()) {
hppDout(warning, c->name() << " fails often.\n" << ss);
} else {
hppDout(warning, c->name() << " succeeds at rate "
<< (value_type)(ss.nbSuccess()) /
(value_type)ss.numberOfObservations()
<< ".");
}
return false;
}
WaypointEdgePtr_t WaypointEdge::create(const std::string& name,
const GraphWkPtr_t& graph,
const StateWkPtr_t& from,
const StateWkPtr_t& to) {
WaypointEdge* ptr = new WaypointEdge(name);
WaypointEdgePtr_t shPtr(ptr);
ptr->init(shPtr, graph, from, to);
return shPtr;
}
void WaypointEdge::init(const WaypointEdgeWkPtr_t& weak,
const GraphWkPtr_t& graph, const StateWkPtr_t& from,
const StateWkPtr_t& to) {
Edge::init(weak, graph, from, to);
nbWaypoints(0);
wkPtr_ = weak;
}
void WaypointEdge::initialize() {
Edge::initialize();
// Set error threshold of internal edge to error threshold of
// waypoint edge divided by number of edges.
assert(targetConstraint()->configProjector());
value_type eps(targetConstraint()->configProjector()->errorThreshold() /
(value_type)edges_.size());
for (Edges_t::iterator it(edges_.begin()); it != edges_.end(); ++it) {
(*it)->initialize();
assert((*it)->targetConstraint());
assert((*it)->targetConstraint()->configProjector());
(*it)->targetConstraint()->configProjector()->errorThreshold(eps);
}
}
bool WaypointEdge::canConnect(ConfigurationIn_t q1,
ConfigurationIn_t q2) const {
/// TODO: This is not correct
for (std::size_t i = 0; i < edges_.size(); ++i)
if (!edges_[i]->canConnect(q1, q2)) return false;
return true;
}
bool WaypointEdge::build(core::PathPtr_t& path, ConfigurationIn_t q1,
ConfigurationIn_t q2) const {
core::PathPtr_t p;
core::PathVectorPtr_t pv =
core::PathVector::create(graph_.lock()->robot()->configSize(),
graph_.lock()->robot()->numberDof());
// Many times, this will be called rigth after
// WaypointEdge::generateTargetConfig so config_ already satisfies the
// constraints.
size_type n = edges_.size();
assert(configs_.cols() == n + 1);
bool useCache = lastSucceeded_ && configs_.col(0).isApprox(q1) &&
configs_.col(n).isApprox(q2);
configs_.col(0) = q1;
configs_.col(n) = q2;
for (size_type i = 0; i < n; ++i) {
if (i < (n - 1) && !useCache) configs_.col(i + 1) = q2;
if (i < (n - 1) && !edges_[i]->generateTargetConfig(configs_.col(i),
configs_.col(i + 1))) {
hppDout(info, "Waypoint edge "
<< name()
<< ": generateTargetConfig failed at waypoint " << i
<< "."
<< "\nUse cache: " << useCache);
lastSucceeded_ = false;
return false;
}
assert(targetConstraint());
assert(targetConstraint()->configProjector());
value_type eps(targetConstraint()->configProjector()->errorThreshold());
if ((configs_.col(i) - configs_.col(i + 1)).squaredNorm() > eps * eps) {
if (!edges_[i]->build(p, configs_.col(i), configs_.col(i + 1))) {
hppDout(info, "Waypoint edge "
<< name() << ": build failed at waypoint " << i << "."
<< "\nUse cache: " << useCache);
lastSucceeded_ = false;
return false; return false;
} }
pv->appendPath(p);
WaypointEdgePtr_t WaypointEdge::create (const std::string& name, }
const GraphWkPtr_t& graph, const NodeWkPtr_t& from, }
const NodeWkPtr_t& to)
{ path = pv;
WaypointEdge* ptr = new WaypointEdge (name); lastSucceeded_ = true;
WaypointEdgePtr_t shPtr (ptr); return true;
ptr->init(shPtr, graph, from, to); }
return shPtr;
} bool WaypointEdge::generateTargetConfig(ConfigurationIn_t qStart,
ConfigurationOut_t q) const {
void WaypointEdge::init (const WaypointEdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const NodeWkPtr_t& from, assert(configs_.cols() == size_type(edges_.size() + 1));
const NodeWkPtr_t& to) configs_.col(0) = qStart;
{ for (std::size_t i = 0; i < edges_.size(); ++i) {
Edge::init (weak, graph, from, to); configs_.col(i + 1) = q;
wkPtr_ = weak; if (!edges_[i]->generateTargetConfig(configs_.col(i),
} configs_.col(i + 1))) {
q = configs_.col(i + 1);
bool WaypointEdge::canConnect (ConfigurationIn_t q1, ConfigurationIn_t q2) const lastSucceeded_ = false;
{ return false;
/// TODO: This is not correct }
return waypoints_.back().first->canConnect (q1, q2) && Edge::canConnect (q1, q2); }
} q = configs_.col(edges_.size());
lastSucceeded_ = true;
bool WaypointEdge::build (core::PathPtr_t& path, ConfigurationIn_t q1, return true;
ConfigurationIn_t q2) const }
{
core::PathPtr_t p; void WaypointEdge::nbWaypoints(const size_type number) {
core::PathVectorPtr_t pv = core::PathVector::create edges_.resize(number + 1);
(graph_.lock ()->robot ()->configSize (), states_.resize(number + 1);
graph_.lock ()->robot ()->numberDof ()); states_.back() = stateTo();
// Many times, this will be called rigth after WaypointEdge::applyConstraints so config_ const size_type nbDof = graph_.lock()->robot()->configSize();
// already satisfies the constraints. configs_ = matrix_t(nbDof, number + 2);
bool useCache = result_.isApprox (q2); invalidate();
if (!useCache) configs_.col (0) = q2; }
assert (waypoints_[0].first); void WaypointEdge::setWaypoint(const std::size_t index, const EdgePtr_t wEdge,
if (!waypoints_[0].first->applyConstraints (q1, configs_.col (0))) const StatePtr_t wTo) {
return false; assert(edges_.size() == states_.size());
if (!waypoints_[0].first->build (p, q1, configs_.col (0))) assert(index < edges_.size());
return false; if (index == states_.size() - 1) {
pv->appendPath (p); assert(!wTo || wTo == stateTo());
} else {
for (std::size_t i = 1; i < waypoints_.size (); ++i) { states_[index] = wTo;
assert (waypoints_[i].first); }
if (!useCache) configs_.col (i) = q2; edges_[index] = wEdge;
if (!waypoints_[i].first->applyConstraints (configs_.col(i-1), configs_.col (i))) }
return false;
if (!waypoints_[i].first->build (p, configs_.col(i-1), configs_.col (i))) const EdgePtr_t& WaypointEdge::waypoint(const std::size_t index) const {
return false; assert(index < edges_.size());
pv->appendPath (p); return edges_[index];
} }
if (!Edge::build (p, configs_.col (configs_.cols()-1), q2)) std::ostream& WaypointEdge::print(std::ostream& os) const {
return false; os << "| | |-- ";
pv->appendPath (p); GraphComponent::print(os) << " (waypoint) --> " << stateTo()->name();
return os;
path = pv; }
return true;
} std::ostream& WaypointEdge::dotPrint(std::ostream& os,
dot::DrawingAttributes da) const {
bool WaypointEdge::applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const // First print the waypoint node, then the first edge.
{ da["style"] = "dashed";
assert (waypoints_[0].first); for (std::size_t i = 0; i < states_.size() - 1; ++i)
configs_.col (0) = q; states_[i]->dotPrint(os, da);
if (!waypoints_[0].first->applyConstraints (qoffset, configs_.col (0))) {
q = configs_.col(0); da["style"] = "solid";
return false; for (std::size_t i = 0; i < edges_.size(); ++i)
} edges_[i]->dotPrint(os, da) << std::endl;
for (std::size_t i = 1; i < waypoints_.size (); ++i) {
assert (waypoints_[i].first); da["style"] = "dotted";
configs_.col (i) = q; da["dir"] = "both";
if (!waypoints_[i].first->applyConstraints (configs_.col(i-1), configs_.col (i))) { da["arrowtail"] = "dot";
q = configs_.col(i);
return false; return Edge::dotPrint(os, da);
} }
}
bool success = Edge::applyConstraints (configs_.col (configs_.cols()-1), q); std::ostream& LevelSetEdge::print(std::ostream& os) const {
result_ = q; os << "| | |-- ";
return success; GraphComponent::print(os) << " (level set) --> " << stateTo()->name();
} return os;
}
void WaypointEdge::nbWaypoints (const size_type number)
{ std::ostream& LevelSetEdge::dotPrint(std::ostream& os,
waypoints_.resize (number); dot::DrawingAttributes da) const {
const size_type nbDof = graph_.lock ()->robot ()->configSize (); da.insert("shape", "onormal");
configs_ = matrix_t (nbDof, number); da.insert("style", "dashed");
result_ = Configuration_t (nbDof); return Edge::dotPrint(os, da);
} }
void WaypointEdge::setWaypoint (const std::size_t index, const EdgePtr_t wEdge, const NodePtr_t wTo) void LevelSetEdge::populateTooltip(dot::Tooltip& tp) const {
{ GraphComponent::populateTooltip(tp);
assert (index < waypoints_.size()); tp.addLine("");
waypoints_[index] = Waypoint_t (wEdge, wTo); tp.addLine("Foliation condition constraints:");
} for (NumericalConstraints_t::const_iterator it =
condNumericalConstraints_.begin();
NodePtr_t WaypointEdge::node () const it != condNumericalConstraints_.end(); ++it) {
{ tp.addLine("- " + (*it)->function().name());
if (isInNodeFrom ()) return waypoints_.back().second; }
else return to (); tp.addLine("Foliation parametrization constraints:");
} for (NumericalConstraints_t::const_iterator it =
paramNumericalConstraints_.begin();
template <> it != paramNumericalConstraints_.end(); ++it) {
EdgePtr_t WaypointEdge::waypoint <Edge> (const std::size_t index) const tp.addLine("- " + (*it)->function().name());
{ }
assert (index < waypoints_.size()); }
return waypoints_[index].first;
} bool LevelSetEdge::generateTargetConfig(ConfigurationIn_t qStart,
ConfigurationOut_t q) const {
template <> // First, get an offset from the histogram
WaypointEdgePtr_t WaypointEdge::waypoint <WaypointEdge> (const std::size_t index) const statistics::DiscreteDistribution<RoadmapNodePtr_t> distrib =
{ hist_->getDistrib();
assert (index < waypoints_.size()); if (distrib.size() == 0) {
return HPP_DYNAMIC_PTR_CAST (WaypointEdge, waypoints_[index].first); hppDout(warning, "Edge " << name() << ": Distrib is empty");
} return false;
}
std::ostream& WaypointEdge::print (std::ostream& os) const const Configuration_t& qLeaf = distrib()->configuration();
{
os << "| | |-- "; return generateTargetConfigOnLeaf(qStart, qLeaf, q);
GraphComponent::print (os) }
<< " (waypoint) --> " << to ()->name ();
return os; bool LevelSetEdge::generateTargetConfig(core::NodePtr_t nStart,
} ConfigurationOut_t q) const {
// First, get an offset from the histogram that is not in the same connected
std::ostream& WaypointEdge::dotPrint (std::ostream& os, dot::DrawingAttributes da) const // component.
{ statistics::DiscreteDistribution<RoadmapNodePtr_t> distrib =
// First print the waypoint node, then the first edge. hist_->getDistribOutOfConnectedComponent(nStart->connectedComponent());
da ["style"]="dashed"; if (distrib.size() == 0) {
for (std::size_t i = 0; i < waypoints_.size (); ++i) hppDout(warning, "Edge " << name() << ": Distrib is empty");
waypoints_[i].second->dotPrint (os, da); return false;
}
da ["style"]="solid"; const Configuration_t &qLeaf = distrib()->configuration(),
for (std::size_t i = 0; i < waypoints_.size (); ++i) qStart = nStart->configuration();
waypoints_[i].first->dotPrint (os, da) << std::endl;
return generateTargetConfigOnLeaf(qStart, qLeaf, q);
da ["style"]="dotted"; }
da ["dir"] = "both";
da ["arrowtail"]="dot"; bool LevelSetEdge::generateTargetConfigOnLeaf(ConfigurationIn_t qStart,
// TODO: This is very ugly. There ought to be a better way of ConfigurationIn_t qLeaf,
// getting the real from() Node. ConfigurationOut_t q) const {
// We should be using Edge::dotPrint (...) instead of the following // First, set the offset.
// paragraph. ConstraintSetPtr_t cs = targetConstraint();
da.insert ("shape", "onormal"); const ConfigProjectorPtr_t cp = cs->configProjector();
da.insertWithQuote ("label", name()); assert(cp);
dot::Tooltip tp; tp.addLine ("Edge constains:");
populateTooltip (tp); // Set right hand side of edge constraints with qStart
da.insertWithQuote ("tooltip", tp.toStr()); cp->rightHandSideFromConfig(qStart);
da.insertWithQuote ("labeltooltip", tp.toStr()); // Set right hand side of constraints parameterizing the target state
os << waypoints_.back().second->id () << " -> " << to()->id () << " " << da << ";"; // foliation with qLeaf.
for (NumericalConstraints_t::const_iterator it =
return os; paramNumericalConstraints_.begin();
} it != paramNumericalConstraints_.end(); ++it) {
cp->rightHandSideFromConfig(*it, qLeaf);
std::ostream& LevelSetEdge::print (std::ostream& os) const }
{
os << "| | |-- "; // Eventually, do the projection.
GraphComponent::print (os) if (isShort_) q = qStart;
<< " (level set) --> " << to ()->name (); if (cs->apply(q)) return true;
return os; ::hpp::statistics::SuccessStatistics& ss = cp->statistics();
} if (ss.nbFailure() > ss.nbSuccess()) {
hppDout(warning, cs->name() << " fails often." << std::endl << ss);
std::ostream& LevelSetEdge::dotPrint (std::ostream& os, dot::DrawingAttributes da) const } else {
{ hppDout(warning, cs->name() << " succeeds at rate "
da.insert ("shape", "onormal"); << (value_type)(ss.nbSuccess()) /
da.insert ("style", "dashed"); (value_type)ss.numberOfObservations()
return Edge::dotPrint (os, da); << ".");
} }
return false;
void LevelSetEdge::populateTooltip (dot::Tooltip& tp) const }
{
GraphComponent::populateTooltip (tp); void LevelSetEdge::init(const LevelSetEdgeWkPtr_t& weak,
tp.addLine (""); const GraphWkPtr_t& graph, const StateWkPtr_t& from,
tp.addLine ("Foliation condition constraints:"); const StateWkPtr_t& to) {
for (NumericalConstraints_t::const_iterator it = condNumericalConstraints_.begin (); Edge::init(weak, graph, from, to);
it != condNumericalConstraints_.end (); ++it) { wkPtr_ = weak;
tp.addLine ("- " + (*it)->function ().name ()); }
}
for (LockedJoints_t::const_iterator it = condLockedJoints_.begin (); LevelSetEdgePtr_t LevelSetEdge::create(const std::string& name,
it != condLockedJoints_.end (); ++it) { const GraphWkPtr_t& graph,
tp.addLine ("- " + (*it)->jointName ()); const StateWkPtr_t& from,
} const StateWkPtr_t& to) {
tp.addLine ("Foliation parametrization constraints:"); LevelSetEdge* ptr = new LevelSetEdge(name);
for (NumericalConstraints_t::const_iterator it = paramNumericalConstraints_.begin (); LevelSetEdgePtr_t shPtr(ptr);
it != paramNumericalConstraints_.end (); ++it) { ptr->init(shPtr, graph, from, to);
tp.addLine ("- " + (*it)->function ().name ()); return shPtr;
} }
for (LockedJoints_t::const_iterator it = paramLockedJoints_.begin ();
it != paramLockedJoints_.end (); ++it) { LeafHistogramPtr_t LevelSetEdge::histogram() const { return hist_; }
tp.addLine ("- " + (*it)->jointName ());
} void LevelSetEdge::buildHistogram() {
} Foliation f;
bool LevelSetEdge::applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const /// Build the constraint set.
{ std::string n = "(" + name() + ")";
// First, get an offset from the histogram GraphPtr_t g = graph_.lock();
statistics::DiscreteDistribution < RoadmapNodePtr_t > distrib = hist_->getDistrib ();
if (distrib.size () == 0) { // The parametrizer
hppDout (warning, "Edge " << name() << ": Distrib is empty"); ConstraintSetPtr_t param = ConstraintSet::create(g->robot(), "Set " + n);
return false;
} ConfigProjectorPtr_t proj = ConfigProjector::create(
const Configuration_t& qlevelset = *(distrib ()->configuration ()); g->robot(), "projParam_" + n, g->errorThreshold(), g->maxIterations());
return applyConstraintsWithOffset (qoffset, qlevelset, q); for (const auto& nc : paramNumericalConstraints_) proj->add(nc);
}
param->addConstraint(proj);
bool LevelSetEdge::applyConstraints (core::NodePtr_t n_offset, ConfigurationOut_t q) const param->edge(wkPtr_.lock());
{
// First, get an offset from the histogram that is not in the same connected component. f.parametrizer(param);
statistics::DiscreteDistribution < RoadmapNodePtr_t > distrib = hist_->getDistribOutOfConnectedComponent (n_offset->connectedComponent ());
if (distrib.size () == 0) { // The codition
hppDout (warning, "Edge " << name() << ": Distrib is empty"); // TODO: We assumed that this part of the code can only be reached by
return false; // configurations that are valid.
} // It would be wiser to make sure configurations are valid, for instance
const Configuration_t& qlevelset = *(distrib ()->configuration ()), // by considering only configurations in the destination state of this
qoffset = *(n_offset->configuration ()); // edge.
ConstraintSetPtr_t cond = ConstraintSet::create(g->robot(), "Set " + n);
return applyConstraintsWithOffset (qoffset, qlevelset, q); proj = ConfigProjector::create(g->robot(), "projCond_" + n,
} g->errorThreshold(), g->maxIterations());
bool LevelSetEdge::applyConstraintsWithOffset (ConfigurationIn_t qoffset, for (const auto& nc : condNumericalConstraints_) proj->add(nc);
ConfigurationIn_t qlevelset, ConfigurationOut_t q) const
{ f.condition(cond);
// First, set the offset. cond->addConstraint(proj);
ConstraintSetPtr_t cs = configConstraint ();
const ConfigProjectorPtr_t cp = cs->configProjector (); hppDout(info, "Build histogram of LevelSetEdge " << name()
assert (cp); << "\nParametrizer:\n"
<< *param << "\nCondition:\n"
cp->rightHandSideFromConfig (qoffset); << *cond);
for (NumericalConstraints_t::const_iterator it =
paramNumericalConstraints_.begin (); // TODO: If hist_ is not NULL, remove the previous Histogram.
it != paramNumericalConstraints_.end (); ++it) { // It should not be of any use and it slows down node insertion in the
(*it)->rightHandSideFromConfig (qlevelset); // roadmap.
} hist_ = LeafHistogram::create(f);
for (LockedJoints_t::const_iterator it = paramLockedJoints_.begin (); g->insertHistogram(hist_);
it != paramLockedJoints_.end (); ++it) { }
(*it)->rightHandSideFromConfig (qlevelset);
} void LevelSetEdge::initialize() {
cp->updateRightHandSide (); if (!isInit_) {
Edge::initialize();
// Eventually, do the projection. buildHistogram();
if (isShort_) q = qoffset; }
if (cs->apply (q)) return true; }
::hpp::statistics::SuccessStatistics& ss = cp->statistics ();
if (ss.nbFailure () > ss.nbSuccess ()) { ConstraintSetPtr_t LevelSetEdge::buildTargetConstraint() {
hppDout (warning, cs->name () << " fails often." << std::endl << ss); std::string n = "(" + name() + ")";
} else { GraphPtr_t g = graph_.lock();
hppDout (warning, cs->name () << " succeeds at rate "
<< (value_type)(ss.nbSuccess ()) / ConstraintSetPtr_t constraint = ConstraintSet::create(g->robot(), "Set " + n);
(value_type) ss.numberOfObservations ()
<< "."); ConfigProjectorPtr_t proj = ConfigProjector::create(
} g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations());
return false;
} // Copy constraints from
// - graph,
void LevelSetEdge::init (const LevelSetEdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const NodeWkPtr_t& from, // - param numerical constraints
const NodeWkPtr_t& to) // - this edge,
{ // - the destination state,
Edge::init (weak, graph, from, to); // - the state in which the transition lies if different
wkPtr_ = weak;
} g->insertNumericalConstraints(proj);
for (const auto& nc : paramNumericalConstraints_) proj->add(nc);
LevelSetEdgePtr_t LevelSetEdge::create
(const std::string& name, const GraphWkPtr_t& graph, insertNumericalConstraints(proj);
const NodeWkPtr_t& from, const NodeWkPtr_t& to) stateTo()->insertNumericalConstraints(proj);
{ if (state() != stateTo()) {
LevelSetEdge* ptr = new LevelSetEdge (name); state()->insertNumericalConstraints(proj);
LevelSetEdgePtr_t shPtr (ptr); }
ptr->init(shPtr, graph, from, to); constraint->addConstraint(proj);
return shPtr;
} constraint->edge(wkPtr_.lock());
return constraint;
LeafHistogramPtr_t LevelSetEdge::histogram () const }
{
return hist_; void LevelSetEdge::insertParamConstraint(const constraints::ImplicitPtr_t& nm) {
} invalidate();
paramNumericalConstraints_.push_back(nm);
void LevelSetEdge::buildHistogram () }
{
Foliation f; const NumericalConstraints_t& LevelSetEdge::paramConstraints() const {
return paramNumericalConstraints_;
/// Build the constraint set. }
std::string n = "(" + name () + ")";
GraphPtr_t g = graph_.lock (); void LevelSetEdge::insertConditionConstraint(
const constraints::ImplicitPtr_t& nm) {
// The parametrizer invalidate();
ConstraintSetPtr_t param = ConstraintSet::create (g->robot (), "Set " + n); condNumericalConstraints_.push_back(nm);
}
ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "projParam_" + n, g->errorThreshold(), g->maxIterations());
IntervalsContainer_t::const_iterator itpdof = paramPassiveDofs_.begin (); const NumericalConstraints_t& LevelSetEdge::conditionConstraints() const {
for (NumericalConstraints_t::const_iterator it = paramNumericalConstraints_.begin (); return condNumericalConstraints_;
it != paramNumericalConstraints_.end (); ++it) { }
proj->add (*it, *itpdof);
++itpdof; LevelSetEdge::LevelSetEdge(const std::string& name) : Edge(name) {}
}
assert (itpdof == paramPassiveDofs_.end ()); LevelSetEdge::~LevelSetEdge() {}
} // namespace graph
param->addConstraint (proj); } // namespace manipulation
param->edge (wkPtr_.lock ()); } // namespace hpp
for (LockedJoints_t::const_iterator it = paramLockedJoints_.begin ();
it != paramLockedJoints_.end (); ++it) {
proj->add (*it);
}
f.parametrizer (param);
// The codition
// TODO: We assumed that this part of the code can only be reached by
// configurations that are valid.
// It would be wiser to make sure configurations are valid, for instance
// by considering only configurations in the destination node of this
// edge.
ConstraintSetPtr_t cond = ConstraintSet::create (g->robot (), "Set " + n);
proj = ConfigProjector::create(g->robot(), "projCond_" + n, g->errorThreshold(), g->maxIterations());
itpdof = condPassiveDofs_.begin ();
for (NumericalConstraints_t::const_iterator it = condNumericalConstraints_.begin ();
it != condNumericalConstraints_.end (); ++it) {
proj->add (*it, *itpdof);
++itpdof;
}
assert (itpdof == condPassiveDofs_.end ());
for (LockedJoints_t::const_iterator it = condLockedJoints_.begin ();
it != condLockedJoints_.end (); ++it) {
proj->add (*it);
}
f.condition (cond);
cond->addConstraint (proj);
// TODO: If hist_ is not NULL, remove the previous Histogram.
// It should not be of any use and it slows down node insertion in the
// roadmap.
hist_ = LeafHistogram::create (f);
g->insertHistogram (hist_);
}
ConstraintSetPtr_t LevelSetEdge::buildConfigConstraint() const
{
std::string n = "(" + name () + ")";
GraphPtr_t g = graph_.lock ();
ConstraintSetPtr_t constraint = ConstraintSet::create (g->robot (), "Set " + n);
ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations());
g->insertNumericalConstraints (proj);
IntervalsContainer_t::const_iterator itpdof = paramPassiveDofs_.begin ();
for (NumericalConstraints_t::const_iterator it = paramNumericalConstraints_.begin ();
it != paramNumericalConstraints_.end (); ++it) {
proj->add (*it, *itpdof);
++itpdof;
}
assert (itpdof == paramPassiveDofs_.end ());
insertNumericalConstraints (proj);
to ()->insertNumericalConstraints (proj);
constraint->addConstraint (proj);
g->insertLockedJoints (proj);
for (LockedJoints_t::const_iterator it = paramLockedJoints_.begin ();
it != paramLockedJoints_.end (); ++it) {
proj->add (*it);
}
insertLockedJoints (proj);
to ()->insertLockedJoints (proj);
constraint->edge (wkPtr_.lock ());
return constraint;
}
void LevelSetEdge::insertParamConstraint (const NumericalConstraintPtr_t& nm,
const SizeIntervals_t& passiveDofs)
{
paramNumericalConstraints_.push_back (nm);
paramPassiveDofs_.push_back (passiveDofs);
}
void LevelSetEdge::insertParamConstraint (const DifferentiableFunctionPtr_t function, const ComparisonTypePtr_t ineq)
{
insertParamConstraint (NumericalConstraint::create (function, ineq));
}
void LevelSetEdge::insertParamConstraint (const LockedJointPtr_t lockedJoint)
{
paramLockedJoints_.push_back (lockedJoint);
}
void LevelSetEdge::insertConditionConstraint (const NumericalConstraintPtr_t& nm,
const SizeIntervals_t& passiveDofs)
{
condNumericalConstraints_.push_back (nm);
condPassiveDofs_.push_back (passiveDofs);
}
void LevelSetEdge::insertConditionConstraint (const LockedJointPtr_t lockedJoint)
{
condLockedJoints_.push_back (lockedJoint);
}
LevelSetEdge::LevelSetEdge
(const std::string& name) :
Edge (name)
{
}
LevelSetEdge::~LevelSetEdge ()
{}
} // namespace graph
} // namespace manipulation
} // namespace hpp