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

Target

Select target project
  • gsaurel/hpp-manipulation
  • humanoid-path-planner/hpp-manipulation
2 results
Show changes
Commits on Source (201)
101 additional commits have been omitted to prevent performance issues.
Hide whitespace changes
Inline Side-by-side
Showing
with 919 additions and 763 deletions
.git-blame-ignore-revs 0 → 100644
View file @ 539aab52
# format (Guilhem Saurel, 2022-05-31)
fa649fc248b998a890e2730d8b56b238ca1e29df
# format (Guilhem Saurel, 2022-04-19)
2504d66d880b757858a5d90d2c7e4bf98e6e6139
# pre-commit run -a (Guilhem Saurel, 2022-10-05)
ccd75ad4f6e33d1690353b17383f4bca6dab2907
.github/workflows/nix.yml 0 → 100644
View file @ 539aab52
name: "CI - Nix"
on:
push:
jobs:
tests:
name: "Nix build on ${{ matrix.os }}"
runs-on: "${{ matrix.os }}-latest"
strategy:
matrix:
os: [ubuntu]
steps:
- uses: actions/checkout@v4
- uses: cachix/install-nix-action@v27
- uses: cachix/cachix-action@v15
with:
name: gepetto
authToken: '${{ secrets.CACHIX_AUTH_TOKEN }}'
- run: nix build -L
.gitlab-ci.yml
View file @ 539aab52
include: http://rainboard.laas.fr/project/hpp-manipulation/.gitlab-ci.yml
include: https://rainboard.laas.fr/project/hpp-manipulation/.gitlab-ci.yml
.mergify.yml 0 → 100644
View file @ 539aab52
pull_request_rules:
- name: merge automatically when CI passes and PR is approved
conditions:
- check-success = "gitlab-ci"
- check-success = "Nix build on ubuntu"
- check-success = "pre-commit.ci - pr"
- or:
- author = pre-commit-ci[bot]
- author = dependabot[bot]
actions:
merge:
.pre-commit-config.yaml 0 → 100644
View file @ 539aab52
ci:
autoupdate_branch: devel
repos:
- repo: https://github.com/astral-sh/ruff-pre-commit
rev: v0.8.1
hooks:
- id: ruff
args:
- --fix
- --exit-non-zero-on-fix
- id: ruff-format
- repo: https://github.com/cheshirekow/cmake-format-precommit
rev: v0.6.13
hooks:
- id: cmake-format
- repo: https://github.com/pappasam/toml-sort
rev: v0.24.2
hooks:
- id: toml-sort-fix
exclude: poetry.lock
- repo: https://github.com/pre-commit/mirrors-clang-format
rev: v19.1.4
hooks:
- id: clang-format
args:
- --style=Google
- repo: https://github.com/pre-commit/pre-commit-hooks
rev: v5.0.0
hooks:
- id: check-added-large-files
- id: check-ast
- id: check-executables-have-shebangs
- id: check-json
- id: check-merge-conflict
- id: check-symlinks
- id: check-toml
- id: check-yaml
- id: debug-statements
- id: destroyed-symlinks
- id: detect-private-key
- id: end-of-file-fixer
- id: fix-byte-order-marker
- id: mixed-line-ending
- id: trailing-whitespace
CMakeLists.txt
View file @ 539aab52
#
# Copyright (c) 2014, 2020 CNRS
# Authors: Florent Lamiraux, Guilhem Saurel
#
# Copyright (c) 2014, 2020 CNRS Authors: Florent Lamiraux, Guilhem Saurel
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are
# met:
# 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 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.
# 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.
CMAKE_MINIMUM_REQUIRED(VERSION 3.1)
SET(PROJECT_NAME hpp-manipulation)
SET(PROJECT_DESCRIPTION "Classes for manipulation planning.")
SET(PROJECT_USE_CMAKE_EXPORT TRUE)
SET(CXX_DISABLE_WERROR TRUE)
INCLUDE(cmake/hpp.cmake)
INCLUDE(cmake/boost.cmake)
INCLUDE(cmake/test.cmake)
COMPUTE_PROJECT_ARGS(PROJECT_ARGS LANGUAGES CXX)
PROJECT(${PROJECT_NAME} ${PROJECT_ARGS})
LIST(APPEND PKG_CONFIG_ADDITIONAL_VARIABLES cmake_plugin)
# 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.
cmake_minimum_required(VERSION 3.10)
set(PROJECT_NAME hpp-manipulation)
set(PROJECT_DESCRIPTION "Classes for manipulation planning.")
set(PROJECT_USE_CMAKE_EXPORT TRUE)
set(CXX_DISABLE_WERROR TRUE)
# Check if the submodule cmake have been initialized
set(JRL_CMAKE_MODULES "${CMAKE_CURRENT_LIST_DIR}/cmake")
if(EXISTS "${JRL_CMAKE_MODULES}/base.cmake")
message(STATUS "JRL cmakemodules found in 'cmake/' git submodule")
else()
find_package(jrl-cmakemodules QUIET CONFIG)
if(jrl-cmakemodules_FOUND)
get_property(
JRL_CMAKE_MODULES
TARGET jrl-cmakemodules::jrl-cmakemodules
PROPERTY INTERFACE_INCLUDE_DIRECTORIES)
message(STATUS "JRL cmakemodules found on system at ${JRL_CMAKE_MODULES}")
elseif(${CMAKE_VERSION} VERSION_LESS "3.14.0")
message(
FATAL_ERROR
"\nCan't find jrl-cmakemodules. Please either:\n"
" - use git submodule: 'git submodule update --init'\n"
" - or install https://github.com/jrl-umi3218/jrl-cmakemodules\n"
" - or upgrade your CMake version to >= 3.14 to allow automatic fetching\n"
)
else()
message(STATUS "JRL cmakemodules not found. Let's fetch it.")
include(FetchContent)
FetchContent_Declare(
"jrl-cmakemodules"
GIT_REPOSITORY "https://github.com/jrl-umi3218/jrl-cmakemodules.git")
FetchContent_MakeAvailable("jrl-cmakemodules")
FetchContent_GetProperties("jrl-cmakemodules" SOURCE_DIR JRL_CMAKE_MODULES)
endif()
endif()
include("${JRL_CMAKE_MODULES}/hpp.cmake")
include("${JRL_CMAKE_MODULES}/boost.cmake")
include("${JRL_CMAKE_MODULES}/test.cmake")
compute_project_args(PROJECT_ARGS LANGUAGES CXX)
project(${PROJECT_NAME} ${PROJECT_ARGS})
list(APPEND PKG_CONFIG_ADDITIONAL_VARIABLES cmake_plugin)
# Ask Doxygen to create a tree view in html documentation
SET(DOXYGEN_TREEVIEW "NO" CACHE STRING "Set to YES to generate a tree view in the html documentation")
ADD_PROJECT_DEPENDENCY(Boost REQUIRED COMPONENTS regex)
ADD_PROJECT_DEPENDENCY("hpp-core" REQUIRED)
IF(BUILD_TESTING)
FIND_PACKAGE(Boost REQUIRED COMPONENTS unit_test_framework)
ADD_PROJECT_DEPENDENCY("example-robot-data" REQUIRED)
ENDIF()
CONFIG_FILES(doc/main.hh
include/hpp/manipulation/package-config.hh)
SET(${PROJECT_NAME}_HEADERS
${CMAKE_BINARY_DIR}/include/hpp/manipulation/package-config.hh
include/hpp/manipulation/fwd.hh
include/hpp/manipulation/handle.hh
include/hpp/manipulation/problem.hh
include/hpp/manipulation/problem-solver.hh
include/hpp/manipulation/device.hh
include/hpp/manipulation/weighed-distance.hh
include/hpp/manipulation/constraint-set.hh
include/hpp/manipulation/roadmap.hh
include/hpp/manipulation/roadmap-node.hh
include/hpp/manipulation/connected-component.hh
include/hpp/manipulation/leaf-connected-comp.hh
include/hpp/manipulation/manipulation-planner.hh
include/hpp/manipulation/graph-path-validation.hh
include/hpp/manipulation/graph-optimizer.hh
include/hpp/manipulation/graph/state.hh
include/hpp/manipulation/graph/edge.hh
include/hpp/manipulation/graph/state-selector.hh
include/hpp/manipulation/graph/guided-state-selector.hh
include/hpp/manipulation/graph/graph.hh
include/hpp/manipulation/graph/statistics.hh
include/hpp/manipulation/graph/graph-component.hh
include/hpp/manipulation/graph/fwd.hh
include/hpp/manipulation/graph/dot.hh
include/hpp/manipulation/graph/helper.hh
include/hpp/manipulation/graph/validation.hh
include/hpp/manipulation/path-optimization/enforce-transition-semantic.hh
include/hpp/manipulation/path-optimization/random-shortcut.hh
include/hpp/manipulation/path-optimization/spline-gradient-based.hh
include/hpp/manipulation/path-planner/end-effector-trajectory.hh
include/hpp/manipulation/path-planner/in-state-path.hh
include/hpp/manipulation/path-planner/states-path-finder.hh
include/hpp/manipulation/problem-target/state.hh
include/hpp/manipulation/serialization.hh
include/hpp/manipulation/steering-method/cross-state-optimization.hh
include/hpp/manipulation/steering-method/fwd.hh
include/hpp/manipulation/steering-method/graph.hh
include/hpp/manipulation/steering-method/end-effector-trajectory.hh
)
SET(${PROJECT_NAME}_SOURCES
src/handle.cc
src/manipulation-planner.cc
src/problem-solver.cc
src/roadmap.cc
src/connected-component.cc
src/leaf-connected-comp.cc
src/constraint-set.cc
src/roadmap-node.cc
src/device.cc
src/weighed-distance.cc
src/problem.cc
src/graph-path-validation.cc
src/graph-optimizer.cc
src/graph/state.cc
src/graph/edge.cc
src/graph/graph.cc
src/graph/graph-component.cc
src/graph/state-selector.cc
src/graph/guided-state-selector.cc
src/graph/statistics.cc
src/graph/helper.cc
src/graph/dot.cc
src/graph/validation.cc
src/path-optimization/random-shortcut.cc
src/path-optimization/enforce-transition-semantic.cc
src/path-planner/end-effector-trajectory.cc
src/path-planner/states-path-finder.cc
src/problem-target/state.cc
src/serialization.cc
src/steering-method/end-effector-trajectory.cc
src/steering-method/cross-state-optimization.cc
src/steering-method/graph.cc
)
ADD_LIBRARY(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS})
TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} PUBLIC $<INSTALL_INTERFACE:include>)
TARGET_LINK_LIBRARIES(${PROJECT_NAME} hpp-core::hpp-core Boost::regex)
INSTALL(TARGETS ${PROJECT_NAME} EXPORT ${TARGETS_EXPORT_NAME} DESTINATION lib)
set(DOXYGEN_TREEVIEW
"NO"
CACHE STRING "Set to YES to generate a tree view in the html documentation")
add_project_dependency(Boost REQUIRED COMPONENTS regex)
add_project_dependency("hpp-core" REQUIRED)
if(BUILD_TESTING)
find_package(Boost REQUIRED COMPONENTS unit_test_framework)
add_project_dependency("example-robot-data" REQUIRED)
endif()
config_files(doc/main.hh include/hpp/manipulation/package-config.hh)
set(${PROJECT_NAME}_HEADERS
${CMAKE_BINARY_DIR}/include/hpp/manipulation/package-config.hh
include/hpp/manipulation/fwd.hh
include/hpp/manipulation/handle.hh
include/hpp/manipulation/problem.hh
include/hpp/manipulation/problem-solver.hh
include/hpp/manipulation/device.hh
include/hpp/manipulation/weighed-distance.hh
include/hpp/manipulation/constraint-set.hh
include/hpp/manipulation/roadmap.hh
include/hpp/manipulation/roadmap-node.hh
include/hpp/manipulation/connected-component.hh
include/hpp/manipulation/leaf-connected-comp.hh
include/hpp/manipulation/manipulation-planner.hh
include/hpp/manipulation/graph-path-validation.hh
include/hpp/manipulation/graph-optimizer.hh
include/hpp/manipulation/graph/state.hh
include/hpp/manipulation/graph/edge.hh
include/hpp/manipulation/graph/state-selector.hh
include/hpp/manipulation/graph/guided-state-selector.hh
include/hpp/manipulation/graph/graph.hh
include/hpp/manipulation/graph/statistics.hh
include/hpp/manipulation/graph/graph-component.hh
include/hpp/manipulation/graph/fwd.hh
include/hpp/manipulation/graph/dot.hh
include/hpp/manipulation/graph/helper.hh
include/hpp/manipulation/graph/validation.hh
include/hpp/manipulation/path-optimization/enforce-transition-semantic.hh
include/hpp/manipulation/path-optimization/random-shortcut.hh
include/hpp/manipulation/path-optimization/spline-gradient-based.hh
include/hpp/manipulation/path-planner/end-effector-trajectory.hh
include/hpp/manipulation/path-planner/states-path-finder.hh
include/hpp/manipulation/path-planner/transition-planner.hh
include/hpp/manipulation/problem-target/state.hh
include/hpp/manipulation/serialization.hh
include/hpp/manipulation/steering-method/cross-state-optimization.hh
include/hpp/manipulation/steering-method/fwd.hh
include/hpp/manipulation/steering-method/graph.hh
include/hpp/manipulation/steering-method/end-effector-trajectory.hh)
set(${PROJECT_NAME}_SOURCES
src/handle.cc
src/manipulation-planner.cc
src/problem-solver.cc
src/roadmap.cc
src/connected-component.cc
src/leaf-connected-comp.cc
src/constraint-set.cc
src/roadmap-node.cc
src/device.cc
src/weighed-distance.cc
src/problem.cc
src/graph-path-validation.cc
src/graph-optimizer.cc
src/graph/state.cc
src/graph/edge.cc
src/graph/graph.cc
src/graph/graph-component.cc
src/graph/state-selector.cc
src/graph/guided-state-selector.cc
src/graph/statistics.cc
src/graph/helper.cc
src/graph/dot.cc
src/graph/validation.cc
src/path-optimization/random-shortcut.cc
src/path-optimization/enforce-transition-semantic.cc
src/path-planner/end-effector-trajectory.cc
src/path-planner/states-path-finder.cc
src/path-planner/transition-planner.cc
src/problem-target/state.cc
src/serialization.cc
src/steering-method/end-effector-trajectory.cc
src/steering-method/cross-state-optimization.cc
src/steering-method/graph.cc)
add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SOURCES}
${${PROJECT_NAME}_HEADERS})
target_include_directories(
${PROJECT_NAME} PUBLIC $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>)
target_link_libraries(${PROJECT_NAME} hpp-core::hpp-core Boost::regex)
install(
TARGETS ${PROJECT_NAME}
EXPORT ${TARGETS_EXPORT_NAME}
DESTINATION lib)
# GPL licenced part
SET(${PROJECT_NAME}_SOURCES_GPL
src/path-optimization/spline-gradient-based.cc
)
set(${PROJECT_NAME}_SOURCES_GPL src/path-optimization/spline-gradient-based.cc)
ADD_LIBRARY(${PROJECT_NAME}-gpl SHARED ${${PROJECT_NAME}_SOURCES_GPL})
TARGET_LINK_LIBRARIES(${PROJECT_NAME}-gpl ${PROJECT_NAME} hpp-core::hpp-core-gpl)
add_library(${PROJECT_NAME}-gpl SHARED ${${PROJECT_NAME}_SOURCES_GPL})
target_link_libraries(${PROJECT_NAME}-gpl ${PROJECT_NAME}
hpp-core::hpp-core-gpl)
INSTALL(TARGETS ${PROJECT_NAME}-gpl EXPORT ${TARGETS_EXPORT_NAME} DESTINATION lib)
install(
TARGETS ${PROJECT_NAME}-gpl
EXPORT ${TARGETS_EXPORT_NAME}
DESTINATION lib)
ADD_SUBDIRECTORY(plugins)
IF(BUILD_TESTING)
ADD_SUBDIRECTORY(tests)
ENDIF()
add_subdirectory(plugins)
if(BUILD_TESTING)
add_subdirectory(tests)
endif()
PKG_CONFIG_APPEND_LIBS(${PROJECT_NAME})
pkg_config_append_libs(${PROJECT_NAME})
INSTALL(FILES package.xml DESTINATION share/${PROJECT_NAME})
install(FILES package.xml DESTINATION share/${PROJECT_NAME})
COPYING → LICENSE
View file @ 539aab52
File moved
README.md
View file @ 539aab52
# hpp-manipulation
[![Building Status](https://travis-ci.org/humanoid-path-planner/hpp-manipulation.svg?branch=master)](https://travis-ci.org/humanoid-path-planner/hpp-manipulation)
[![Pipeline status](https://gepgitlab.laas.fr/humanoid-path-planner/hpp-manipulation/badges/master/pipeline.svg)](https://gepgitlab.laas.fr/humanoid-path-planner/hpp-manipulation/commits/master)
[![Coverage report](https://gepgitlab.laas.fr/humanoid-path-planner/hpp-manipulation/badges/master/coverage.svg?job=doc-coverage)](http://projects.laas.fr/gepetto/doc/humanoid-path-planner/hpp-manipulation/master/coverage/)
[![Pipeline status](https://gitlab.laas.fr/humanoid-path-planner/hpp-manipulation/badges/master/pipeline.svg)](https://gitlab.laas.fr/humanoid-path-planner/hpp-manipulation/commits/master)
[![Coverage report](https://gitlab.laas.fr/humanoid-path-planner/hpp-manipulation/badges/master/coverage.svg?job=doc-coverage)](https://gepettoweb.laas.fr/doc/humanoid-path-planner/hpp-manipulation/master/coverage/)
[![Code style: black](https://img.shields.io/badge/code%20style-black-000000.svg)](https://github.com/psf/black)
[![pre-commit.ci status](https://results.pre-commit.ci/badge/github/humanoid-path-planner/hpp-manipulation/master.svg)](https://results.pre-commit.ci/latest/github/humanoid-path-planner/hpp-manipulation)
This package is part of the [HPP] software and extends the functionalities of hpp-core.
It implements a solver for manipulation planning problems.
### Version
1.1
### Dependencies
[hpp-manipulation] needs the following package to be installed:
......
TODO.txt
View file @ 539aab52
......@@ -17,7 +17,7 @@
hard-coded optimizer (Graph-RandomShortcut) and then globally with the
command ps.addPathOptimizer("Graph-RandomShortcut"). Due to how this works,
this is like doing two times the same thing so the first should be removed.
However bugs have been observed when the global optimization is used (Core
However bugs have been observed when the global optimization is used (Core
dumped). For unknown reasons, in GraphOptimizer::optimize, the dynamic
cast of current->constraints() may fail on *some* 0-length paths. Ignoring
empty paths (the "if" I have added) seems to make the problem disappear
......
cmake @ 29c0eb4e
Compare ee7a773c...29c0eb4e
Subproject commit ee7a773c5c23f83dd21eb0ccfa96277e068b0456
Subproject commit 29c0eb4e659304f44d55a0389e2749812d858659
doc/Doxyfile.extra.in
View file @ 539aab52
......@@ -8,3 +8,4 @@ ALIASES += Link{1}="\ref \1"
ALIASES += Link{2}="\ref \1 \"\2\""
ALIASES += LHPP{2}="\Link{hpp::\1::\2,\2}"
ALIASES += LPinocchio{1}="\LHPP{pinocchio,\1}"
USE_MATHJAX= YES
flake.lock 0 → 100644
View file @ 539aab52
{
"nodes": {
"flake-parts": {
"inputs": {
"nixpkgs-lib": [
"nixpkgs"
]
},
"locked": {
"lastModified": 1719877454,
"narHash": "sha256-g5N1yyOSsPNiOlFfkuI/wcUjmtah+nxdImJqrSATjOU=",
"owner": "hercules-ci",
"repo": "flake-parts",
"rev": "4e3583423212f9303aa1a6337f8dffb415920e4f",
"type": "github"
},
"original": {
"owner": "hercules-ci",
"repo": "flake-parts",
"type": "github"
}
},
"nixpkgs": {
"locked": {
"lastModified": 1727174734,
"narHash": "sha256-xa3TynMF5vaWonmTOg/Ejc1Fmo0GkQnCaVRVkBc3z2I=",
"owner": "gepetto",
"repo": "nixpkgs",
"rev": "0ad139a0e4372abc12320c8c92ee90e0e5e296e1",
"type": "github"
},
"original": {
"owner": "gepetto",
"repo": "nixpkgs",
"type": "github"
}
},
"root": {
"inputs": {
"flake-parts": "flake-parts",
"nixpkgs": "nixpkgs"
}
}
},
"root": "root",
"version": 7
}
flake.nix 0 → 100644
View file @ 539aab52
{
description = "Classes for manipulation planning";
inputs = {
nixpkgs.url = "github:gepetto/nixpkgs";
flake-parts = {
url = "github:hercules-ci/flake-parts";
inputs.nixpkgs-lib.follows = "nixpkgs";
};
};
outputs =
inputs:
inputs.flake-parts.lib.mkFlake { inherit inputs; } {
systems = [
"x86_64-linux"
"aarch64-linux"
"aarch64-darwin"
"x86_64-darwin"
];
perSystem =
{ pkgs, self', ... }:
{
devShells.default = pkgs.mkShell { inputsFrom = [ self'.packages.default ]; };
packages = {
default = self'.packages.hpp-manipulation;
hpp-manipulation = pkgs.hpp-manipulation.overrideAttrs (_: {
src = pkgs.lib.fileset.toSource {
root = ./.;
fileset = pkgs.lib.fileset.unions [
./CMakeLists.txt
./doc
./include
./package.xml
./plugins
./src
./tests
];
};
});
};
};
};
}
include/hpp/manipulation/connected-component.hh
View file @ 539aab52
......@@ -36,42 +36,45 @@
#include <hpp/manipulation/graph/fwd.hh>
namespace hpp {
namespace manipulation {
/// Extension of hpp::core::connected-component. Adds a list of roadmap nodes for
/// every contraint graph state within the connected component. Thus every roadmap
/// node is assigned to a grahp state, which minimises computation time.
class HPP_MANIPULATION_DLLAPI ConnectedComponent : public core::ConnectedComponent
{
public:
/// Map of graph states within the connected component
typedef std::map <graph::StatePtr_t, RoadmapNodes_t> GraphStates_t;
ConnectedComponent() {}
namespace manipulation {
/// Extension of hpp::core::connected-component. Adds a list of roadmap nodes
/// for every contraint graph state within the connected component. Thus every
/// roadmap node is assigned to a grahp state, which minimises computation time.
class HPP_MANIPULATION_DLLAPI ConnectedComponent
: public core::ConnectedComponent {
public:
/// Map of graph states within the connected component
typedef std::map<graph::StatePtr_t, RoadmapNodes_t> GraphStates_t;
/// return a shared pointer to new instance of manipulation::ConnectedComponent
static ConnectedComponentPtr_t create (const RoadmapWkPtr_t& roadmap);
ConnectedComponent() {}
/// Merge two connected components (extension of core::ConnectedComponent::merge)
/// \param other manipulation connected component to merge into this one.
/// \note other will be empty after calling this method.
void merge (const core::ConnectedComponentPtr_t& otherCC);
/// Add roadmap node to connected component
/// \param roadmap node to be added
void addNode (const core::NodePtr_t& node);
/// return a shared pointer to new instance of
/// manipulation::ConnectedComponent
static ConnectedComponentPtr_t create(const RoadmapWkPtr_t& roadmap);
const RoadmapNodes_t& getRoadmapNodes (const graph::StatePtr_t graphState) const;
protected:
private:
bool check () const;
GraphStates_t graphStateMap_;
// a RoadmapWkPtr_t so that memory can be released ?
RoadmapWkPtr_t roadmap_;
static RoadmapNodes_t empty_;
/// Merge two connected components (extension of
/// core::ConnectedComponent::merge) \param other manipulation connected
/// component to merge into this one. \note other will be empty after calling
/// this method.
void merge(const core::ConnectedComponentPtr_t& otherCC);
HPP_SERIALIZABLE();
}; // class ConnectedComponent
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_CONNECTED_COMPONENT_HH
/// Add roadmap node to connected component
/// \param roadmap node to be added
void addNode(const core::NodePtr_t& node);
const RoadmapNodes_t& getRoadmapNodes(
const graph::StatePtr_t graphState) const;
protected:
private:
bool check() const;
GraphStates_t graphStateMap_;
// a RoadmapWkPtr_t so that memory can be released ?
RoadmapWkPtr_t roadmap_;
static RoadmapNodes_t empty_;
HPP_SERIALIZABLE();
}; // class ConnectedComponent
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_CONNECTED_COMPONENT_HH
include/hpp/manipulation/constraint-set.hh
View file @ 539aab52
......@@ -27,74 +27,68 @@
// DAMAGE.
#ifndef HPP_MANIPULATION_CONSTRAINT_SET_HH
# define HPP_MANIPULATION_CONSTRAINT_SET_HH
#define HPP_MANIPULATION_CONSTRAINT_SET_HH
# include <hpp/core/constraint-set.hh>
# include <hpp/manipulation/fwd.hh>
# include <hpp/manipulation/graph/fwd.hh>
# include <hpp/manipulation/config.hh>
#include <hpp/core/constraint-set.hh>
#include <hpp/manipulation/config.hh>
#include <hpp/manipulation/fwd.hh>
#include <hpp/manipulation/graph/fwd.hh>
namespace hpp {
namespace manipulation {
/// \addtogroup constraints
/// \{
/// a core::ConstraintSet remembering which edge created it
class HPP_MANIPULATION_DLLAPI ConstraintSet : public core::ConstraintSet
{
public:
typedef core::ConstraintSet Parent_t;
/// Return shared pointer to new object
static ConstraintSetPtr_t create (const DevicePtr_t& robot,
const std::string& name);
/// Return shared pointer to new object
static ConstraintSetPtr_t createCopy (const ConstraintSetPtr_t& cs);
/// return shared pointer to copy
virtual ConstraintPtr_t copy () const;
void edge (graph::EdgePtr_t edge);
graph::EdgePtr_t edge () const;
protected:
/// Constructor
ConstraintSet (const DevicePtr_t& robot, const std::string& name);
/// Copy constructor
ConstraintSet (const ConstraintSet& other);
/// Store weak pointer to itself.
void init (const ConstraintSetPtr_t& self);
virtual std::ostream& print (std::ostream& os) const;
private:
graph::EdgePtr_t edge_;
ConstraintSetWkPtr_t weak_;
ConstraintSet() {}
HPP_SERIALIZABLE();
}; // class ConstraintSet
struct ConstraintAndComplement_t {
ImplicitPtr_t constraint;
ImplicitPtr_t complement;
ImplicitPtr_t both;
ConstraintAndComplement_t (const ImplicitPtr_t& constr,
const ImplicitPtr_t& comp,
const ImplicitPtr_t& b) :
constraint (constr), complement (comp), both (b)
{
}
};
typedef std::vector <ConstraintAndComplement_t>
ConstraintsAndComplements_t;
/// \}
} // namespace manipulation
} // namespace hpp
namespace manipulation {
/// \addtogroup constraints
/// \{
/// a core::ConstraintSet remembering which edge created it
class HPP_MANIPULATION_DLLAPI ConstraintSet : public core::ConstraintSet {
public:
typedef core::ConstraintSet Parent_t;
/// Return shared pointer to new object
static ConstraintSetPtr_t create(const DevicePtr_t& robot,
const std::string& name);
/// Return shared pointer to new object
static ConstraintSetPtr_t createCopy(const ConstraintSetPtr_t& cs);
/// return shared pointer to copy
virtual ConstraintPtr_t copy() const;
void edge(graph::EdgePtr_t edge);
graph::EdgePtr_t edge() const;
protected:
/// Constructor
ConstraintSet(const DevicePtr_t& robot, const std::string& name);
/// Copy constructor
ConstraintSet(const ConstraintSet& other);
/// Store weak pointer to itself.
void init(const ConstraintSetPtr_t& self);
virtual std::ostream& print(std::ostream& os) const;
private:
graph::EdgePtr_t edge_;
ConstraintSetWkPtr_t weak_;
ConstraintSet() {}
HPP_SERIALIZABLE();
}; // class ConstraintSet
struct ConstraintAndComplement_t {
ImplicitPtr_t constraint;
ImplicitPtr_t complement;
ImplicitPtr_t both;
ConstraintAndComplement_t(const ImplicitPtr_t& constr,
const ImplicitPtr_t& comp, const ImplicitPtr_t& b)
: constraint(constr), complement(comp), both(b) {}
};
typedef std::vector<ConstraintAndComplement_t> ConstraintsAndComplements_t;
/// \}
} // namespace manipulation
} // namespace hpp
BOOST_CLASS_EXPORT_KEY(hpp::manipulation::ConstraintSet)
#endif // HPP_MANIPULATION_CONSTRAINT_SET_HH
#endif // HPP_MANIPULATION_CONSTRAINT_SET_HH
include/hpp/manipulation/device.hh
View file @ 539aab52
......@@ -29,91 +29,84 @@
// DAMAGE.
#ifndef HPP_MANIPULATION_DEVICE_HH
# define HPP_MANIPULATION_DEVICE_HH
#define HPP_MANIPULATION_DEVICE_HH
# include <hpp/pinocchio/humanoid-robot.hh>
#include <hpp/core/container.hh>
#include <hpp/manipulation/config.hh>
#include <hpp/manipulation/fwd.hh>
#include <hpp/pinocchio/humanoid-robot.hh>
# include <hpp/core/container.hh>
namespace hpp {
namespace manipulation {
/// Device with handles.
///
/// As a deriving class of hpp::pinocchio::HumanoidRobot,
/// it is compatible with hpp::pinocchio::urdf::loadHumanoidRobot
///
/// This class also contains pinocchio::Gripper, Handle and \ref
/// JointAndShapes_t
class HPP_MANIPULATION_DLLAPI Device : public pinocchio::HumanoidRobot {
public:
typedef pinocchio::HumanoidRobot Parent_t;
# include <hpp/manipulation/fwd.hh>
# include <hpp/manipulation/config.hh>
/// Constructor
/// \param name of the new instance,
static DevicePtr_t create(const std::string& name) {
Device* ptr = new Device(name);
DevicePtr_t shPtr(ptr);
ptr->init(shPtr);
return shPtr;
}
namespace hpp {
namespace manipulation {
/// Device with handles.
///
/// As a deriving class of hpp::pinocchio::HumanoidRobot,
/// it is compatible with hpp::pinocchio::urdf::loadHumanoidRobot
///
/// This class also contains pinocchio::Gripper, Handle and \ref JointAndShapes_t
class HPP_MANIPULATION_DLLAPI Device : public pinocchio::HumanoidRobot
{
public:
typedef pinocchio::HumanoidRobot Parent_t;
/// Constructor
/// \param name of the new instance,
static DevicePtr_t create (const std::string& name)
{
Device* ptr = new Device (name);
DevicePtr_t shPtr (ptr);
ptr->init (shPtr);
return shPtr;
}
DevicePtr_t self () const { return self_.lock(); }
/// Print object in a stream
virtual std::ostream& print (std::ostream& os) const;
void setRobotRootPosition (const std::string& robotName,
const Transform3f& positionWRTParentJoint);
virtual pinocchio::DevicePtr_t clone () const;
std::vector<std::string> robotNames () const;
FrameIndices_t robotFrames (const std::string& robotName) const;
void removeJoints(const std::vector<std::string>& jointNames,
Configuration_t referenceConfig);
core::Container <HandlePtr_t> handles;
core::Container <GripperPtr_t> grippers;
core::Container <JointAndShapes_t> jointAndShapes;
protected:
/// Constructor
/// \param name of the new instance,
/// \param robot Robots that manipulate objects,
/// \param objects Set of objects manipulated by the robot.
Device (const std::string& name) :
Parent_t (name)
{}
void init (const DeviceWkPtr_t& self)
{
Parent_t::init (self);
self_ = self;
}
void initCopy (const DeviceWkPtr_t& self, const Device& other)
{
Parent_t::initCopy (self, other);
self_ = self;
}
/// For serialization only
Device() {}
private:
DeviceWkPtr_t self_;
HPP_SERIALIZABLE();
}; // class Device
} // namespace manipulation
} // namespace hpp
DevicePtr_t self() const { return self_.lock(); }
/// Print object in a stream
virtual std::ostream& print(std::ostream& os) const;
void setRobotRootPosition(const std::string& robotName,
const Transform3s& positionWRTParentJoint);
virtual pinocchio::DevicePtr_t clone() const;
std::vector<std::string> robotNames() const;
FrameIndices_t robotFrames(const std::string& robotName) const;
void removeJoints(const std::vector<std::string>& jointNames,
Configuration_t referenceConfig);
core::Container<HandlePtr_t> handles;
core::Container<GripperPtr_t> grippers;
core::Container<JointAndShapes_t> jointAndShapes;
protected:
/// Constructor
/// \param name of the new instance,
/// \param robot Robots that manipulate objects,
/// \param objects Set of objects manipulated by the robot.
Device(const std::string& name) : Parent_t(name) {}
void init(const DeviceWkPtr_t& self) {
Parent_t::init(self);
self_ = self;
}
void initCopy(const DeviceWkPtr_t& self, const Device& other) {
Parent_t::initCopy(self, other);
self_ = self;
}
/// For serialization only
Device() {}
private:
DeviceWkPtr_t self_;
HPP_SERIALIZABLE();
}; // class Device
} // namespace manipulation
} // namespace hpp
BOOST_CLASS_EXPORT_KEY(hpp::manipulation::Device)
#endif // HPP_MANIPULATION_DEVICE_HH
#endif // HPP_MANIPULATION_DEVICE_HH
include/hpp/manipulation/fwd.hh
View file @ 539aab52
......@@ -29,146 +29,147 @@
// DAMAGE.
#ifndef HPP_MANIPULATION_FWD_HH
# define HPP_MANIPULATION_FWD_HH
#define HPP_MANIPULATION_FWD_HH
# include <map>
# include <hpp/manipulation/config.hh>
# include <hpp/core/fwd.hh>
#include <hpp/core/fwd.hh>
#include <map>
namespace hpp {
namespace manipulation {
HPP_PREDEF_CLASS (Device);
typedef shared_ptr <Device> DevicePtr_t;
typedef shared_ptr <const Device> DeviceConstPtr_t;
typedef pinocchio::Joint Joint;
typedef pinocchio::JointPtr_t JointPtr_t;
typedef pinocchio::JointIndex JointIndex;
typedef std::vector<JointIndex> JointIndices_t;
typedef pinocchio::FrameIndex FrameIndex;
typedef std::vector<pinocchio::FrameIndex> FrameIndices_t;
typedef pinocchio::Configuration_t Configuration_t;
typedef pinocchio::ConfigurationIn_t ConfigurationIn_t;
typedef pinocchio::ConfigurationOut_t ConfigurationOut_t;
typedef core::ConfigurationPtr_t ConfigurationPtr_t;
typedef pinocchio::GripperPtr_t GripperPtr_t;
typedef pinocchio::LiegroupElement LiegroupElement;
typedef pinocchio::LiegroupSpace LiegroupSpace;
typedef pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t;
HPP_PREDEF_CLASS (AxialHandle);
typedef shared_ptr <AxialHandle> AxialHandlePtr_t;
HPP_PREDEF_CLASS (Handle);
typedef shared_ptr <Handle> HandlePtr_t;
HPP_PREDEF_CLASS (Object);
typedef shared_ptr <Object> ObjectPtr_t;
typedef shared_ptr <const Object> ObjectConstPtr_t;
HPP_PREDEF_CLASS (ProblemSolver);
typedef ProblemSolver* ProblemSolverPtr_t;
HPP_PREDEF_CLASS (Problem);
typedef shared_ptr <Problem> ProblemPtr_t;
typedef shared_ptr <const Problem> ProblemConstPtr_t;
HPP_PREDEF_CLASS (Roadmap);
typedef shared_ptr <Roadmap> RoadmapPtr_t;
HPP_PREDEF_CLASS (RoadmapNode);
typedef RoadmapNode* RoadmapNodePtr_t;
typedef std::vector<RoadmapNodePtr_t> RoadmapNodes_t;
HPP_PREDEF_CLASS (ConnectedComponent);
typedef shared_ptr<ConnectedComponent> ConnectedComponentPtr_t;
HPP_PREDEF_CLASS (LeafConnectedComp);
typedef shared_ptr<LeafConnectedComp> LeafConnectedCompPtr_t;
typedef shared_ptr<const LeafConnectedComp>
LeafConnectedCompConstPtr_t;
typedef std::set<LeafConnectedCompPtr_t> LeafConnectedComps_t;
HPP_PREDEF_CLASS (WeighedLeafConnectedComp);
typedef shared_ptr<WeighedLeafConnectedComp> WeighedLeafConnectedCompPtr_t;
HPP_PREDEF_CLASS (WeighedDistance);
typedef shared_ptr<WeighedDistance> WeighedDistancePtr_t;
typedef constraints::RelativeOrientation RelativeOrientation;
typedef constraints::RelativePosition RelativePosition;
typedef constraints::RelativeOrientationPtr_t RelativeOrientationPtr_t;
typedef constraints::RelativePositionPtr_t RelativePositionPtr_t;
typedef constraints::RelativeTransformation RelativeTransformation;
typedef constraints::RelativeTransformationR3xSO3
RelativeTransformationR3xSO3;
typedef constraints::RelativeTransformationPtr_t
RelativeTransformationPtr_t;
typedef core::value_type value_type;
typedef core::size_type size_type;
typedef core::Transform3f Transform3f;
typedef core::vector_t vector_t;
typedef core::vectorIn_t vectorIn_t;
typedef core::vectorOut_t vectorOut_t;
HPP_PREDEF_CLASS (ManipulationPlanner);
typedef shared_ptr < ManipulationPlanner > ManipulationPlannerPtr_t;
namespace pathPlanner {
HPP_PREDEF_CLASS (RMRStar);
typedef shared_ptr < RMRStar > RMRStarPtr_t;
HPP_PREDEF_CLASS (StatesPathFinder);
typedef shared_ptr < StatesPathFinder > StatesPathFinderPtr_t;
HPP_PREDEF_CLASS (InStatePath);
typedef shared_ptr < InStatePath > InStatePathPtr_t;
HPP_PREDEF_CLASS (StateShooter);
typedef shared_ptr < StateShooter > StateShooterPtr_t;
} // namespace pathPlanner
HPP_PREDEF_CLASS (GraphPathValidation);
typedef shared_ptr < GraphPathValidation > GraphPathValidationPtr_t;
HPP_PREDEF_CLASS (SteeringMethod);
typedef shared_ptr < SteeringMethod > SteeringMethodPtr_t;
typedef core::PathOptimizer PathOptimizer;
typedef core::PathOptimizerPtr_t PathOptimizerPtr_t;
HPP_PREDEF_CLASS (GraphOptimizer);
typedef shared_ptr < GraphOptimizer > GraphOptimizerPtr_t;
HPP_PREDEF_CLASS (GraphNodeOptimizer);
typedef shared_ptr < GraphNodeOptimizer > GraphNodeOptimizerPtr_t;
typedef core::PathProjectorPtr_t PathProjectorPtr_t;
namespace manipulation {
HPP_PREDEF_CLASS(Device);
typedef shared_ptr<Device> DevicePtr_t;
typedef shared_ptr<const Device> DeviceConstPtr_t;
typedef pinocchio::Joint Joint;
typedef pinocchio::JointPtr_t JointPtr_t;
typedef pinocchio::JointIndex JointIndex;
typedef std::vector<JointIndex> JointIndices_t;
typedef pinocchio::FrameIndex FrameIndex;
typedef std::vector<pinocchio::FrameIndex> FrameIndices_t;
typedef pinocchio::Configuration_t Configuration_t;
typedef pinocchio::ConfigurationIn_t ConfigurationIn_t;
typedef pinocchio::ConfigurationOut_t ConfigurationOut_t;
typedef pinocchio::GripperPtr_t GripperPtr_t;
typedef pinocchio::LiegroupElement LiegroupElement;
typedef pinocchio::LiegroupSpace LiegroupSpace;
typedef pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t;
HPP_PREDEF_CLASS(AxialHandle);
typedef shared_ptr<AxialHandle> AxialHandlePtr_t;
HPP_PREDEF_CLASS(Handle);
typedef shared_ptr<Handle> HandlePtr_t;
HPP_PREDEF_CLASS(Object);
typedef shared_ptr<Object> ObjectPtr_t;
typedef shared_ptr<const Object> ObjectConstPtr_t;
HPP_PREDEF_CLASS(ProblemSolver);
typedef ProblemSolver* ProblemSolverPtr_t;
HPP_PREDEF_CLASS(Problem);
typedef shared_ptr<Problem> ProblemPtr_t;
typedef shared_ptr<const Problem> ProblemConstPtr_t;
HPP_PREDEF_CLASS(Roadmap);
typedef shared_ptr<Roadmap> RoadmapPtr_t;
HPP_PREDEF_CLASS(RoadmapNode);
typedef RoadmapNode* RoadmapNodePtr_t;
typedef std::vector<RoadmapNodePtr_t> RoadmapNodes_t;
HPP_PREDEF_CLASS(ConnectedComponent);
typedef shared_ptr<ConnectedComponent> ConnectedComponentPtr_t;
HPP_PREDEF_CLASS(LeafConnectedComp);
typedef shared_ptr<LeafConnectedComp> LeafConnectedCompPtr_t;
typedef shared_ptr<const LeafConnectedComp> LeafConnectedCompConstPtr_t;
typedef std::set<LeafConnectedCompPtr_t> LeafConnectedComps_t;
HPP_PREDEF_CLASS(WeighedLeafConnectedComp);
typedef shared_ptr<WeighedLeafConnectedComp> WeighedLeafConnectedCompPtr_t;
HPP_PREDEF_CLASS(WeighedDistance);
typedef shared_ptr<WeighedDistance> WeighedDistancePtr_t;
typedef constraints::RelativeOrientation RelativeOrientation;
typedef constraints::RelativePosition RelativePosition;
typedef constraints::RelativeOrientationPtr_t RelativeOrientationPtr_t;
typedef constraints::RelativePositionPtr_t RelativePositionPtr_t;
typedef constraints::RelativeTransformation RelativeTransformation;
typedef constraints::RelativeTransformationR3xSO3 RelativeTransformationR3xSO3;
typedef constraints::RelativeTransformationPtr_t RelativeTransformationPtr_t;
typedef core::value_type value_type;
typedef core::size_type size_type;
typedef core::Transform3s Transform3s;
typedef core::vector_t vector_t;
typedef core::vectorIn_t vectorIn_t;
typedef core::vectorOut_t vectorOut_t;
HPP_PREDEF_CLASS(ManipulationPlanner);
typedef shared_ptr<ManipulationPlanner> ManipulationPlannerPtr_t;
namespace pathPlanner {
HPP_PREDEF_CLASS(EndEffectorTrajectory);
typedef shared_ptr<EndEffectorTrajectory> EndEffectorTrajectoryPtr_t;
HPP_PREDEF_CLASS(StatesPathFinder);
typedef shared_ptr<StatesPathFinder> StatesPathFinderPtr_t;
HPP_PREDEF_CLASS(InStatePath);
typedef shared_ptr<InStatePath> InStatePathPtr_t;
HPP_PREDEF_CLASS(StateShooter);
typedef shared_ptr<StateShooter> StateShooterPtr_t;
HPP_PREDEF_CLASS(TransitionPlanner);
typedef shared_ptr<TransitionPlanner> TransitionPlannerPtr_t;
} // namespace pathPlanner
HPP_PREDEF_CLASS(GraphPathValidation);
typedef shared_ptr<GraphPathValidation> GraphPathValidationPtr_t;
HPP_PREDEF_CLASS(SteeringMethod);
typedef shared_ptr<SteeringMethod> SteeringMethodPtr_t;
namespace steeringMethod {
HPP_PREDEF_CLASS(EndEffectorTrajectory);
typedef shared_ptr<EndEffectorTrajectory> EndEffectorTrajectoryPtr_t;
} // namespace steeringMethod
typedef core::PathOptimizer PathOptimizer;
typedef core::PathOptimizerPtr_t PathOptimizerPtr_t;
HPP_PREDEF_CLASS(GraphOptimizer);
typedef shared_ptr<GraphOptimizer> GraphOptimizerPtr_t;
HPP_PREDEF_CLASS(GraphNodeOptimizer);
typedef shared_ptr<GraphNodeOptimizer> GraphNodeOptimizerPtr_t;
typedef core::PathProjectorPtr_t PathProjectorPtr_t;
typedef std::vector <pinocchio::DevicePtr_t> Devices_t;
typedef std::vector <ObjectPtr_t> Objects_t;
typedef core::Constraint Constraint;
typedef core::ConstraintPtr_t ConstraintPtr_t;
typedef constraints::Explicit Explicit;
typedef constraints::ExplicitPtr_t ExplicitPtr_t;
typedef constraints::ImplicitPtr_t ImplicitPtr_t;
typedef constraints::LockedJoint LockedJoint;
typedef constraints::LockedJointPtr_t LockedJointPtr_t;
typedef hpp::core::ComparisonTypes_t ComparisonTypes_t;
typedef core::ConfigProjector ConfigProjector;
typedef core::ConfigProjectorPtr_t ConfigProjectorPtr_t;
HPP_PREDEF_CLASS (ConstraintSet);
typedef shared_ptr <ConstraintSet> ConstraintSetPtr_t;
typedef core::DifferentiableFunctionPtr_t DifferentiableFunctionPtr_t;
typedef core::ConfigurationShooter ConfigurationShooter;
typedef core::ConfigurationShooterPtr_t ConfigurationShooterPtr_t;
typedef core::ValidationReport ValidationReport;
typedef core::NumericalConstraints_t NumericalConstraints_t;
typedef core::PathValidationPtr_t PathValidationPtr_t;
typedef core::PathValidationReportPtr_t PathValidationReportPtr_t;
typedef core::matrix_t matrix_t;
typedef core::matrixIn_t matrixIn_t;
typedef core::matrixOut_t matrixOut_t;
typedef core::size_type size_type;
typedef core::value_type value_type;
typedef core::vector3_t vector3_t;
typedef core::matrix3_t matrix3_t;
typedef std::vector<pinocchio::DevicePtr_t> Devices_t;
typedef std::vector<ObjectPtr_t> Objects_t;
typedef core::Constraint Constraint;
typedef core::ConstraintPtr_t ConstraintPtr_t;
typedef constraints::Explicit Explicit;
typedef constraints::ExplicitPtr_t ExplicitPtr_t;
typedef constraints::ImplicitPtr_t ImplicitPtr_t;
typedef constraints::LockedJoint LockedJoint;
typedef constraints::LockedJointPtr_t LockedJointPtr_t;
typedef hpp::core::ComparisonTypes_t ComparisonTypes_t;
typedef core::ConfigProjector ConfigProjector;
typedef core::ConfigProjectorPtr_t ConfigProjectorPtr_t;
HPP_PREDEF_CLASS(ConstraintSet);
typedef shared_ptr<ConstraintSet> ConstraintSetPtr_t;
typedef core::DifferentiableFunctionPtr_t DifferentiableFunctionPtr_t;
typedef core::ConfigurationShooter ConfigurationShooter;
typedef core::ConfigurationShooterPtr_t ConfigurationShooterPtr_t;
typedef core::ValidationReport ValidationReport;
typedef core::NumericalConstraints_t NumericalConstraints_t;
typedef core::PathValidationPtr_t PathValidationPtr_t;
typedef core::PathValidationReportPtr_t PathValidationReportPtr_t;
typedef core::matrix_t matrix_t;
typedef core::matrixIn_t matrixIn_t;
typedef core::matrixOut_t matrixOut_t;
typedef core::size_type size_type;
typedef core::value_type value_type;
typedef core::vector3_t vector3_t;
typedef core::matrix3_t matrix3_t;
typedef core::Shape_t Shape_t;
typedef core::JointAndShape_t JointAndShape_t;
typedef core::JointAndShapes_t JointAndShapes_t;
typedef core::Shape_t Shape_t;
typedef core::JointAndShape_t JointAndShape_t;
typedef core::JointAndShapes_t JointAndShapes_t;
typedef std::list <std::string> StringList_t;
typedef std::vector <std::string> Strings_t;
typedef std::list<std::string> StringList_t;
typedef std::vector<std::string> Strings_t;
namespace pathOptimization {
HPP_PREDEF_CLASS (SmallSteps);
typedef shared_ptr < SmallSteps > SmallStepsPtr_t;
HPP_PREDEF_CLASS (Keypoints);
typedef shared_ptr < Keypoints > KeypointsPtr_t;
} // namespace pathOptimization
namespace pathOptimization {
HPP_PREDEF_CLASS(SmallSteps);
typedef shared_ptr<SmallSteps> SmallStepsPtr_t;
HPP_PREDEF_CLASS(Keypoints);
typedef shared_ptr<Keypoints> KeypointsPtr_t;
} // namespace pathOptimization
namespace problemTarget {
HPP_PREDEF_CLASS (State);
typedef shared_ptr < State > StatePtr_t;
} // namespace problemTarget
} // namespace manipulation
} // namespace hpp
namespace problemTarget {
HPP_PREDEF_CLASS(State);
typedef shared_ptr<State> StatePtr_t;
} // namespace problemTarget
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_FWD_HH
#endif // HPP_MANIPULATION_FWD_HH
include/hpp/manipulation/graph-node-optimizer.hh
View file @ 539aab52
......@@ -27,52 +27,48 @@
// DAMAGE.
#ifndef HPP_MANIPULATION_GRAPH_NODE_OPTIMIZER_HH
# define HPP_MANIPULATION_GRAPH_NODE_OPTIMIZER_HH
#define HPP_MANIPULATION_GRAPH_NODE_OPTIMIZER_HH
# include <hpp/core/path.hh>
# include <hpp/core/path-vector.hh>
# include <hpp/core/path-optimizer.hh>
# include <hpp/core/problem.hh>
# include <hpp/core/problem-solver.hh>
# include <hpp/manipulation/fwd.hh>
# include <hpp/manipulation/graph/fwd.hh>
# include <hpp/manipulation/config.hh>
# include <hpp/manipulation/constraint-set.hh>
#include <hpp/core/path-optimizer.hh>
#include <hpp/core/path-vector.hh>
#include <hpp/core/path.hh>
#include <hpp/core/problem-solver.hh>
#include <hpp/core/problem.hh>
#include <hpp/manipulation/config.hh>
#include <hpp/manipulation/constraint-set.hh>
#include <hpp/manipulation/fwd.hh>
#include <hpp/manipulation/graph/fwd.hh>
namespace hpp {
namespace manipulation {
using hpp::core::Path;
using hpp::core::PathPtr_t;
using hpp::core::PathVector;
using hpp::core::PathVectorPtr_t;
namespace manipulation {
using hpp::core::Path;
using hpp::core::PathPtr_t;
using hpp::core::PathVector;
using hpp::core::PathVectorPtr_t;
/// \addtogroup path_optimization
/// \{
/// \addtogroup path_optimization
/// \{
/// Path optimizer that recompute the edge parameter of the constraints
///
/// This class encapsulates another path optimizer class. This optimizer
/// calls the inner optimizer on every subpaths with the same set of
/// constraints.
class HPP_MANIPULATION_DLLAPI GraphNodeOptimizer : public PathOptimizer
{
public:
static GraphNodeOptimizerPtr_t create
(const core::ProblemConstPtr_t& problem);
/// Path optimizer that recompute the edge parameter of the constraints
///
/// This class encapsulates another path optimizer class. This optimizer
/// calls the inner optimizer on every subpaths with the same set of
/// constraints.
class HPP_MANIPULATION_DLLAPI GraphNodeOptimizer : public PathOptimizer {
public:
static GraphNodeOptimizerPtr_t create(const core::ProblemConstPtr_t& problem);
virtual PathVectorPtr_t optimize (const PathVectorPtr_t& path);
virtual PathVectorPtr_t optimize(const PathVectorPtr_t& path);
protected:
/// Constructor
GraphNodeOptimizer (const core::ProblemConstPtr_t& problem) :
PathOptimizer (problem)
{}
protected:
/// Constructor
GraphNodeOptimizer(const core::ProblemConstPtr_t& problem)
: PathOptimizer(problem) {}
private:
};
/// \}
} // namespace manipulation
} // namespace hpp
private:
};
/// \}
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_GRAPH_NODE_OPTIMIZER_HH
#endif // HPP_MANIPULATION_GRAPH_NODE_OPTIMIZER_HH
include/hpp/manipulation/graph-optimizer.hh
View file @ 539aab52
......@@ -26,75 +26,65 @@
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
#ifndef HPP_MANIPULATION_GRAPHOPTIMIZER_HH
# define HPP_MANIPULATION_GRAPHOPTIMIZER_HH
# include <hpp/core/path-optimizer.hh>
# include <hpp/core/problem-solver.hh> // PathOptimizerBuilder_t
#define HPP_MANIPULATION_GRAPHOPTIMIZER_HH
# include <hpp/manipulation/fwd.hh>
# include <hpp/manipulation/config.hh>
# include <hpp/manipulation/graph/fwd.hh>
#include <hpp/core/path-optimizer.hh>
#include <hpp/core/problem-solver.hh> // PathOptimizerBuilder_t
#include <hpp/manipulation/config.hh>
#include <hpp/manipulation/fwd.hh>
#include <hpp/manipulation/graph/fwd.hh>
namespace hpp {
namespace manipulation {
using hpp::core::Path;
using hpp::core::PathPtr_t;
using hpp::core::PathVector;
using hpp::core::PathVectorPtr_t;
/// \addtogroup path_optimization
/// \{
/// Path optimizer for paths created with the constraint graph
///
/// This class encapsulates another path optimizer class. This optimizer
/// calls the inner optimizer on every subpaths with the same set of
/// constraints.
class HPP_MANIPULATION_DLLAPI GraphOptimizer : public PathOptimizer
{
public:
typedef core::PathOptimizerBuilder_t PathOptimizerBuilder_t;
template <typename TraitsOrInnerType>
static GraphOptimizerPtr_t create
(const core::ProblemConstPtr_t& problem);
virtual PathVectorPtr_t optimize (const PathVectorPtr_t& path);
/// Get the encapsulated optimizer
const PathOptimizerPtr_t& innerOptimizer ()
{
return pathOptimizer_;
}
protected:
/// Constructor
GraphOptimizer (const core::ProblemConstPtr_t& problem,
PathOptimizerBuilder_t factory) :
PathOptimizer (problem), factory_ (factory), pathOptimizer_ ()
{}
private:
PathOptimizerBuilder_t factory_;
/// The encapsulated PathOptimizer
PathOptimizerPtr_t pathOptimizer_;
};
/// \}
/// Member function definition
template <typename TraitsOrInnerType>
GraphOptimizerPtr_t GraphOptimizer::create
(const core::ProblemConstPtr_t& problem)
{
return GraphOptimizerPtr_t (
new GraphOptimizer (problem, TraitsOrInnerType::create)
);
}
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_GRAPHOPTIMIZER_HH
namespace manipulation {
using hpp::core::Path;
using hpp::core::PathPtr_t;
using hpp::core::PathVector;
using hpp::core::PathVectorPtr_t;
/// \addtogroup path_optimization
/// \{
/// Path optimizer for paths created with the constraint graph
///
/// This class encapsulates another path optimizer class. This optimizer
/// calls the inner optimizer on every subpaths with the same set of
/// constraints.
class HPP_MANIPULATION_DLLAPI GraphOptimizer : public PathOptimizer {
public:
typedef core::PathOptimizerBuilder_t PathOptimizerBuilder_t;
template <typename TraitsOrInnerType>
static GraphOptimizerPtr_t create(const core::ProblemConstPtr_t& problem);
virtual PathVectorPtr_t optimize(const PathVectorPtr_t& path);
/// Get the encapsulated optimizer
const PathOptimizerPtr_t& innerOptimizer() { return pathOptimizer_; }
protected:
/// Constructor
GraphOptimizer(const core::ProblemConstPtr_t& problem,
PathOptimizerBuilder_t factory)
: PathOptimizer(problem), factory_(factory), pathOptimizer_() {}
private:
PathOptimizerBuilder_t factory_;
/// The encapsulated PathOptimizer
PathOptimizerPtr_t pathOptimizer_;
};
/// \}
/// Member function definition
template <typename TraitsOrInnerType>
GraphOptimizerPtr_t GraphOptimizer::create(
const core::ProblemConstPtr_t& problem) {
return GraphOptimizerPtr_t(
new GraphOptimizer(problem, TraitsOrInnerType::create));
}
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_GRAPHOPTIMIZER_HH
include/hpp/manipulation/graph-path-validation.hh
View file @ 539aab52
......@@ -26,182 +26,167 @@
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
#ifndef HPP_MANIPULATION_GRAPHPATHVALIDATOR_HH
# define HPP_MANIPULATION_GRAPHPATHVALIDATOR_HH
# include <hpp/core/path-validation.hh>
# include <hpp/core/obstacle-user.hh>
#define HPP_MANIPULATION_GRAPHPATHVALIDATOR_HH
# include <hpp/manipulation/fwd.hh>
# include <hpp/manipulation/config.hh>
# include <hpp/manipulation/graph/fwd.hh>
#include <hpp/core/obstacle-user.hh>
#include <hpp/core/path-validation.hh>
#include <hpp/manipulation/config.hh>
#include <hpp/manipulation/fwd.hh>
#include <hpp/manipulation/graph/fwd.hh>
namespace hpp {
namespace manipulation {
using hpp::core::PathValidation;
using hpp::core::PathValidationPtr_t;
using hpp::core::Path;
using hpp::core::PathPtr_t;
using hpp::core::PathVector;
using hpp::core::PathVectorPtr_t;
/// \addtogroup validation
/// \{
/// Path validation for a constraint graph
///
/// This class encapsulates another path validation class.
/// The encapsulated path validation is responsible for collision
/// checking, whereas this class checks if a path is valid regarding
/// the constraint graph.
class HPP_MANIPULATION_DLLAPI GraphPathValidation :
public PathValidation,
public core::ObstacleUserInterface
{
public:
/// Check that path is valid regarding the constraint graph.
///
/// \param path the path to check for validity,
/// \param reverse if true check from the end,
/// \retval the extracted valid part of the path, pointer to path if
/// path is valid,
/// \retval report information about the validation process. unused in
/// this case,
/// \return whether the whole path is valid.
///
/// \notice Call the encapsulated PathValidation::validate.
virtual bool validate (const PathPtr_t& path, bool reverse,
PathPtr_t& validPart,
PathValidationReportPtr_t& report);
/// Set the encapsulated path validator.
void innerValidation (const PathValidationPtr_t& pathValidation)
{
pathValidation_ = pathValidation;
}
/// Get the encapsulated path validator.
const PathValidationPtr_t& innerValidation ()
{
return pathValidation_;
}
/// Set the graph of constraints
void constraintGraph (const graph::GraphPtr_t& graph)
{
constraintGraph_ = graph;
}
/// Get the graph of constraints
graph::GraphPtr_t constraintGraph () const
{
return constraintGraph_;
}
/// Create a new instance of this class.
/// \param pathValidation a PathValidation that is responsible for collision
// checking.
// \param graph A pointer to the constraint graph.
static GraphPathValidationPtr_t create (
const PathValidationPtr_t& pathValidation);
template <typename T>
static GraphPathValidationPtr_t create (
const pinocchio::DevicePtr_t& robot, const value_type& stepSize);
/// Add obstacle in the environment
///
/// Dynamic cast inner path validation into
/// hpp::core::ObstacleUserInterface and calls
/// hpp::core::ObstacleUserInterface::addObstacle in case of success.
void addObstacle (const hpp::core::CollisionObjectConstPtr_t& object)
{
shared_ptr<core::ObstacleUserInterface> oui =
HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
if (oui) oui->addObstacle (object);
}
/// Remove a collision pair between a joint and an obstacle
///
/// Dynamic cast inner path validation into
/// hpp::core::ObstacleUserInterface and calls
/// hpp::core::ObstacleUserInterface::removeObstacleFromJoint in case
/// of success.
void removeObstacleFromJoint (const JointPtr_t& joint,
const pinocchio::CollisionObjectConstPtr_t& obstacle)
{
shared_ptr<core::ObstacleUserInterface> oui =
HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
if (oui) oui->removeObstacleFromJoint (joint, obstacle);
}
/// \brief Filter collision pairs.
///
/// Dynamic cast inner path validation into
/// hpp::core::ObstacleUserInterface and calls
/// hpp::core::ObstacleUserInterface::filterCollisionPairs in case of
/// success.
void filterCollisionPairs (const core::RelativeMotion::matrix_type& relMotion)
{
shared_ptr<core::ObstacleUserInterface> oui =
HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
if (oui) oui->filterCollisionPairs (relMotion);
}
/// Set different security margins for collision pairs
///
/// Dynamic cast inner path validation into
/// hpp::core::ObstacleUserInterface and calls
/// hpp::core::ObstacleUserInterface::setSecurityMargins in case of
/// success.
void setSecurityMargins(const matrix_t& securityMargins)
{
shared_ptr<core::ObstacleUserInterface> oui =
HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
if (oui) oui->setSecurityMargins(securityMargins);
}
/// \copydoc hpp::core::ObstacleUserInterface::setSecurityMarginBetweenBodies
///
/// Dynamic cast inner path validation into
/// hpp::core::ObstacleUserInterface and calls
/// hpp::core::ObstacleUserInterface::setSecurityMargins in case of
/// success.
void setSecurityMarginBetweenBodies(const std::string& body_a,
const std::string& body_b,
const value_type& margin)
{
shared_ptr<core::ObstacleUserInterface> oui =
HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
if (oui) oui->setSecurityMarginBetweenBodies(body_a, body_b, margin);
}
protected:
/// Constructor
GraphPathValidation (const PathValidationPtr_t& pathValidation);
private:
/// Do validation regarding the constraint graph for PathVector
bool impl_validate (const PathVectorPtr_t& path, bool reverse,
PathPtr_t& validPart, PathValidationReportPtr_t& report);
/// Do validation regarding the constraint graph for Path
bool impl_validate (const PathPtr_t& path, bool reverse,
PathPtr_t& validPart, PathValidationReportPtr_t& report);
/// The encapsulated PathValidation.
PathValidationPtr_t pathValidation_;
/// Pointer to the constraint graph.
graph::GraphPtr_t constraintGraph_;
};
template <typename T>
GraphPathValidationPtr_t GraphPathValidation::create
(const pinocchio::DevicePtr_t& robot, const value_type& stepSize)
{
return GraphPathValidation::create (T::create (robot, stepSize));
}
/// \}
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_GRAPHPATHVALIDATOR_HH
namespace manipulation {
using hpp::core::Path;
using hpp::core::PathPtr_t;
using hpp::core::PathValidation;
using hpp::core::PathValidationPtr_t;
using hpp::core::PathVector;
using hpp::core::PathVectorPtr_t;
/// \addtogroup validation
/// \{
/// Path validation for a constraint graph
///
/// This class encapsulates another path validation class.
/// The encapsulated path validation is responsible for collision
/// checking, whereas this class checks if a path is valid regarding
/// the constraint graph.
class HPP_MANIPULATION_DLLAPI GraphPathValidation
: public PathValidation,
public core::ObstacleUserInterface {
public:
/// Check that path is valid regarding the constraint graph.
///
/// \param path the path to check for validity,
/// \param reverse if true check from the end,
/// \retval the extracted valid part of the path, pointer to path if
/// path is valid,
/// \retval report information about the validation process. unused in
/// this case,
/// \return whether the whole path is valid.
///
/// \notice Call the encapsulated PathValidation::validate.
virtual bool validate(const PathPtr_t& path, bool reverse,
PathPtr_t& validPart,
PathValidationReportPtr_t& report);
/// Set the encapsulated path validator.
void innerValidation(const PathValidationPtr_t& pathValidation) {
pathValidation_ = pathValidation;
}
/// Get the encapsulated path validator.
const PathValidationPtr_t& innerValidation() { return pathValidation_; }
/// Set the graph of constraints
void constraintGraph(const graph::GraphPtr_t& graph) {
constraintGraph_ = graph;
}
/// Get the graph of constraints
graph::GraphPtr_t constraintGraph() const { return constraintGraph_; }
/// Create a new instance of this class.
/// \param pathValidation a PathValidation that is responsible for collision
// checking.
// \param graph A pointer to the constraint graph.
static GraphPathValidationPtr_t create(
const PathValidationPtr_t& pathValidation);
template <typename T>
static GraphPathValidationPtr_t create(const pinocchio::DevicePtr_t& robot,
const value_type& stepSize);
/// Add obstacle in the environment
///
/// Dynamic cast inner path validation into
/// hpp::core::ObstacleUserInterface and calls
/// hpp::core::ObstacleUserInterface::addObstacle in case of success.
void addObstacle(const hpp::core::CollisionObjectConstPtr_t& object) {
shared_ptr<core::ObstacleUserInterface> oui =
HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
if (oui) oui->addObstacle(object);
}
/// Remove a collision pair between a joint and an obstacle
///
/// Dynamic cast inner path validation into
/// hpp::core::ObstacleUserInterface and calls
/// hpp::core::ObstacleUserInterface::removeObstacleFromJoint in case
/// of success.
void removeObstacleFromJoint(
const JointPtr_t& joint,
const pinocchio::CollisionObjectConstPtr_t& obstacle) {
shared_ptr<core::ObstacleUserInterface> oui =
HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
if (oui) oui->removeObstacleFromJoint(joint, obstacle);
}
/// \brief Filter collision pairs.
///
/// Dynamic cast inner path validation into
/// hpp::core::ObstacleUserInterface and calls
/// hpp::core::ObstacleUserInterface::filterCollisionPairs in case of
/// success.
void filterCollisionPairs(
const core::RelativeMotion::matrix_type& relMotion) {
shared_ptr<core::ObstacleUserInterface> oui =
HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
if (oui) oui->filterCollisionPairs(relMotion);
}
/// Set different security margins for collision pairs
///
/// Dynamic cast inner path validation into
/// hpp::core::ObstacleUserInterface and calls
/// hpp::core::ObstacleUserInterface::setSecurityMargins in case of
/// success.
void setSecurityMargins(const matrix_t& securityMargins) {
shared_ptr<core::ObstacleUserInterface> oui =
HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
if (oui) oui->setSecurityMargins(securityMargins);
}
/// \copydoc hpp::core::ObstacleUserInterface::setSecurityMarginBetweenBodies
///
/// Dynamic cast inner path validation into
/// hpp::core::ObstacleUserInterface and calls
/// hpp::core::ObstacleUserInterface::setSecurityMargins in case of
/// success.
void setSecurityMarginBetweenBodies(const std::string& body_a,
const std::string& body_b,
const value_type& margin) {
shared_ptr<core::ObstacleUserInterface> oui =
HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
if (oui) oui->setSecurityMarginBetweenBodies(body_a, body_b, margin);
}
protected:
/// Constructor
GraphPathValidation(const PathValidationPtr_t& pathValidation);
private:
/// Do validation regarding the constraint graph for PathVector
bool impl_validate(const PathVectorPtr_t& path, bool reverse,
PathPtr_t& validPart, PathValidationReportPtr_t& report);
/// Do validation regarding the constraint graph for Path
bool impl_validate(const PathPtr_t& path, bool reverse, PathPtr_t& validPart,
PathValidationReportPtr_t& report);
/// The encapsulated PathValidation.
PathValidationPtr_t pathValidation_;
/// Pointer to the constraint graph.
graph::GraphPtr_t constraintGraph_;
};
template <typename T>
GraphPathValidationPtr_t GraphPathValidation::create(
const pinocchio::DevicePtr_t& robot, const value_type& stepSize) {
return GraphPathValidation::create(T::create(robot, stepSize));
}
/// \}
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_GRAPHPATHVALIDATOR_HH