├── .git-blame-ignore-revs ├── .github └── workflows │ └── nix.yml ├── .gitlab-ci.yml ├── .gitmodules ├── .mergify.yml ├── .pre-commit-config.yaml ├── AUTHORS ├── CMakeLists.txt ├── INSTALL ├── LICENSE ├── NEWS ├── README.md ├── cmake-modules └── hpp-core-plugin.cmake ├── doc ├── Doxyfile.extra.in ├── WEIGHTED_long_box100.png ├── WEIGHTED_long_box100_2x.png ├── algorithm.sty ├── algorithmicx.sty ├── algpseudocode.sty ├── architecture-constraint ├── collision_constraints.tex ├── continuous-collision-checking.pdf ├── continuous-collision-checking.tex ├── continuous-validation.pdf ├── continuous-validation.tex ├── explicit-numerical-constraints.tex ├── finiteStateMachine.pdf ├── footer.html ├── framesConstraintError.pdf ├── gradient.pdf ├── header.html ├── kinematic-chain.fig ├── kinematic-chain.pdf ├── kinematic-chain.pdf_t ├── linearError.pdf ├── main-page │ ├── news.hh │ ├── package.hh │ └── porting-notes.hh ├── path-optimization.pdf ├── path-optimization.tex ├── pictures │ ├── archi.fig │ ├── archi.pdf │ ├── archi.png │ └── footer.jpg ├── projection.pdf ├── projection.tex ├── quaternions.pdf ├── rotation.tex ├── spline-gradient-based.tex ├── spline-path.tex ├── tikz-uml.sty └── weighted_path.pdf ├── flake.lock ├── flake.nix ├── include └── hpp │ └── core │ ├── bi-rrt-planner.hh │ ├── collision-pair.hh │ ├── collision-path-validation-report.hh │ ├── collision-validation-report.hh │ ├── collision-validation.hh │ ├── config-projector.hh │ ├── config-validation.hh │ ├── config-validations.hh │ ├── configuration-shooter.hh │ ├── configuration-shooter │ ├── gaussian.hh │ ├── uniform-tpl.hh │ ├── uniform-tpl.hxx │ └── uniform.hh │ ├── connected-component.hh │ ├── constraint-set.hh │ ├── constraint.hh │ ├── container.hh │ ├── continuous-validation.hh │ ├── continuous-validation │ ├── body-pair-collision.hh │ ├── dichotomy.hh │ ├── interval-validation.hh │ ├── progressive.hh │ └── solid-solid-collision.hh │ ├── diffusing-planner.hh │ ├── distance-between-objects.hh │ ├── distance.hh │ ├── distance │ └── reeds-shepp.hh │ ├── dubins-path.hh │ ├── edge.hh │ ├── fwd.hh │ ├── interpolated-path.hh │ ├── joint-bound-validation.hh │ ├── kinodynamic-distance.hh │ ├── kinodynamic-oriented-path.hh │ ├── kinodynamic-path.hh │ ├── nearest-neighbor.hh │ ├── node.hh │ ├── obstacle-user.hh │ ├── parameter.hh │ ├── parser │ └── roadmap.hh │ ├── path-optimization │ ├── config-optimization.hh │ ├── cost.hh │ ├── gradient-based.hh │ ├── linear-constraint.hh │ ├── partial-shortcut.hh │ ├── quadratic-program.hh │ ├── random-shortcut.hh │ ├── rs-time-parameterization.hh │ ├── simple-shortcut.hh │ ├── simple-time-parameterization.hh │ ├── spline-gradient-based-abstract.hh │ └── spline-gradient-based.hh │ ├── path-optimizer.hh │ ├── path-planner.hh │ ├── path-planner │ ├── bi-rrt-star.hh │ ├── k-prm-star.hh │ └── search-in-roadmap.hh │ ├── path-planning-failed.hh │ ├── path-projector.hh │ ├── path-projector │ ├── dichotomy.hh │ ├── global.hh │ ├── progressive.hh │ └── recursive-hermite.hh │ ├── path-validation-report.hh │ ├── path-validation.hh │ ├── path-validation │ ├── discretized-collision-checking.hh │ ├── discretized-joint-bound.hh │ └── discretized.hh │ ├── path-validations.hh │ ├── path-vector.hh │ ├── path.hh │ ├── path │ ├── hermite.hh │ ├── math.hh │ └── spline.hh │ ├── plan-and-optimize.hh │ ├── plugin.hh │ ├── problem-solver.hh │ ├── problem-target.hh │ ├── problem-target │ ├── goal-configurations.hh │ └── task-target.hh │ ├── problem.hh │ ├── projection-error.hh │ ├── relative-motion.hh │ ├── roadmap.hh │ ├── steering-method.hh │ ├── steering-method │ ├── car-like.hh │ ├── constant-curvature.hh │ ├── dubins.hh │ ├── fwd.hh │ ├── hermite.hh │ ├── interpolated-steering-method.hh │ ├── reeds-shepp.hh │ ├── snibud.hh │ ├── spline.hh │ ├── steering-kinodynamic.hh │ └── straight.hh │ ├── straight-path.hh │ ├── subchain-path.hh │ ├── time-parameterization.hh │ ├── time-parameterization │ ├── piecewise-polynomial.hh │ └── polynomial.hh │ ├── validation-report.hh │ ├── visibility-prm-planner.hh │ └── weighed-distance.hh ├── package.xml ├── plugins ├── CMakeLists.txt ├── spline-gradient-based.cc ├── toppra.cc └── toppra.hh ├── pyproject.toml ├── src ├── astar.hh ├── bi-rrt-planner.cc ├── collision-validation.cc ├── config-projector.cc ├── config-validations.cc ├── configuration-shooter │ ├── gaussian.cc │ ├── uniform-tpl.cc │ └── uniform.cc ├── connected-component.cc ├── constraint-set.cc ├── constraint.cc ├── continuous-validation.cc ├── continuous-validation │ ├── body-pair-collision.cc │ ├── dichotomy.cc │ ├── helper.hh │ ├── intervals.hh │ ├── progressive.cc │ └── solid-solid-collision.cc ├── diffusing-planner.cc ├── distance-between-objects.cc ├── distance │ ├── reeds-shepp.cc │ └── serialization.cc ├── dubins-path.cc ├── dubins.cc ├── dubins.hh ├── extracted-path.cc ├── extracted-path.hh ├── interpolated-path.cc ├── joint-bound-validation.cc ├── kinodynamic-distance.cc ├── kinodynamic-oriented-path.cc ├── kinodynamic-path.cc ├── nearest-neighbor │ ├── basic.cc │ ├── basic.hh │ ├── k-d-tree.cc │ ├── k-d-tree.hh │ └── serialization.cc ├── node.cc ├── obstacle-user.cc ├── parameter.cc ├── path-optimization │ ├── linear-constraint.cc │ ├── partial-shortcut.cc │ ├── quadratic-program.cc │ ├── random-shortcut.cc │ ├── reeds-shepp │ │ ├── piecewise-quadratic.cc │ │ └── piecewise-quadratic.hh │ ├── rs-time-parameterization.cc │ ├── simple-shortcut.cc │ ├── simple-time-parameterization.cc │ ├── spline-gradient-based-abstract.cc │ ├── spline-gradient-based.cc │ └── spline-gradient-based │ │ ├── collision-constraint.hh │ │ ├── cost.hh │ │ ├── eiquadprog_2011.hpp │ │ └── joint-bounds.hh ├── path-optimizer.cc ├── path-planner.cc ├── path-planner │ ├── bi-rrt-star.cc │ └── k-prm-star.cc ├── path-projector.cc ├── path-projector │ ├── dichotomy.cc │ ├── global.cc │ ├── progressive.cc │ └── recursive-hermite.cc ├── path-validation.cc ├── path-validation │ ├── discretized-collision-checking.cc │ ├── discretized-joint-bound.cc │ ├── discretized.cc │ └── no-validation.hh ├── path-validations.cc ├── path-vector.cc ├── path.cc ├── path │ ├── hermite.cc │ └── spline.cc ├── plan-and-optimize.cc ├── plugin.cc ├── problem-solver.cc ├── problem-target │ ├── goal-configurations.cc │ └── task-target.cc ├── problem.cc ├── reeds-shepp-path.cc ├── relative-motion.cc ├── roadmap.cc ├── serialization.cc ├── steering-method │ ├── car-like.cc │ ├── constant-curvature.cc │ ├── dubins.cc │ ├── reeds-shepp.cc │ ├── snibud.cc │ ├── spline.cc │ ├── steering-kinodynamic.cc │ └── straight.cc ├── straight-path.cc ├── visibility-prm-planner.cc └── weighed-distance.cc └── tests ├── CMakeLists.txt ├── configuration-shooters.cc ├── containers.cc ├── data └── empty.urdf ├── explicit-relative-transformation.cc ├── hermite-path.cc ├── path-projectors.cc ├── paths.cc ├── plugin-test ├── CMakeLists.txt └── example.cc ├── plugin.cc.in ├── problem.cc ├── random-numbers.hh ├── reeds-and-shepp.cc ├── relative-motion.cc ├── roadmap-1.cc ├── spline-path.cc ├── test-config-validations.cc ├── test-configprojector.cc ├── test-continuous-validation.cc ├── test-gradient-based.cc ├── test-intervals.cc ├── test-kdTree.cc ├── test-kinodynamic.cc ├── test-path-extraction.cc ├── test-solid-solid-collision.cc ├── test-spline-gradient-based.cc ├── time-parameterization.cc └── util.hh /.git-blame-ignore-revs: -------------------------------------------------------------------------------- 1 | # format (Guilhem Saurel, 2022-05-31) 2 | 1ed8adce25a819b078019c81be8a794929ed29da 3 | 4 | # format (Guilhem Saurel, 2022-04-19) 5 | 3406c95635d9ecc55fa28f0e79d862873d6f0c81 6 | # pre-commit run -a (Guilhem Saurel, 2022-10-05) 7 | 4a2031e70744f89980478487e02777c39e9099d4 8 | -------------------------------------------------------------------------------- /.github/workflows/nix.yml: -------------------------------------------------------------------------------- 1 | name: "CI - Nix" 2 | 3 | on: 4 | push: 5 | 6 | jobs: 7 | tests: 8 | name: "Nix build on ${{ matrix.os }}" 9 | runs-on: "${{ matrix.os }}-latest" 10 | strategy: 11 | matrix: 12 | os: [ubuntu] 13 | steps: 14 | - uses: actions/checkout@v4 15 | - uses: cachix/install-nix-action@v27 16 | - uses: cachix/cachix-action@v15 17 | with: 18 | name: gepetto 19 | authToken: '${{ secrets.CACHIX_AUTH_TOKEN }}' 20 | - run: nix build -L 21 | -------------------------------------------------------------------------------- /.gitlab-ci.yml: -------------------------------------------------------------------------------- 1 | include: https://rainboard.laas.fr/project/hpp-core/.gitlab-ci.yml 2 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "cmake"] 2 | path = cmake 3 | url = https://github.com/jrl-umi3218/jrl-cmakemodules.git 4 | -------------------------------------------------------------------------------- /.mergify.yml: -------------------------------------------------------------------------------- 1 | pull_request_rules: 2 | - name: merge automatically when CI passes and PR is approved 3 | conditions: 4 | - check-success = "gitlab-ci" 5 | - check-success = "Nix build on ubuntu" 6 | - check-success = "pre-commit.ci - pr" 7 | - or: 8 | - author = pre-commit-ci[bot] 9 | - author = dependabot[bot] 10 | actions: 11 | merge: 12 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | ci: 2 | autoupdate_branch: devel 3 | repos: 4 | - repo: https://github.com/astral-sh/ruff-pre-commit 5 | rev: v0.8.1 6 | hooks: 7 | - id: ruff 8 | args: 9 | - --fix 10 | - --exit-non-zero-on-fix 11 | - id: ruff-format 12 | - repo: https://github.com/cheshirekow/cmake-format-precommit 13 | rev: v0.6.13 14 | hooks: 15 | - id: cmake-format 16 | - repo: https://github.com/pappasam/toml-sort 17 | rev: v0.24.2 18 | hooks: 19 | - id: toml-sort-fix 20 | exclude: poetry.lock 21 | - repo: https://github.com/pre-commit/mirrors-clang-format 22 | rev: v19.1.4 23 | hooks: 24 | - id: clang-format 25 | args: 26 | - --style=Google 27 | - repo: https://github.com/pre-commit/pre-commit-hooks 28 | rev: v5.0.0 29 | hooks: 30 | - id: check-added-large-files 31 | - id: check-ast 32 | - id: check-executables-have-shebangs 33 | - id: check-json 34 | - id: check-merge-conflict 35 | - id: check-symlinks 36 | - id: check-toml 37 | - id: check-yaml 38 | - id: debug-statements 39 | - id: destroyed-symlinks 40 | - id: detect-private-key 41 | - id: end-of-file-fixer 42 | - id: fix-byte-order-marker 43 | - id: mixed-line-ending 44 | - id: trailing-whitespace 45 | -------------------------------------------------------------------------------- /AUTHORS: -------------------------------------------------------------------------------- 1 | Collectively developed within the scope of the Associated 2 | International Laboratory: Joint Japanese-French Robotics Laboratory 3 | (JRL) between AIST Japan and CNRS France. 4 | -------------------------------------------------------------------------------- /INSTALL: -------------------------------------------------------------------------------- 1 | Installation instructions 2 | 3 | Configuration 4 | ------------- 5 | untar the package 6 | cd hpp-core 7 | mkdir _build 8 | cd _build 9 | cmake [OPTIONS] .. 10 | 11 | Compilation and Installation 12 | ---------------------------- 13 | 14 | make install 15 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 2 | 3 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 4 | 5 | 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. 6 | 7 | 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. 8 | -------------------------------------------------------------------------------- /NEWS: -------------------------------------------------------------------------------- 1 | -*- outline -*- 2 | * Make Path::operator() deprecated: use method Path::eval instead. 3 | * Serialize several classes for save and loading roadmaps 4 | * use shared pointers to class Problem only. Classes previously storing const 5 | references to Problem now store const weak pointers. Classes that use to 6 | take a const reference to problem in the constructor now take a const 7 | shared pointer. 8 | New in v4.10.0 9 | * Add a parameter to set the center of the Gaussian configuration shooter. 10 | * Fix bug in Reeds and Shepp paths when curvature is not equal to 1. 11 | * Handle LockedJoint and Implicit instances in the same container in ProblemSolver class. make method ProblemSolver::addLockedJointToConfigProjector deprecated. 12 | * Replace class ContinuousCollisionChecking by class ContinuousValidation for broader generalisation. 13 | * Remove deprecated methods and files 14 | - ProblemSolver::addLockedJoint, 15 | - hpp::core::JointBoundException, 16 | - include/hpp/core/continuous-collision-checking.hh, 17 | - include/hpp/core/continuous-collision-checking/dichotomy.hh, 18 | - include/hpp/core/continuous-collision-checking/progressive.hh, 19 | - include/hpp/core/discretized-collision-checking.hh, 20 | - include/hpp/core/discretized-path-validation.hh, 21 | - include/hpp/core/locked-joint.hh, 22 | - include/hpp/core/random-shortcut.hh, 23 | - include/hpp/core/steering-method-straight.hh. 24 | * in SteeringMethod, assert that if q1 == q2, operator() returns a path. 25 | * class SplineGradientBased has been improved 26 | - some bugs have been fixed, 27 | - length of input path should not be 0. 28 | * PathVector::extract(t,t) does not returns an empty path. 29 | * InterpolatedPath supports non-zero start of interval of definition 30 | * Add abstract class ObstacleUserInterface. This class is aimed at being parent of an class handling obstacles. Provide some implementations like 31 | - ObstacleUser, 32 | - ObstacleUserVector. 33 | * Add method setSecurityMargin to class ObstacleUserInterface. 34 | New in 4.6.0 35 | * Keep frames of obstacles. 36 | * Add Gaussian configuration shooter 37 | * Enable computation of path derivative up to order 2 for path with time parameterization. 38 | * Improve parameters in problem 39 | * SimpleTimeParameterization accepts acceleration limits. 40 | 41 | New in 4.1, 2018-05-17 42 | * Diffusing Planner calls path projector if defined. 43 | * ProblemSolver::directPath now calls path projector if defined. 44 | * Implement Dubins, reverse Dubins (Snibud) steering methods 45 | * Rewrite Reeds and Shepp steering method. 46 | * In ProblemSolver, when loading new robot, reset obstacles. 47 | * Move LockedJointPtr_t and JointAndShapes_t from hpp::manipulation::ProblemSolver 48 | * Use MATHJAX for documentation generation. 49 | * Explicit relative transformation takes values in SE3. 50 | * Add getter to joint1 and joint2 in ExplicitRelativeTransform. 51 | * Add k nearest neightbor search in whole roadmap. 52 | * Implement k-PRM*. 53 | * Implement continuousCollisionChecking::Progressive::filterCollisionPairs. 54 | * Add accessor to the SteeringMethod::problem_. 55 | * Add TimeParameterization and add it to Path. 56 | * Fix loop of shared ptr of ConnectedComponent. 57 | * member BasicConfigurationShooter is not a const reference anymore. 58 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # HPP-core 2 | 3 | [![Building Status](https://travis-ci.org/humanoid-path-planner/hpp-core.svg?branch=master)](https://travis-ci.org/humanoid-path-planner/hpp-core) 4 | [![Pipeline status](https://gitlab.laas.fr/humanoid-path-planner/hpp-core/badges/master/pipeline.svg)](https://gitlab.laas.fr/humanoid-path-planner/hpp-core/commits/master) 5 | [![Coverage report](https://gitlab.laas.fr/humanoid-path-planner/hpp-core/badges/master/coverage.svg?job=doc-coverage)](https://gepettoweb.laas.fr/doc/humanoid-path-planner/hpp-core/master/coverage/) 6 | [![Code style: black](https://img.shields.io/badge/code%20style-black-000000.svg)](https://github.com/psf/black) 7 | [![pre-commit.ci status](https://results.pre-commit.ci/badge/github/humanoid-path-planner/hpp-core/master.svg)](https://results.pre-commit.ci/latest/github/humanoid-path-planner/hpp-core) 8 | 9 | 10 | This package implements path planning algorithms for kinematic chains. 11 | Kinematic chains are represented by class hpp::pinocchio::Device. 12 | 13 | ### The main classes are: 14 | 15 | * hpp::core::Problem: defines a canonical path planning problem, 16 | * hpp::core::PathPlanner: implements an algorithm to solve a problem, 17 | * hpp::core::Roadmap: stores a network of collision-free paths 18 | * hpp::core::SteeringMethod: builds paths between configurations taking into 19 | account kinematic constraints. 20 | * hpp::core::Path: paths for a robot. 21 | 22 | ### Embedding hpp-core into an application 23 | 24 | Class hpp::core::ProblemSolver is a container aiming at embedding 25 | hpp-core into an application. It stores elements of the problem 26 | that can be provided in random order and builds a valid problem 27 | upon call of method solve. After completion of method solve, it 28 | stores solution paths. 29 | -------------------------------------------------------------------------------- /cmake-modules/hpp-core-plugin.cmake: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2019, Joseph Mirabel Authors: Joseph Mirabel 2 | # (joseph.mirabel@laas.fr) 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # 7 | # 1. Redistributions of source code must retain the above copyright notice, this 8 | # list of conditions and the following disclaimer. 9 | # 10 | # 1. Redistributions in binary form must reproduce the above copyright notice, 11 | # this list of conditions and the following disclaimer in the documentation 12 | # and/or other materials provided with the distribution. 13 | # 14 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 15 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 16 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 17 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 18 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 19 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 20 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 21 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 22 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 23 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 24 | 25 | macro(HPP_ADD_PLUGIN PLUGIN_NAME) 26 | set(options EXCLUDE_FROM_ALL) 27 | set(oneValueArgs) 28 | set(multiValueArgs SOURCES LINK_DEPENDENCIES PKG_CONFIG_DEPENDENCIES EXPORT) 29 | cmake_parse_arguments(PLUGIN "${options}" "${oneValueArgs}" 30 | "${multiValueArgs}" ${ARGN}) 31 | if(PLUGIN_EXCLUDE_FROM_ALL) 32 | set(_options ${_options} EXCLUDE_FROM_ALL) 33 | endif() 34 | add_library(${PLUGIN_NAME} MODULE ${_options} ${PLUGIN_SOURCES}) 35 | set_target_properties(${PLUGIN_NAME} PROPERTIES PREFIX "" 36 | BUILD_WITH_INSTALL_RPATH TRUE) 37 | 38 | target_link_libraries(${PLUGIN_NAME} ${PLUGIN_LINK_DEPENDENCIES}) 39 | foreach(DEP ${PLUGIN_PKG_CONFIG_DEPENDENCIES}) 40 | pkg_config_use_dependency(${PLUGIN_NAME} ${DEP}) 41 | endforeach() 42 | 43 | if(NOT PLUGIN_EXCLUDE_FROM_ALL) 44 | install( 45 | TARGETS ${PLUGIN_NAME} 46 | EXPORT ${PLUGIN_EXPORT} 47 | DESTINATION lib/hppPlugins) 48 | endif() 49 | endmacro() 50 | 51 | macro(ADD_PLUGIN PLUGIN_NAME) 52 | message( 53 | AUTHOR_WARNING 54 | "Macro ADD_PLUGIN is deprecated and should be replaced by HPP_ADD_PLUGIN") 55 | hpp_add_plugin(${PLUGIN_NAME} ${ARGN}) 56 | endmacro() 57 | -------------------------------------------------------------------------------- /doc/Doxyfile.extra.in: -------------------------------------------------------------------------------- 1 | INPUT = @CMAKE_SOURCE_DIR@/include \ 2 | @CMAKE_SOURCE_DIR@/doc 3 | 4 | HIDE_FRIEND_COMPOUNDS = YES 5 | 6 | IMAGE_PATH = @CMAKE_SOURCE_DIR@/doc 7 | USE_MATHJAX = YES 8 | HTML_EXTRA_FILES = @CMAKE_SOURCE_DIR@/doc/continuous-collision-checking.pdf \ 9 | @CMAKE_SOURCE_DIR@/doc/continuous-validation.pdf 10 | -------------------------------------------------------------------------------- /doc/WEIGHTED_long_box100.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humanoid-path-planner/hpp-core/a306cad1195138b0ec8b07ce98a589240083b5e6/doc/WEIGHTED_long_box100.png -------------------------------------------------------------------------------- /doc/WEIGHTED_long_box100_2x.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humanoid-path-planner/hpp-core/a306cad1195138b0ec8b07ce98a589240083b5e6/doc/WEIGHTED_long_box100_2x.png -------------------------------------------------------------------------------- /doc/algorithm.sty: -------------------------------------------------------------------------------- 1 | % ALGORITHM STYLE -- Released 8 April 1996 2 | % for LaTeX-2e 3 | % Copyright -- 1994 Peter Williams 4 | % E-mail Peter.Williams@dsto.defence.gov.au 5 | \NeedsTeXFormat{LaTeX2e} 6 | \ProvidesPackage{algorithm} 7 | \typeout{Document Style `algorithm' - floating environment} 8 | 9 | \RequirePackage{float} 10 | \RequirePackage{ifthen} 11 | \newcommand{\ALG@within}{nothing} 12 | \newboolean{ALG@within} 13 | \setboolean{ALG@within}{false} 14 | \newcommand{\ALG@floatstyle}{ruled} 15 | \newcommand{\ALG@name}{Algorithm} 16 | \newcommand{\listalgorithmname}{List of \ALG@name s} 17 | 18 | % Declare Options 19 | % first appearance 20 | \DeclareOption{plain}{ 21 | \renewcommand{\ALG@floatstyle}{plain} 22 | } 23 | \DeclareOption{ruled}{ 24 | \renewcommand{\ALG@floatstyle}{ruled} 25 | } 26 | \DeclareOption{boxed}{ 27 | \renewcommand{\ALG@floatstyle}{boxed} 28 | } 29 | % then numbering convention 30 | \DeclareOption{part}{ 31 | \renewcommand{\ALG@within}{part} 32 | \setboolean{ALG@within}{true} 33 | } 34 | \DeclareOption{chapter}{ 35 | \renewcommand{\ALG@within}{chapter} 36 | \setboolean{ALG@within}{true} 37 | } 38 | \DeclareOption{section}{ 39 | \renewcommand{\ALG@within}{section} 40 | \setboolean{ALG@within}{true} 41 | } 42 | \DeclareOption{subsection}{ 43 | \renewcommand{\ALG@within}{subsection} 44 | \setboolean{ALG@within}{true} 45 | } 46 | \DeclareOption{subsubsection}{ 47 | \renewcommand{\ALG@within}{subsubsection} 48 | \setboolean{ALG@within}{true} 49 | } 50 | \DeclareOption{nothing}{ 51 | \renewcommand{\ALG@within}{nothing} 52 | \setboolean{ALG@within}{true} 53 | } 54 | \DeclareOption*{\edef\ALG@name{\CurrentOption}} 55 | 56 | % ALGORITHM 57 | % 58 | \ProcessOptions 59 | \floatstyle{\ALG@floatstyle} 60 | \ifthenelse{\boolean{ALG@within}}{ 61 | \ifthenelse{\equal{\ALG@within}{part}} 62 | {\newfloat{algorithm}{htbp}{loa}[part]}{} 63 | \ifthenelse{\equal{\ALG@within}{chapter}} 64 | {\newfloat{algorithm}{htbp}{loa}[chapter]}{} 65 | \ifthenelse{\equal{\ALG@within}{section}} 66 | {\newfloat{algorithm}{htbp}{loa}[section]}{} 67 | \ifthenelse{\equal{\ALG@within}{subsection}} 68 | {\newfloat{algorithm}{htbp}{loa}[subsection]}{} 69 | \ifthenelse{\equal{\ALG@within}{subsubsection}} 70 | {\newfloat{algorithm}{htbp}{loa}[subsubsection]}{} 71 | \ifthenelse{\equal{\ALG@within}{nothing}} 72 | {\newfloat{algorithm}{htbp}{loa}}{} 73 | }{ 74 | \newfloat{algorithm}{htbp}{loa} 75 | } 76 | \floatname{algorithm}{\ALG@name} 77 | 78 | \newcommand{\listofalgorithms}{\listof{algorithm}{\listalgorithmname}} 79 | -------------------------------------------------------------------------------- /doc/architecture-constraint: -------------------------------------------------------------------------------- 1 | 2 | 3 | Constraint 4 | 5 | addToSet (set): 6 | set.push_back (self) 7 | addLockedJoint (lockedJoint): 8 | pass 9 | 10 | 11 | ConfigProjector ConstraintSet 12 | 13 | add (LockedJoint ld): addToSet (set): 14 | lockedJoints_.push_back (ld) for c in constraints_: 15 | c.addToSet (set) 16 | addToSet (set): 17 | if set.configProjector_: addConstraint (c): 18 | throw c.addToSet (self) 19 | Constraint.addToSet (set) 20 | set.removeFirstElement () 21 | set.configProjector_ = self 22 | Constraint::addToSet (set) 23 | 24 | 25 | LockedJoint 26 | -------------------------------------------------------------------------------- /doc/continuous-collision-checking.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humanoid-path-planner/hpp-core/a306cad1195138b0ec8b07ce98a589240083b5e6/doc/continuous-collision-checking.pdf -------------------------------------------------------------------------------- /doc/continuous-validation.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humanoid-path-planner/hpp-core/a306cad1195138b0ec8b07ce98a589240083b5e6/doc/continuous-validation.pdf -------------------------------------------------------------------------------- /doc/continuous-validation.tex: -------------------------------------------------------------------------------- 1 | \title{AlgorithmTemplate} 2 | \documentclass[10pt]{article} 3 | \usepackage{fullpage} 4 | \usepackage{times} 5 | \usepackage{fancyhdr,graphicx,amsmath,amssymb} 6 | \usepackage{algorithm}% http://ctan.org/pkg/algorithms 7 | \usepackage{listings} 8 | \usepackage{algpseudocode}% http://ctan.org/pkg/algorithmicx 9 | \usepackage{tikz-uml} 10 | 11 | \renewcommand{\algorithmiccomment}[1]{\hfill$\mathbin{ \textcolor{blue}{\triangleright} }$\textcolor{blue}{\ #1}} 12 | 13 | 14 | \begin{document} 15 | 16 | \title{Continuous validation in hpp-core} 17 | \author{Diane Bury} 18 | \maketitle 19 | 20 | \section{UML diagramm of the new Continuous Validation architecture} 21 | 22 | To enable the use of other type of validations than collision checking between two solids, the continuous validation in $hpp-core$ has been reworked (previously \textit{continuous collision checking}). With this new architecture, it is possible to create new classes for continuous validation. The continuous collision checking is now a part of this larger continuous validation. The following diagram is simplified for clarity. 23 | 24 | \begin{center} 25 | \begin{tikzpicture} 26 | \begin{umlpackage}{hpp-core} 27 | \umlclass[x=-2.5, y=6, type=abstract]{PathValidation}{}{ 28 | + \textit{validate()}\\ 29 | } 30 | 31 | \umlclass[x=1.5, y=6, type=abstract]{ObstacleUserInterface}{}{ 32 | + addObstacle()\\ 33 | + removeObstacleFromJoints() } 34 | 35 | \umlclass[y=0.5,x=-6.5, type=abstract]{IntervalValidation}{ 36 | - path\_ : PathPtr\_t \\ 37 | - validInterval\_ : interval\_set}{ 38 | + \textit{validateConfiguration()}\\ 39 | + path()} 40 | 41 | 42 | \umlclass[y=4,x=-6.5, type=abstract]{BodyPairCollision}{ 43 | \# pairs 44 | }{ 45 | + validateConfiguration()\\ 46 | - collisionFreeInterval()\\ 47 | - computeDistanceLowerBound()\\ 48 | - \textit{computeMaxVelocity()} } 49 | 50 | \umlclass[y=7.5,x=-6.5]{SolidSolidCollision}{ 51 | - joint\_a\_ : JointPtr\_t\\ 52 | - joint\_b\_ : JointPtr\_t}{ 53 | - computeMaximalVelocity()\\ 54 | - computeSequenceOfJoints()\\ 55 | - computeCoefficients()} 56 | 57 | \umlclass[x=-0.5, y=1.5, type=abstract]{ContinuousValidation} 58 | {\# tolerance : value\_type\\ 59 | \# intervalValidations\_ : IntervalValidations\_t} 60 | {+ validate()\\ 61 | + setPath()\\ 62 | + addObstacle()\\ 63 | + removeObstacleFromJoints()\\ 64 | \# validateConfiguration()\\ 65 | - generateAutoCollision();\\ 66 | - \textit{validateStraightPath()}\\ 67 | } 68 | 69 | \umlclass[y=-2.6, x=-5]{ContinuousValidation::Dichotomy}{ 70 | }{ 71 | - validateStraightPath() \\ 72 | - buildTensionIntervalValidations() \\ 73 | - buildBodyPairCollisions() \\ 74 | } 75 | 76 | \umlclass[y=-2.5, x=1]{ContinuousValidation::Progressive}{}{ 77 | - validateStraightPath()} 78 | 79 | \end{umlpackage} 80 | 81 | \umlinherit{BodyPairCollision}{IntervalValidation} 82 | \umlinherit{SolidSolidCollision}{BodyPairCollision} 83 | \umlinherit{ContinuousValidation}{PathValidation} 84 | \umlinherit{ContinuousValidation}{ObstacleUserInterface} 85 | \umlinherit{ContinuousValidation::Dichotomy}{ContinuousValidation} 86 | \umlinherit{ContinuousValidation::Progressive}{ContinuousValidation} 87 | \umlaggreg[mult=n]{ContinuousValidation}{IntervalValidation} 88 | 89 | \end{tikzpicture} 90 | \end{center} 91 | 92 | \end{document} 93 | -------------------------------------------------------------------------------- /doc/finiteStateMachine.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humanoid-path-planner/hpp-core/a306cad1195138b0ec8b07ce98a589240083b5e6/doc/finiteStateMachine.pdf -------------------------------------------------------------------------------- /doc/footer.html: -------------------------------------------------------------------------------- 1 |

2 |
3 |
4 | 5 |
$projectname library documentation
6 |
7 |
8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /doc/framesConstraintError.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humanoid-path-planner/hpp-core/a306cad1195138b0ec8b07ce98a589240083b5e6/doc/framesConstraintError.pdf -------------------------------------------------------------------------------- /doc/gradient.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humanoid-path-planner/hpp-core/a306cad1195138b0ec8b07ce98a589240083b5e6/doc/gradient.pdf -------------------------------------------------------------------------------- /doc/header.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | $projectname library documentation 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /doc/kinematic-chain.fig: -------------------------------------------------------------------------------- 1 | #FIG 3.2 Produced by xfig version 3.2.5b 2 | Landscape 3 | Center 4 | Metric 5 | A4 6 | 100.00 7 | Single 8 | -2 9 | 1200 2 10 | 6 4545 900 5220 1575 11 | 2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 1 0 2 12 | 0 0 1.00 60.00 120.00 13 | 4590 1530 5220 1530 14 | 2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 1 0 2 15 | 0 0 1.00 60.00 120.00 16 | 4590 1530 4590 900 17 | -6 18 | 6 3690 1350 4545 1980 19 | 2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 1 0 2 20 | 0 0 1.00 60.00 120.00 21 | 3950 1967 4542 1751 22 | 2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 1 0 2 23 | 0 0 1.00 60.00 120.00 24 | 3950 1967 3734 1374 25 | -6 26 | 6 3285 1035 3870 1935 27 | 2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 1 0 2 28 | 0 0 1.00 60.00 120.00 29 | 3319 1607 3864 1922 30 | 2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 1 0 2 31 | 0 0 1.00 60.00 120.00 32 | 3319 1607 3634 1061 33 | -6 34 | 6 1485 1530 2160 2205 35 | 2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 1 0 2 36 | 0 0 1.00 60.00 120.00 37 | 1530 2160 2160 2160 38 | 2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 1 0 2 39 | 0 0 1.00 60.00 120.00 40 | 1530 2160 1530 1530 41 | -6 42 | 6 630 2070 1485 2700 43 | 2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 1 0 2 44 | 0 0 1.00 60.00 120.00 45 | 890 2687 1482 2471 46 | 2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 1 0 2 47 | 0 0 1.00 60.00 120.00 48 | 890 2687 674 2094 49 | -6 50 | 1 3 0 1 0 7 50 -1 -1 0.000 1 0.0000 4590 1530 101 101 4590 1530 4680 1575 51 | 1 3 0 1 0 7 50 -1 -1 0.000 1 0.0000 900 2700 101 101 900 2700 990 2745 52 | 1 3 0 1 0 7 50 -1 -1 0.000 1 0.0000 1530 2160 101 101 1530 2160 1620 2205 53 | 1 3 0 1 0 7 50 -1 -1 0.000 1 0.0000 3960 1935 101 101 3960 1935 4050 1980 54 | 1 3 0 1 0 7 50 -1 -1 0.000 1 0.0000 3285 1620 101 101 3285 1620 3375 1665 55 | 1 3 0 1 0 0 50 -1 20 0.000 1 0.0000 225 2295 45 45 225 2295 270 2295 56 | 4 0 0 50 -1 0 12 0.0000 2 180 945 4950 1755 $O_{m-1}$\001 57 | 4 0 0 50 -1 0 12 0.0000 2 180 885 4770 990 $J_{m-1}$\001 58 | 4 0 0 50 -1 0 12 0.0000 2 180 735 1665 2475 $O_{1}$\001 59 | 4 0 0 50 -1 0 12 0.0000 2 180 675 1620 1710 $J_{1}$\001 60 | 4 0 0 50 -1 0 12 0.0000 2 180 675 315 1980 $J_{0}$\001 61 | 4 0 0 50 -1 0 12 0.0000 2 180 735 855 3060 $O_{0}$\001 62 | 4 0 0 50 -1 0 12 0.0000 2 180 945 3780 2295 $O_{m-2}$\001 63 | 4 0 0 50 -1 0 12 0.0000 2 180 885 3870 1530 $J_{m-2}$\001 64 | 4 0 0 50 -1 0 12 0.0000 2 180 885 3330 945 $J_{m-3}$\001 65 | 4 0 0 50 -1 0 12 0.0000 2 180 945 2880 1350 $O_{m-3}$\001 66 | 4 0 0 50 -1 0 12 0.0000 2 180 780 90 2610 $M_{0}$\001 67 | -------------------------------------------------------------------------------- /doc/kinematic-chain.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humanoid-path-planner/hpp-core/a306cad1195138b0ec8b07ce98a589240083b5e6/doc/kinematic-chain.pdf -------------------------------------------------------------------------------- /doc/kinematic-chain.pdf_t: -------------------------------------------------------------------------------- 1 | \begin{picture}(0,0)% 2 | \includegraphics{kinematic-chain.pdf}% 3 | \end{picture}% 4 | \setlength{\unitlength}{4144sp}% 5 | % 6 | \begingroup\makeatletter\ifx\SetFigFont\undefined% 7 | \gdef\SetFigFont#1#2#3#4#5{% 8 | \reset@font\fontsize{#1}{#2pt}% 9 | \fontfamily{#3}\fontseries{#4}\fontshape{#5}% 10 | \selectfont}% 11 | \fi\endgroup% 12 | \begin{picture}(5157,2343)(76,-2290) 13 | \put(4951,-916){\makebox(0,0)[lb]{\smash{{\SetFigFont{12}{14.4}{\familydefault}{\mddefault}{\updefault}{\color[rgb]{0,0,0}$O_{m-1}$}% 14 | }}}} 15 | \put(4771,-151){\makebox(0,0)[lb]{\smash{{\SetFigFont{12}{14.4}{\familydefault}{\mddefault}{\updefault}{\color[rgb]{0,0,0}$J_{m-1}$}% 16 | }}}} 17 | \put(1666,-1636){\makebox(0,0)[lb]{\smash{{\SetFigFont{12}{14.4}{\familydefault}{\mddefault}{\updefault}{\color[rgb]{0,0,0}$O_{1}$}% 18 | }}}} 19 | \put(1621,-871){\makebox(0,0)[lb]{\smash{{\SetFigFont{12}{14.4}{\familydefault}{\mddefault}{\updefault}{\color[rgb]{0,0,0}$J_{1}$}% 20 | }}}} 21 | \put(316,-1141){\makebox(0,0)[lb]{\smash{{\SetFigFont{12}{14.4}{\familydefault}{\mddefault}{\updefault}{\color[rgb]{0,0,0}$J_{0}$}% 22 | }}}} 23 | \put(856,-2221){\makebox(0,0)[lb]{\smash{{\SetFigFont{12}{14.4}{\familydefault}{\mddefault}{\updefault}{\color[rgb]{0,0,0}$O_{0}$}% 24 | }}}} 25 | \put(3781,-1456){\makebox(0,0)[lb]{\smash{{\SetFigFont{12}{14.4}{\familydefault}{\mddefault}{\updefault}{\color[rgb]{0,0,0}$O_{m-2}$}% 26 | }}}} 27 | \put(3871,-691){\makebox(0,0)[lb]{\smash{{\SetFigFont{12}{14.4}{\familydefault}{\mddefault}{\updefault}{\color[rgb]{0,0,0}$J_{m-2}$}% 28 | }}}} 29 | \put(3331,-106){\makebox(0,0)[lb]{\smash{{\SetFigFont{12}{14.4}{\familydefault}{\mddefault}{\updefault}{\color[rgb]{0,0,0}$J_{m-3}$}% 30 | }}}} 31 | \put(2881,-511){\makebox(0,0)[lb]{\smash{{\SetFigFont{12}{14.4}{\familydefault}{\mddefault}{\updefault}{\color[rgb]{0,0,0}$O_{m-3}$}% 32 | }}}} 33 | \put( 91,-1771){\makebox(0,0)[lb]{\smash{{\SetFigFont{12}{14.4}{\familydefault}{\mddefault}{\updefault}{\color[rgb]{0,0,0}$M_{0}$}% 34 | }}}} 35 | \end{picture}% 36 | -------------------------------------------------------------------------------- /doc/linearError.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humanoid-path-planner/hpp-core/a306cad1195138b0ec8b07ce98a589240083b5e6/doc/linearError.pdf -------------------------------------------------------------------------------- /doc/main-page/news.hh: -------------------------------------------------------------------------------- 1 | /** 2 | 3 | \page hpp_core_news News 4 | 5 | New features from one version to another. 6 | 7 | \section hpp_core_news_3_1to_3_2 New features between version 3.1 and 3.2 8 | 9 | \li Several path optimizer can be inserted in the problem solver. When calling method \c solve, they are called in the order in which they have been inserted. Method \c clearPathOptimizers has also been added. 10 | 11 | \li Copy constructors have been implemented in classes related to constraints. Copying a path now implies to copy the associated constraints. This modification fixes some hazards in the use of constraints. 12 | 13 | \li A static method that creates a \c ProblemSolver instance has been added. 14 | Using this method makes the latest created instance available by static 15 | method \c ProblemSolver::latest. 16 | 17 | \li ConfigProjector::computeValueAndJacobian is now public. This enables users 18 | to handle themselves a set of numerical constraints, and to implement their 19 | own resolution algorithm. In the same class, method \c compressVector, 20 | \c uncompressVector, \c compressMatrix, \c uncompressMatrix enable users 21 | to switch from velocity vectors without locked degrees of freedom to 22 | velocity vector that contains locked degrees of freedom, and to switch 23 | from matrices without the columns (and the lines for Hessian matrices) 24 | corresponding to locked degrees of freedom to matrices that contain them. 25 | -------------------------------------------------------------------------------- /doc/path-optimization.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humanoid-path-planner/hpp-core/a306cad1195138b0ec8b07ce98a589240083b5e6/doc/path-optimization.pdf -------------------------------------------------------------------------------- /doc/pictures/archi.fig: -------------------------------------------------------------------------------- 1 | #FIG 3.2 2 | Portrait 3 | Center 4 | Metric 5 | Letter 6 | 100.00 7 | Single 8 | -2 9 | 1200 2 10 | 6 8321 320 12668 1104 11 | 3 2 1 1 0 7 50 -1 -1 4.000 0 1 0 3 12 | 3 1 1.00 67.98 135.96 13 | 8321 730 9566 413 10768 1104 14 | 0.000 -1.000 0.000 15 | 4 0 0 50 -1 0 15 0.0000 4 217 2105 10563 628 dynamic library loading\001 16 | -6 17 | 6 2557 5650 4952 6575 18 | 2 2 0 3 0 7 50 -1 -1 0.000 0 0 7 0 0 5 19 | 2626 5707 4919 5707 4919 6522 2626 6522 2626 5707 20 | 4 1 0 50 -1 2 17 0.0000 4 181 1433 3747 6211 KineoWorks\001 21 | -6 22 | 2 1 0 2 0 7 50 -1 -1 0.000 0 0 -1 0 0 2 23 | 1361 2133 2378 2133 24 | 2 1 0 2 0 7 50 -1 -1 0.000 0 0 -1 0 0 2 25 | 1361 2431 2378 2431 26 | 2 1 0 2 0 7 50 -1 -1 0.000 0 0 -1 0 0 3 27 | 2274 2019 2591 2260 2304 2518 28 | 2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 0 0 2 29 | 2611 1975 4923 1975 30 | 2 2 0 3 0 7 50 -1 -1 0.000 0 0 -1 0 0 5 31 | 6126 2950 8449 2950 8449 1590 6126 1590 6126 2950 32 | 2 1 0 3 0 7 50 -1 -1 0.000 0 0 -1 1 0 3 33 | 3 1 1.00 67.13 136.08 34 | 4919 4164 7242 4164 7242 2963 35 | 2 1 1 3 0 7 50 -1 -1 4.000 0 0 -1 1 0 2 36 | 3 1 1.00 67.13 136.08 37 | 3770 2963 3754 3418 38 | 2 2 0 3 0 7 50 -1 -1 0.000 0 0 -1 0 0 5 39 | 2611 2950 4935 2950 4935 1590 2611 1590 2611 2950 40 | 2 2 0 2 0 7 50 -1 -1 0.000 0 0 -1 0 0 5 41 | 10402 2950 12725 2950 12725 1590 10402 1590 10402 2950 42 | 2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 1 0 2 43 | 3 1 1.00 72.58 145.16 44 | 8450 2248 10400 2248 45 | 2 1 1 3 0 7 50 -1 -1 4.000 0 0 -1 1 0 2 46 | 3 1 1.00 72.58 145.16 47 | 6131 2255 4934 2255 48 | 2 2 1 3 0 7 50 -1 -1 4.000 0 0 -1 0 0 5 49 | 2013 864 8623 864 8623 7011 2013 7011 2013 864 50 | 2 2 1 3 0 7 50 -1 -1 4.000 0 0 -1 0 0 5 51 | 2176 1082 5453 1082 5453 6793 2176 6793 2176 1082 52 | 2 1 1 1 0 7 50 -1 -1 4.000 0 0 -1 1 0 2 53 | 3 1 1.00 67.98 135.96 54 | 3754 5323 3770 5692 55 | 2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 0 0 2 56 | 2611 4017 4897 4017 57 | 2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 0 0 2 58 | 2611 4452 4897 4452 59 | 2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 0 0 2 60 | 2611 4888 4897 4888 61 | 2 2 0 3 0 7 50 -1 -1 0.000 0 0 7 0 0 5 62 | 2611 3472 4919 3472 4919 5323 2611 5323 2611 3472 63 | 2 1 1 3 0 7 50 -1 -1 4.000 0 0 -1 1 0 2 64 | 3 1 1.00 67.13 136.08 65 | 8841 5971 11164 5971 66 | 2 1 1 3 0 7 50 -1 -1 4.000 0 0 -1 0 1 3 67 | 3 1 1.00 72.58 145.16 68 | 4952 6140 11713 6140 11701 2982 69 | 2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 0 0 2 70 | 2611 2329 4952 2329 71 | 2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 0 0 2 72 | 2611 2710 4952 2710 73 | 4 1 0 50 -1 2 17 0.0000 4 235 1615 7180 1896 Kpp Interface\001 74 | 4 1 0 50 -1 0 17 0.0000 4 181 1887 11551 1896 KineoPathPlanner\001 75 | 4 0 0 50 -1 0 15 0.0000 4 163 1470 8769 2103 initializeModule\001 76 | 4 0 0 50 -1 0 15 0.0000 4 150 735 1391 2350 CORBA\001 77 | 4 0 0 50 -1 2 17 0.0000 4 181 1415 5789 1255 KPP module\001 78 | 4 0 0 50 -1 2 15 0.0000 4 163 1706 5540 4054 send notifications\001 79 | 4 1 0 50 -1 2 17 0.0000 4 235 1651 3700 3853 HPP packages\001 80 | 4 1 0 50 -1 2 17 0.0000 4 235 980 3700 4289 hppCore\001 81 | 4 1 0 50 -1 2 17 0.0000 4 235 1978 3700 4724 hppWalkPlanner\001 82 | 4 1 0 50 -1 2 17 0.0000 4 36 218 3700 5160 ...\001 83 | 4 0 0 50 -1 2 15 0.0000 4 217 2322 8965 5870 compilation dependence\001 84 | 4 1 0 50 -1 2 17 0.0000 4 235 2341 3809 2601 hppWPCorbaServer\001 85 | 4 1 0 50 -1 2 17 0.0000 4 235 1905 3754 2220 hppCorbaServer\001 86 | 4 1 0 50 -1 2 17 0.0000 4 181 2068 3700 1896 CORBA Interface\001 87 | 4 0 0 50 -1 2 17 0.0000 4 181 1833 2257 1388 CORBA module\001 88 | -------------------------------------------------------------------------------- /doc/pictures/archi.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humanoid-path-planner/hpp-core/a306cad1195138b0ec8b07ce98a589240083b5e6/doc/pictures/archi.pdf -------------------------------------------------------------------------------- /doc/pictures/archi.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humanoid-path-planner/hpp-core/a306cad1195138b0ec8b07ce98a589240083b5e6/doc/pictures/archi.png -------------------------------------------------------------------------------- /doc/pictures/footer.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humanoid-path-planner/hpp-core/a306cad1195138b0ec8b07ce98a589240083b5e6/doc/pictures/footer.jpg -------------------------------------------------------------------------------- /doc/projection.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humanoid-path-planner/hpp-core/a306cad1195138b0ec8b07ce98a589240083b5e6/doc/projection.pdf -------------------------------------------------------------------------------- /doc/projection.tex: -------------------------------------------------------------------------------- 1 | \documentclass {article} 2 | 3 | \newcommand\reals{\mathbf{R}} 4 | \begin{document} 5 | \paragraph {Projection on the kernel of a matrix $J$} 6 | $$ 7 | J\in \reals^{m\times n} 8 | $$ 9 | SVD decomposition 10 | \begin{eqnarray*} 11 | J &=& U S V^T \ \ \ U\in O(m)\ \ \ V\in O(n)\\ 12 | J^{+} &=& V S^{+} U^T \\ 13 | J^{+} J &=& V S^{+} S V^T \\ 14 | &=& V \left(\begin{array} {cc} 15 | I_m & O_{m\times n-m} \\ 16 | O_{n-m\times m} & O_{n-m\times n-m} 17 | \end{array}\right) V^T 18 | \end{eqnarray*} 19 | where $m$ is the (full) rank of $J$. 20 | \begin{eqnarray*} 21 | I_{n} - J^{+} J &=& V \left(\begin{array} {cc} 22 | O_m & O_{m\times n-m} \\ 23 | O_{n-m\times m} & I_{n-m} 24 | \end{array}\right) V^T \\ 25 | &=& \left(\begin{array}{cc} 26 | V_{1} & V_{0} \end{array}\right) 27 | \left(\begin{array} {cc} 28 | O_m & O_{m\times n-m} \\ 29 | O_{n-m\times m} & I_{n-m} 30 | \end{array}\right) 31 | \left(\begin{array}{c} 32 | V^T_{1} \\ 33 | V^T_{0} \end{array}\right)\\ 34 | &=& 35 | V_0 V_0^T 36 | \end{eqnarray*} 37 | 38 | \paragraph {Projection on the kernel of a matrix $J$} 39 | $$ 40 | J\in \reals^{m\times n} 41 | $$ 42 | \begin{eqnarray*} 43 | J^T P &=& QR 44 | \end{eqnarray*} 45 | where $P\in O(m)$ is a permutation matrix, $Q\in O(n)$ and $R\in\reals^{n\times m}$ is an upper triangular matrix. 46 | \begin{eqnarray*} 47 | J &=& PLQ^T 48 | \end{eqnarray*} 49 | where $L = R^T$. 50 | \begin{eqnarray*} 51 | L &=& \left(\begin{array}{cc} L_1 & 0 \end{array}\right)\\ 52 | L^+ &=& \left(\begin{array}{c} L_1^-1 \\ 0 \end{array}\right)\\ 53 | L^+ L &=& \left(\begin{array}{cc} 54 | I_r & 0 \\ 55 | 0 & 0 \end{array}\right) 56 | \end{eqnarray*} 57 | where $r$ is the rank of $L$. 58 | \begin{eqnarray*} 59 | J^+ J &=& \left(\begin{array}{cc} Q_1 & Q_0\end{array}\right) 60 | \left(\begin{array}{cc} I_r & 0 \\ 0 & 0\end{array}\right) 61 | \left(\begin{array}{cc} Q_1 & Q_0\end{array}\right)^T \\ 62 | &=& Q_1 Q_1^T\\ 63 | I_n - J^+ J &=& Q_0 Q_0^T 64 | \end{eqnarray*} 65 | \end{document} 66 | -------------------------------------------------------------------------------- /doc/quaternions.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humanoid-path-planner/hpp-core/a306cad1195138b0ec8b07ce98a589240083b5e6/doc/quaternions.pdf -------------------------------------------------------------------------------- /doc/weighted_path.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humanoid-path-planner/hpp-core/a306cad1195138b0ec8b07ce98a589240083b5e6/doc/weighted_path.pdf -------------------------------------------------------------------------------- /flake.lock: -------------------------------------------------------------------------------- 1 | { 2 | "nodes": { 3 | "flake-parts": { 4 | "inputs": { 5 | "nixpkgs-lib": [ 6 | "nixpkgs" 7 | ] 8 | }, 9 | "locked": { 10 | "lastModified": 1719877454, 11 | "narHash": "sha256-g5N1yyOSsPNiOlFfkuI/wcUjmtah+nxdImJqrSATjOU=", 12 | "owner": "hercules-ci", 13 | "repo": "flake-parts", 14 | "rev": "4e3583423212f9303aa1a6337f8dffb415920e4f", 15 | "type": "github" 16 | }, 17 | "original": { 18 | "owner": "hercules-ci", 19 | "repo": "flake-parts", 20 | "type": "github" 21 | } 22 | }, 23 | "nixpkgs": { 24 | "locked": { 25 | "lastModified": 1727174734, 26 | "narHash": "sha256-xa3TynMF5vaWonmTOg/Ejc1Fmo0GkQnCaVRVkBc3z2I=", 27 | "owner": "gepetto", 28 | "repo": "nixpkgs", 29 | "rev": "0ad139a0e4372abc12320c8c92ee90e0e5e296e1", 30 | "type": "github" 31 | }, 32 | "original": { 33 | "owner": "gepetto", 34 | "repo": "nixpkgs", 35 | "type": "github" 36 | } 37 | }, 38 | "root": { 39 | "inputs": { 40 | "flake-parts": "flake-parts", 41 | "nixpkgs": "nixpkgs" 42 | } 43 | } 44 | }, 45 | "root": "root", 46 | "version": 7 47 | } 48 | -------------------------------------------------------------------------------- /flake.nix: -------------------------------------------------------------------------------- 1 | { 2 | description = "The core algorithms of the Humanoid Path Planner framework"; 3 | 4 | inputs = { 5 | nixpkgs.url = "github:gepetto/nixpkgs"; 6 | flake-parts = { 7 | url = "github:hercules-ci/flake-parts"; 8 | inputs.nixpkgs-lib.follows = "nixpkgs"; 9 | }; 10 | }; 11 | 12 | outputs = 13 | inputs: 14 | inputs.flake-parts.lib.mkFlake { inherit inputs; } { 15 | systems = [ 16 | "x86_64-linux" 17 | "aarch64-linux" 18 | "aarch64-darwin" 19 | "x86_64-darwin" 20 | ]; 21 | perSystem = 22 | { pkgs, self', ... }: 23 | { 24 | devShells.default = pkgs.mkShell { inputsFrom = [ self'.packages.default ]; }; 25 | packages = { 26 | default = self'.packages.hpp-core; 27 | hpp-core = pkgs.hpp-core.overrideAttrs (_: { 28 | # TODO: remove this after next release 29 | patches = []; 30 | src = pkgs.lib.fileset.toSource { 31 | root = ./.; 32 | fileset = pkgs.lib.fileset.unions [ 33 | ./cmake-modules 34 | ./CMakeLists.txt 35 | ./doc 36 | ./include 37 | ./package.xml 38 | ./plugins 39 | ./src 40 | ./tests 41 | ]; 42 | }; 43 | }); 44 | }; 45 | }; 46 | }; 47 | } 48 | -------------------------------------------------------------------------------- /include/hpp/core/bi-rrt-planner.hh: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2014 CNRS 3 | // Authors: Florent Lamiraux 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #ifndef HPP_CORE_BIRRT_PLANNER_HH 31 | #define HPP_CORE_BIRRT_PLANNER_HH 32 | 33 | #include 34 | 35 | namespace hpp { 36 | namespace core { 37 | /// \addtogroup path_planning 38 | /// \{ 39 | 40 | /// Implementation of directional bi-RRT algorithm 41 | /// maintaining only two connected components for 42 | /// respectively the start and goal configurations 43 | class HPP_CORE_DLLAPI BiRRTPlanner : public PathPlanner { 44 | public: 45 | /// Return shared pointer to new object. 46 | static BiRRTPlannerPtr_t createWithRoadmap(const ProblemConstPtr_t& problem, 47 | const RoadmapPtr_t& roadmap); 48 | /// Return shared pointer to new object. 49 | static BiRRTPlannerPtr_t create(const ProblemConstPtr_t& problem); 50 | /// One step of extension. 51 | virtual void startSolve(); 52 | /// One step of extension. 53 | virtual void oneStep(); 54 | 55 | protected: 56 | /// Constructor 57 | BiRRTPlanner(const ProblemConstPtr_t& problem, const RoadmapPtr_t& roadmap); 58 | /// Constructor with roadmap 59 | BiRRTPlanner(const ProblemConstPtr_t& problem); 60 | /// Store weak pointer to itself 61 | void init(const BiRRTPlannerWkPtr_t& weak); 62 | PathPtr_t extendInternal(const SteeringMethodPtr_t& sm, 63 | Configuration_t& qProj_, const NodePtr_t& near, 64 | const Configuration_t& target, bool reverse = false); 65 | 66 | ConfigurationShooterPtr_t configurationShooter_; 67 | ConnectedComponentPtr_t startComponent_; 68 | std::vector endComponents_; 69 | 70 | private: 71 | mutable Configuration_t qProj_; 72 | BiRRTPlannerWkPtr_t weakPtr_; 73 | }; 74 | /// \} 75 | } // namespace core 76 | } // namespace hpp 77 | #endif // HPP_CORE_BIRRT_PLANNER_HH 78 | -------------------------------------------------------------------------------- /include/hpp/core/collision-path-validation-report.hh: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2014 CNRS 3 | // Authors: Florent Lamiraux 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #ifndef HPP_CORE_COLLISION_PATH_VALIDATION_REPORT_HH 31 | #define HPP_CORE_COLLISION_PATH_VALIDATION_REPORT_HH 32 | 33 | #include 34 | #include 35 | 36 | namespace hpp { 37 | namespace core { 38 | /// \addtogroup validation 39 | /// \{ 40 | 41 | /// Path validation report used for standard collision checking 42 | struct HPP_CORE_DLLAPI CollisionPathValidationReport 43 | : public PathValidationReport { 44 | CollisionPathValidationReport() : PathValidationReport() { 45 | configurationReport = 46 | CollisionValidationReportPtr_t(new CollisionValidationReport); 47 | } 48 | 49 | CollisionPathValidationReport(const value_type& param, 50 | const ValidationReportPtr_t& report) 51 | : PathValidationReport(param, report) {} 52 | }; // struct CollisionPathValidationReport 53 | /// \} 54 | } // namespace core 55 | } // namespace hpp 56 | 57 | #endif // HPP_CORE_COLLISION_PATH_VALIDATION_REPORT_HH 58 | -------------------------------------------------------------------------------- /include/hpp/core/collision-validation.hh: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2014 CNRS 3 | // Authors: Florent Lamiraux 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #ifndef HPP_CORE_COLLISION_VALIDATION_HH 31 | #define HPP_CORE_COLLISION_VALIDATION_HH 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | namespace hpp { 40 | namespace core { 41 | /// \addtogroup validation 42 | /// \{ 43 | 44 | /// Validate a configuration with respect to collision 45 | /// 46 | class HPP_CORE_DLLAPI CollisionValidation : public ConfigValidation, 47 | public ObstacleUser { 48 | public: 49 | static CollisionValidationPtr_t create(const DevicePtr_t& robot); 50 | 51 | /// Compute whether the configuration is valid 52 | /// 53 | /// \param config the config to check for validity, 54 | /// \retval validationReport report on validation. If non valid, 55 | /// a validation report will be allocated and returned via this 56 | /// shared pointer. 57 | /// \return whether the whole config is valid. 58 | virtual bool validate(const Configuration_t& config, 59 | ValidationReportPtr_t& validationReport); 60 | 61 | void checkParameterized(bool active) { checkParameterized_ = active; } 62 | 63 | void computeAllContacts(bool computeAllContacts) { 64 | computeAllContacts_ = computeAllContacts; 65 | } 66 | 67 | bool checkParameterized() const { return checkParameterized_; } 68 | 69 | protected: 70 | CollisionValidation(const DevicePtr_t& robot); 71 | DevicePtr_t robot_; 72 | 73 | private: 74 | bool checkParameterized_; 75 | bool computeAllContacts_; 76 | 77 | }; // class ConfigValidation 78 | /// \} 79 | } // namespace core 80 | } // namespace hpp 81 | 82 | #endif // HPP_CORE_COLLISION_VALIDATION_HH 83 | -------------------------------------------------------------------------------- /include/hpp/core/config-validation.hh: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2014 CNRS 3 | // Authors: Florent Lamiraux 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #ifndef HPP_CORE_CONFIG_VALIDATION_HH 31 | #define HPP_CORE_CONFIG_VALIDATION_HH 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | namespace hpp { 38 | namespace core { 39 | /// \addtogroup validation 40 | /// \{ 41 | 42 | /// Abstraction of configuration validation 43 | /// 44 | /// Instances of this class validate configurations with respect to some 45 | /// criteria 46 | class HPP_CORE_DLLAPI ConfigValidation { 47 | public: 48 | /// Compute whether the configuration is valid 49 | /// 50 | /// \param config the config to check for validity, 51 | /// \retval validationReport report on validation. If non valid, 52 | /// a validation report will be allocated and returned via this 53 | /// shared pointer. 54 | /// \return whether the whole config is valid. 55 | virtual bool validate(const Configuration_t& config, 56 | ValidationReportPtr_t& validationReport) = 0; 57 | 58 | virtual ~ConfigValidation() = default; 59 | 60 | protected: 61 | ConfigValidation() = default; 62 | }; // class ConfigValidation 63 | /// \} 64 | } // namespace core 65 | } // namespace hpp 66 | 67 | #endif // HPP_CORE_CONFIG_VALIDATION_HH 68 | -------------------------------------------------------------------------------- /include/hpp/core/config-validations.hh: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2014 CNRS 3 | // Authors: Florent Lamiraux 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #ifndef HPP_CORE_CONFIG_VALIDATIONS_HH 31 | #define HPP_CORE_CONFIG_VALIDATIONS_HH 32 | 33 | #include 34 | #include 35 | 36 | namespace hpp { 37 | namespace core { 38 | /// \addtogroup validation 39 | /// \{ 40 | 41 | /// Validate a configuration with respect to collision 42 | /// 43 | class HPP_CORE_DLLAPI ConfigValidations 44 | : public ConfigValidation, 45 | public ObstacleUserVector { 46 | public: 47 | static ConfigValidationsPtr_t create(); 48 | 49 | /// Compute whether the configuration is valid 50 | /// 51 | /// \param config the config to check for validity, 52 | /// \retval validationReport report on validation. If non valid, 53 | /// a validation report will be allocated and returned via this 54 | /// shared pointer. 55 | /// \return whether the whole config is valid. 56 | virtual bool validate(const Configuration_t& config, 57 | ValidationReportPtr_t& validationReport); 58 | /// Add a configuration validation object 59 | void add(const ConfigValidationPtr_t& configValidation); 60 | 61 | /// Return the number of config validations 62 | size_type numberConfigValidations() const; 63 | 64 | protected: 65 | ConfigValidations() = default; 66 | ConfigValidations(std::initializer_list validations) 67 | : ObstacleUserVector(validations) {}; 68 | }; // class ConfigValidation 69 | /// \} 70 | } // namespace core 71 | } // namespace hpp 72 | 73 | #endif // HPP_CORE_CONFIG_VALIDATIONS_HH 74 | -------------------------------------------------------------------------------- /include/hpp/core/configuration-shooter.hh: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2014 CNRS 3 | // Authors: Florent Lamiraux 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #ifndef HPP_CORE_CONFIGURATION_SHOOTER_HH 31 | #define HPP_CORE_CONFIGURATION_SHOOTER_HH 32 | 33 | #include 34 | #include 35 | 36 | namespace hpp { 37 | namespace core { 38 | /// \addtogroup configuration_sampling 39 | /// \{ 40 | 41 | /// Abstraction of configuration shooter 42 | /// 43 | /// Configuration shooters are used by random sampling algorithms to 44 | /// generate new configurations 45 | class HPP_CORE_DLLAPI ConfigurationShooter { 46 | public: 47 | /// Shoot a random configuration 48 | virtual Configuration_t shoot() const { 49 | Configuration_t q; 50 | shoot(q); 51 | return q; 52 | } 53 | 54 | /// Shoot a random configuration 55 | /// \param q the configuration (resized if necessary). 56 | /// 57 | /// \deprecated This method is virtual for backward compatibility. It will 58 | /// become non-virtual in the future. Child classes should rather implement 59 | /// \ref impl_shoot so that both prototype of method shoot remain available. 60 | virtual void shoot(Configuration_t& q) const { impl_shoot(q); } 61 | 62 | virtual ~ConfigurationShooter() {}; 63 | 64 | protected: 65 | ConfigurationShooter() {} 66 | /// Store weak pointer to itself 67 | void init(const ConfigurationShooterWkPtr_t& weak) { weakPtr_ = weak; } 68 | 69 | virtual void impl_shoot(Configuration_t& q) const = 0; 70 | 71 | private: 72 | ConfigurationShooterWkPtr_t weakPtr_; 73 | }; // class 74 | } // namespace core 75 | /// \} 76 | } // namespace hpp 77 | #endif // HPP_CORE_CONFIGURATION_SHOOTER_HH 78 | -------------------------------------------------------------------------------- /include/hpp/core/configuration-shooter/uniform-tpl.hh: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Eureka Robotics 2 | // Authors: Joseph Mirabel 3 | 4 | #ifndef HPP_CORE_CONFIGURATION_SHOOTER_UNIFORM_TPL_HH 5 | #define HPP_CORE_CONFIGURATION_SHOOTER_UNIFORM_TPL_HH 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | namespace hpp { 12 | namespace core { 13 | namespace configurationShooter { 14 | 15 | /// \addtogroup configuration_sampling 16 | /// \{ 17 | 18 | /// Uniformly sample with bounds of degrees of freedom using a custom generator. 19 | template 20 | class HPP_CORE_DLLAPI UniformTpl : public ConfigurationShooter { 21 | public: 22 | typedef shared_ptr> Ptr_t; 23 | typedef weak_ptr> WkPtr_t; 24 | 25 | static Ptr_t create(const DevicePtr_t& robot) { 26 | UniformTpl* ptr = new UniformTpl(robot); 27 | Ptr_t shPtr(ptr); 28 | ptr->init(shPtr); 29 | return shPtr; 30 | } 31 | 32 | /// Set the generator seed. 33 | void seed(typename generator_t::result_type seed) { generator_.seed(seed); } 34 | 35 | protected: 36 | /// Uniformly sample configuration space 37 | /// 38 | /// Note that translation joints have to be bounded. 39 | UniformTpl(const DevicePtr_t& robot) : robot_(robot) {} 40 | void init(const Ptr_t& self) { 41 | ConfigurationShooter::init(self); 42 | weak_ = self; 43 | } 44 | 45 | virtual void impl_shoot(Configuration_t& q) const; 46 | 47 | private: 48 | DevicePtr_t robot_; 49 | WkPtr_t weak_; 50 | 51 | // The generator must be mutable because impl_shoot is const and 52 | // generator_t::operator() is not. 53 | mutable generator_t generator_; 54 | }; // class UniformTpl 55 | /// \} 56 | 57 | typedef UniformTpl UniformSeedable; 58 | 59 | } // namespace configurationShooter 60 | } // namespace core 61 | } // namespace hpp 62 | 63 | #endif // HPP_CORE_CONFIGURATION_SHOOTER_UNIFORM_TPL_HH 64 | -------------------------------------------------------------------------------- /include/hpp/core/configuration-shooter/uniform-tpl.hxx: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Eureka Robotics 2 | // Authors: Joseph Mirabel 3 | 4 | #ifndef HPP_CORE_CONFIGURATION_SHOOTER_UNIFORM_TPL_HXX 5 | #define HPP_CORE_CONFIGURATION_SHOOTER_UNIFORM_TPL_HXX 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | namespace hpp { 12 | namespace core { 13 | namespace configurationShooter { 14 | 15 | template 16 | void UniformTpl::impl_shoot(Configuration_t& config) const { 17 | typedef std::uniform_real_distribution distribution_t; 18 | 19 | if (!robot_->configSpace()->isVectorSpace()) { 20 | throw std::invalid_argument( 21 | "The device config space must be a vector space" 22 | " to use UniformTpl"); 23 | } 24 | 25 | config.resize(robot_->configSize()); 26 | auto const& model = robot_->model(); 27 | 28 | for (size_type i = 0; i < model.nq; ++i) { 29 | distribution_t distrib(model.lowerPositionLimit[i], 30 | model.upperPositionLimit[i]); 31 | config[i] = distrib(generator_); 32 | } 33 | auto const& ecs = robot_->extraConfigSpace(); 34 | for (size_type i = 0; i < ecs.dimension(); ++i) { 35 | distribution_t distrib(ecs.lower(i), ecs.upper(i)); 36 | config[model.nq + i] = distrib(generator_); 37 | } 38 | } 39 | 40 | } // namespace configurationShooter 41 | } // namespace core 42 | } // namespace hpp 43 | 44 | #endif // HPP_CORE_CONFIGURATION_SHOOTER_UNIFORM_TPL_HXX 45 | -------------------------------------------------------------------------------- /include/hpp/core/configuration-shooter/uniform.hh: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2014 CNRS 3 | // Authors: Florent Lamiraux 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #ifndef HPP_CORE_CONFIGURATION_SHOOTER_UNIFORM_HH 31 | #define HPP_CORE_CONFIGURATION_SHOOTER_UNIFORM_HH 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | namespace hpp { 38 | namespace core { 39 | namespace configurationShooter { 40 | /// \addtogroup configuration_sampling 41 | /// \{ 42 | 43 | /// Uniformly sample with bounds of degrees of freedom. 44 | class HPP_CORE_DLLAPI Uniform : public ConfigurationShooter { 45 | public: 46 | static UniformPtr_t create(const DevicePtr_t& robot) { 47 | Uniform* ptr = new Uniform(robot); 48 | UniformPtr_t shPtr(ptr); 49 | ptr->init(shPtr); 50 | return shPtr; 51 | } 52 | 53 | void sampleExtraDOF(bool sampleExtraDOF) { sampleExtraDOF_ = sampleExtraDOF; } 54 | 55 | protected: 56 | /// Uniformly sample configuration space 57 | /// 58 | /// Note that translation joints have to be bounded. 59 | Uniform(const DevicePtr_t& robot) : robot_(robot), sampleExtraDOF_(true) {} 60 | void init(const UniformPtr_t& self) { 61 | ConfigurationShooter::init(self); 62 | weak_ = self; 63 | } 64 | 65 | virtual void impl_shoot(Configuration_t& q) const; 66 | 67 | private: 68 | DevicePtr_t robot_; 69 | bool sampleExtraDOF_; 70 | UniformWkPtr_t weak_; 71 | }; // class Uniform 72 | /// \} 73 | } // namespace configurationShooter 74 | } // namespace core 75 | } // namespace hpp 76 | 77 | #endif // HPP_CORE_CONFIGURATION_SHOOTER_UNIFORM_HH 78 | -------------------------------------------------------------------------------- /include/hpp/core/diffusing-planner.hh: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2014 CNRS 3 | // Authors: Florent Lamiraux 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #ifndef HPP_CORE_DIFFUSING_PLANNER_HH 31 | #define HPP_CORE_DIFFUSING_PLANNER_HH 32 | 33 | #include 34 | 35 | namespace hpp { 36 | namespace core { 37 | /// \addtogroup path_planning 38 | /// \{ 39 | 40 | /// Generic implementation of RRT algorithm 41 | class HPP_CORE_DLLAPI DiffusingPlanner : public PathPlanner { 42 | public: 43 | typedef PathPlanner Parent_t; 44 | /// Return shared pointer to new object. 45 | static DiffusingPlannerPtr_t createWithRoadmap( 46 | const ProblemConstPtr_t& problem, const RoadmapPtr_t& roadmap); 47 | /// Return shared pointer to new object. 48 | static DiffusingPlannerPtr_t create(const ProblemConstPtr_t& problem); 49 | /// Initialize the problem resolution 50 | /// Call parent implementation and check that goal is defined as 51 | /// a set of configurations. 52 | virtual void startSolve(); 53 | /// One step of extension. 54 | virtual void oneStep(); 55 | /// Set configuration shooter. 56 | void configurationShooter(const ConfigurationShooterPtr_t& shooter); 57 | 58 | protected: 59 | /// Constructor 60 | DiffusingPlanner(const ProblemConstPtr_t& problem, 61 | const RoadmapPtr_t& roadmap); 62 | /// Constructor with roadmap 63 | DiffusingPlanner(const ProblemConstPtr_t& problem); 64 | /// Store weak pointer to itself 65 | void init(const DiffusingPlannerWkPtr_t& weak); 66 | /// Extend a node in the direction of a configuration 67 | /// \param near node in the roadmap, 68 | /// \param target target configuration 69 | virtual PathPtr_t extend(const NodePtr_t& near, ConfigurationIn_t target); 70 | 71 | private: 72 | ConfigurationShooterPtr_t configurationShooter_; 73 | mutable Configuration_t qProj_; 74 | DiffusingPlannerWkPtr_t weakPtr_; 75 | }; 76 | /// \} 77 | } // namespace core 78 | } // namespace hpp 79 | #endif // HPP_CORE_DIFFUSING_PLANNER_HH 80 | -------------------------------------------------------------------------------- /include/hpp/core/distance-between-objects.hh: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2014 CNRS 3 | // Authors: Florent Lamiraux 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #ifndef HPP_CORE_DISTANCE_BETWEEN_OBJECTS_HH 31 | #define HPP_CORE_DISTANCE_BETWEEN_OBJECTS_HH 32 | 33 | #include 34 | 35 | namespace hpp { 36 | namespace core { 37 | /// Computation of distances between pairs of objects 38 | class DistanceBetweenObjects { 39 | public: 40 | DistanceBetweenObjects(const DevicePtr_t& robot); 41 | /// Add an obstacle 42 | /// \param object obstacle to add 43 | /// Create distance computation pairs for each body of the robot 44 | void addObstacle(const CollisionObjectConstPtr_t& object); 45 | /// Add a list of obstacles 46 | void obstacles(const ObjectStdVector_t& obstacles); 47 | /// Compute distances between pairs of objects stored in bodies 48 | void computeDistances(); 49 | /// Get collision pairs 50 | const CollisionPairs_t& collisionPairs() const { return collisionPairs_; }; 51 | /// Get result of distance computations 52 | const DistanceResults_t& distanceResults() const { return distanceResults_; }; 53 | /// \} 54 | 55 | private: 56 | DevicePtr_t robot_; 57 | CollisionPairs_t collisionPairs_; 58 | DistanceResults_t distanceResults_; 59 | }; 60 | } // namespace core 61 | } // namespace hpp 62 | 63 | #endif // HPP_CORE_DISTANCE_BETWEEN_OBJECTS_HH 64 | -------------------------------------------------------------------------------- /include/hpp/core/distance.hh: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2014 CNRS 3 | // Authors: Florent Lamiraux 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #ifndef HPP_CORE_DISTANCE_HH 31 | #define HPP_CORE_DISTANCE_HH 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | namespace hpp { 40 | namespace core { 41 | /// \addtogroup steering_method 42 | /// \{ 43 | 44 | /// Abstract class for distance between configurations 45 | class HPP_CORE_DLLAPI Distance { 46 | public: 47 | value_type operator()(ConfigurationIn_t q1, ConfigurationIn_t q2) const { 48 | return impl_distance(q1, q2); 49 | } 50 | 51 | value_type operator()(NodePtr_t n1, NodePtr_t n2) const { 52 | return impl_distance(n1, n2); 53 | } 54 | 55 | value_type compute(ConfigurationIn_t q1, ConfigurationIn_t q2) const { 56 | return impl_distance(q1, q2); 57 | } 58 | 59 | value_type compute(NodePtr_t n1, NodePtr_t n2) const { 60 | return impl_distance(n1, n2); 61 | } 62 | 63 | virtual DistancePtr_t clone() const = 0; 64 | 65 | virtual ~Distance() {}; 66 | 67 | protected: 68 | Distance() {} 69 | /// Derived class should implement this function 70 | virtual value_type impl_distance(ConfigurationIn_t q1, 71 | ConfigurationIn_t q2) const = 0; 72 | virtual value_type impl_distance(NodePtr_t n1, NodePtr_t n2) const { 73 | return impl_distance(n1->configuration(), n2->configuration()); 74 | } 75 | 76 | HPP_SERIALIZABLE(); 77 | }; // class Distance 78 | /// \} 79 | } // namespace core 80 | } // namespace hpp 81 | #endif // HPP_CORE_DISTANCE_HH 82 | -------------------------------------------------------------------------------- /include/hpp/core/edge.hh: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2014 CNRS 3 | // Authors: Florent Lamiraux 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #ifndef HPP_CORE_EDGE_HH 31 | #define HPP_CORE_EDGE_HH 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | namespace hpp { 38 | namespace core { 39 | /// \addtogroup roadmap 40 | /// \{ 41 | 42 | /// Edge of a roadmap 43 | /// 44 | /// Links two nodes and stores a path linking the configurations stored in 45 | /// the nodes the edge links. 46 | class HPP_CORE_DLLAPI Edge { 47 | public: 48 | Edge(NodePtr_t n1, NodePtr_t n2, const PathPtr_t& path) 49 | : n1_(n1), n2_(n2), path_(path) {} 50 | NodePtr_t from() const { return n1_; } 51 | NodePtr_t to() const { return n2_; } 52 | PathPtr_t path() const { return path_; } 53 | 54 | protected: 55 | Edge() {} 56 | 57 | private: 58 | NodePtr_t n1_; 59 | NodePtr_t n2_; 60 | PathPtr_t path_; 61 | 62 | HPP_SERIALIZABLE(); 63 | }; // class Edge 64 | /// \} 65 | } // namespace core 66 | } // namespace hpp 67 | #endif // HPP_CORE_EDGE_HH 68 | -------------------------------------------------------------------------------- /include/hpp/core/kinodynamic-distance.hh: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2014 CNRS 3 | // Authors: Florent Lamiraux 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #ifndef HPP_CORE_KINODYNAMIC_DISTANCE_HH 31 | #define HPP_CORE_KINODYNAMIC_DISTANCE_HH 32 | 33 | #include 34 | 35 | namespace hpp { 36 | namespace core { 37 | /// \addtogroup steering_method 38 | /// \{ 39 | 40 | /// This class computed the Distance between two states as the minimal time 41 | /// required to connect this two states with a "bang-bang" trajectory, given 42 | /// velocity and acceleration bounds. 43 | /// 44 | /// This time is the same as the length() of a KinodynamicPath computed between 45 | /// this two states by the steeringMethod::Kinodynamic. 46 | /// 47 | /// This class require that the dimension of the extraConfigSpace is at least 6 48 | /// and store the velocity and acceleration of the root. 49 | /// 50 | 51 | class HPP_CORE_DLLAPI KinodynamicDistance : public Distance { 52 | public: 53 | static KinodynamicDistancePtr_t create(const DevicePtr_t& robot); 54 | static KinodynamicDistancePtr_t createFromProblem( 55 | const ProblemConstPtr_t& problem); 56 | 57 | static KinodynamicDistancePtr_t createCopy( 58 | const KinodynamicDistancePtr_t& distance); 59 | virtual DistancePtr_t clone() const; 60 | 61 | /// Get robot 62 | const DevicePtr_t& robot() const { return robot_; } 63 | 64 | protected: 65 | KinodynamicDistance(const DevicePtr_t& robot); 66 | KinodynamicDistance(const ProblemConstPtr_t& problem); 67 | KinodynamicDistance(const KinodynamicDistance& distance); 68 | void init(KinodynamicDistanceWkPtr_t self); 69 | /// Derived class should implement this function 70 | virtual value_type impl_distance(ConfigurationIn_t q1, 71 | ConfigurationIn_t q2) const; 72 | 73 | double computeMinTime(double p1, double p2, double v1, double v2) const; 74 | 75 | private: 76 | DevicePtr_t robot_; 77 | double aMax_; 78 | double vMax_; 79 | KinodynamicDistanceWkPtr_t weak_; 80 | }; // class KinodynamicDistance 81 | /// \} 82 | } // namespace core 83 | } // namespace hpp 84 | #endif // HPP_CORE_WEIGHED_DISTANCE_HH 85 | -------------------------------------------------------------------------------- /include/hpp/core/path-optimization/cost.hh: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2014 CNRS 3 | // Authors: Florent Lamiraux 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #ifndef HPP_CORE_PATH_OPTIMIZATION_COST_HH 31 | #define HPP_CORE_PATH_OPTIMIZATION_COST_HH 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | namespace hpp { 38 | namespace core { 39 | namespace pathOptimization { 40 | /// numerical cost for path optimization 41 | /// 42 | /// Provides an initial guess for the Hessian to initialize quasi-Newton 43 | /// methods. 44 | class HPP_CORE_DLLAPI Cost : public DifferentiableFunction { 45 | public: 46 | /// Return an approximation of the Hessian at minimum 47 | /// \retval hessian Hessian matrix of right size 48 | virtual void hessian(matrixOut_t hessian) const = 0; 49 | 50 | protected: 51 | Cost(size_type inputSize, size_type inputDerivativeSize, 52 | const std::string& name) 53 | : DifferentiableFunction(inputSize, inputDerivativeSize, 54 | LiegroupSpace::R1(), name) {} 55 | }; // Cost 56 | } // namespace pathOptimization 57 | } // namespace core 58 | } // namespace hpp 59 | #endif // HPP_CORE_PATH_OPTIMIZATION_COST_HH 60 | -------------------------------------------------------------------------------- /include/hpp/core/path-optimization/random-shortcut.hh: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2014 CNRS 3 | // Authors: Florent Lamiraux 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #ifndef HPP_CORE_PATH_OPTIMIZATION_RANDOM_SHORTCUT_HH 31 | #define HPP_CORE_PATH_OPTIMIZATION_RANDOM_SHORTCUT_HH 32 | 33 | #include 34 | 35 | namespace hpp { 36 | namespace core { 37 | namespace pathOptimization { 38 | /// \addtogroup path_optimization 39 | /// \{ 40 | 41 | /// Random shortcut 42 | /// 43 | /// Path optimizer that iteratively samples random configurations along a 44 | /// path and that tries to connect these configurations by a call to 45 | /// the steering method. 46 | /// 47 | /// \note The optimizer assumes that the input path is a vector of optimal 48 | /// paths for the distance function. 49 | class HPP_CORE_DLLAPI RandomShortcut : public PathOptimizer { 50 | public: 51 | /// Return shared pointer to new object. 52 | static RandomShortcutPtr_t create(const ProblemConstPtr_t& problem); 53 | 54 | /// Optimize path 55 | virtual PathVectorPtr_t optimize(const PathVectorPtr_t& path); 56 | 57 | protected: 58 | RandomShortcut(const ProblemConstPtr_t& problem); 59 | 60 | /// Sample times along currentOpt 61 | /// \param currentOpt the current path 62 | /// \param[in] t0, t3 are the start and end of currentOpt 63 | /// \param[out] t1, t2  are the position where to apply shortcut. 64 | /// \warning The function should output t1 < t2 65 | /// \return true in case of success 66 | virtual bool shootTimes(const PathVectorPtr_t& currentOpt, 67 | const value_type& t0, value_type& t1, value_type& t2, 68 | const value_type& t3); 69 | }; // class RandomShortcut 70 | /// \} 71 | } // namespace pathOptimization 72 | } // namespace core 73 | } // namespace hpp 74 | #endif // HPP_CORE_PATH_OPTIMIZATION_RANDOM_SHORTCUT_HH 75 | -------------------------------------------------------------------------------- /include/hpp/core/path-optimization/rs-time-parameterization.hh: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2019 - 2024 CNRS 3 | // 4 | // Author: Florent Lamiraux 5 | // 6 | 7 | // Redistribution and use in source and binary forms, with or without 8 | // modification, are permitted provided that the following conditions are 9 | // met: 10 | // 11 | // 1. Redistributions of source code must retain the above copyright 12 | // notice, this list of conditions and the following disclaimer. 13 | // 14 | // 2. Redistributions in binary form must reproduce the above copyright 15 | // notice, this list of conditions and the following disclaimer in the 16 | // documentation and/or other materials provided with the distribution. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 19 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 20 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 21 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 22 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 23 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 24 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 25 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 26 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 29 | // DAMAGE. 30 | 31 | #ifndef HPP_CORE_PATH_OPTIMIZATION_RS_TIME_PARAMETERIZATION_HH 32 | #define HPP_CORE_PATH_OPTIMIZATION_RS_TIME_PARAMETERIZATION_HH 33 | 34 | #include 35 | #include 36 | 37 | namespace hpp { 38 | namespace core { 39 | namespace pathOptimization { 40 | /// Time parameterization of Reeds and Shepp paths 41 | /// 42 | /// This class computes a time parameterization for a concatentation 43 | /// of Reeds and Shepp paths (in fact of constant curvature paths) 44 | class HPP_CORE_DLLAPI RSTimeParameterization : public PathOptimizer { 45 | public: 46 | /// Create instance and return shared pointer 47 | static RSTimeParameterizationPtr_t create(const ProblemConstPtr_t& problem); 48 | /// Compute the time parameterization of a path 49 | /// 50 | /// \param path input path, 51 | /// \precond path should be a concatenation of 52 | /// hpp::core::ConstantCurvature instances 53 | /// 54 | /// The parameterized path starts and ends with velocity equal to 0. 55 | /// On each segment, it accelerates, moves at maximal speed and 56 | /// decelerates to the minimal velocity to pass curvature discontinuity. 57 | virtual PathVectorPtr_t optimize(const PathVectorPtr_t& path); 58 | 59 | protected: 60 | /// Constructor 61 | /// \param minLinVel, maxLinVel minimal and maximal velocity. 62 | /// Minimal velocity is the linear velocity allowed when 63 | /// switching discontinuously the radius of curvature, 64 | /// \param maxAngVel maximal angular velocity, 65 | /// \param linearAcceletion linearDeceleration linear acceleration and 66 | /// deceleration that can be different. 67 | RSTimeParameterization(const ProblemConstPtr_t& problem) 68 | : PathOptimizer(problem) {} 69 | }; // class ReedsShepp 70 | 71 | } // namespace pathOptimization 72 | } // namespace core 73 | } // namespace hpp 74 | #endif // HPP_CORE_PATH_OPTIMIZATION_RS_TIME_PARAMETERIZATION_HH 75 | -------------------------------------------------------------------------------- /include/hpp/core/path-optimization/simple-shortcut.hh: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2019 CNRS 3 | // Authors: Florent Lamiraux 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #ifndef HPP_CORE_PATH_OPTIMIZATION_SIMPLE_SHORTCUT_HH 31 | #define HPP_CORE_PATH_OPTIMIZATION_SIMPLE_SHORTCUT_HH 32 | 33 | #include 34 | 35 | namespace hpp { 36 | namespace core { 37 | namespace pathOptimization { 38 | /// \addtogroup path_optimization 39 | /// \{ 40 | /// Simple shortcut 41 | /// 42 | /// Find shortest path composed of direct paths between all pairs of 43 | /// waypoints of input path. 44 | /// 45 | /// To do so, the optimizer builds a roadmap the nodes of which are the 46 | /// input path waypoints and the edges of which are the collision-free 47 | /// output of the steering method between all pairs of nodes. 48 | class HPP_CORE_DLLAPI SimpleShortcut : public PathOptimizer { 49 | public: 50 | /// Return shared pointer to new object. 51 | static SimpleShortcutPtr_t create(const ProblemConstPtr_t& problem); 52 | 53 | /// Optimize path 54 | virtual PathVectorPtr_t optimize(const PathVectorPtr_t& path); 55 | 56 | protected: 57 | SimpleShortcut(const ProblemConstPtr_t& problem); 58 | }; // class SimpleShortcut 59 | /// \} 60 | } // namespace pathOptimization 61 | } // namespace core 62 | } // namespace hpp 63 | #endif // HPP_CORE_PATH_OPTIMIZATION_SIMPLE_SHORTCUT_HH 64 | -------------------------------------------------------------------------------- /include/hpp/core/path-planning-failed.hh: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 CNRS 2 | // Authors: Florent Lamiraux 3 | // 4 | 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above copyright 13 | // notice, this list of conditions and the following disclaimer in the 14 | // documentation and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 17 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 18 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 19 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 20 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 21 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 22 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 23 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 24 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 26 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 27 | // DAMAGE. 28 | 29 | #ifndef HPP_CORE_PATH_PLANNING_FAILED_HH 30 | #define HPP_CORE_PATH_PLANNING_FAILED_HH 31 | 32 | #include 33 | 34 | namespace hpp { 35 | namespace core { 36 | struct HPP_CORE_DLLAPI path_planning_failed : public std::exception { 37 | path_planning_failed() : msg_() {} 38 | 39 | path_planning_failed(const std::string& msg) : msg_(msg) {} 40 | 41 | path_planning_failed(const path_planning_failed& other) 42 | : std::exception(other), msg_(other.msg_) {} 43 | 44 | virtual ~path_planning_failed() noexcept {}; 45 | virtual const char* what() const noexcept { return msg_.c_str(); }; 46 | 47 | std::string msg_; 48 | }; 49 | 50 | } // namespace core 51 | } // namespace hpp 52 | 53 | #endif // HPP_CORE_PATH_PLANNING_FAILED_HH 54 | -------------------------------------------------------------------------------- /include/hpp/core/path-projector.hh: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2014, LAAS-CNRS 2 | // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) 3 | // 4 | 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above copyright 13 | // notice, this list of conditions and the following disclaimer in the 14 | // documentation and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 17 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 18 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 19 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 20 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 21 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 22 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 23 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 24 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 26 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 27 | // DAMAGE. 28 | 29 | #ifndef HPP_CORE_PATHPROJECTOR_HH 30 | #define HPP_CORE_PATHPROJECTOR_HH 31 | 32 | #include 33 | #include 34 | 35 | namespace hpp { 36 | namespace core { 37 | /// This class projects a path using constraints. 38 | class HPP_CORE_DLLAPI PathProjector { 39 | public: 40 | typedef hpp::core::Path Path; 41 | typedef hpp::core::PathPtr_t PathPtr_t; 42 | typedef hpp::core::PathVector PathVector; 43 | typedef hpp::core::PathVectorPtr_t PathVectorPtr_t; 44 | 45 | /// Destructor 46 | virtual ~PathProjector(); 47 | 48 | /// Apply the constraints to the path. 49 | /// \param[in] the input path, 50 | /// \param[out] the output path. 51 | /// \return True if projection succeded 52 | bool apply(const PathPtr_t& path, PathPtr_t& projection) const; 53 | 54 | protected: 55 | /// Constructor 56 | /// 57 | /// distance distance metric between configurations 58 | /// steeringMethod steering method used to build new paths. The instance 59 | /// is copied. 60 | /// copySteeringMethodConstraints whether to keep the constraints of the 61 | /// input steering method. 62 | PathProjector(const DistancePtr_t& distance, 63 | const SteeringMethodPtr_t& steeringMethod, 64 | bool keepSteeringMethodConstraints = false); 65 | 66 | /// Method to be reimplemented by inherited class. 67 | virtual bool impl_apply(const PathPtr_t& path, 68 | PathPtr_t& projection) const = 0; 69 | 70 | value_type d(ConfigurationIn_t q1, ConfigurationIn_t q2) const; 71 | PathPtr_t steer(ConfigurationIn_t q1, ConfigurationIn_t q2) const; 72 | SteeringMethodPtr_t steeringMethod_; 73 | 74 | private: 75 | DistancePtr_t distance_; 76 | }; 77 | } // namespace core 78 | } // namespace hpp 79 | 80 | #endif // HPP_CORE_PATHPROJECTOR_HH 81 | -------------------------------------------------------------------------------- /include/hpp/core/path-projector/dichotomy.hh: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2014, LAAS-CNRS 2 | // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) 3 | // 4 | 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above copyright 13 | // notice, this list of conditions and the following disclaimer in the 14 | // documentation and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 17 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 18 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 19 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 20 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 21 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 22 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 23 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 24 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 26 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 27 | // DAMAGE. 28 | 29 | #ifndef HPP_CORE_PATHPROJECTOR_DICHOTOMY_HH 30 | #define HPP_CORE_PATHPROJECTOR_DICHOTOMY_HH 31 | 32 | #include 33 | 34 | #include "hpp/core/problem.hh" 35 | 36 | namespace hpp { 37 | namespace core { 38 | namespace pathProjector { 39 | class HPP_CORE_DLLAPI Dichotomy : public PathProjector { 40 | public: 41 | typedef hpp::core::StraightPath StraightPath; 42 | typedef hpp::core::StraightPathPtr_t StraightPathPtr_t; 43 | static DichotomyPtr_t create(const DistancePtr_t& distance, 44 | const SteeringMethodPtr_t& steeringMethod, 45 | value_type maxPathLength) { 46 | return DichotomyPtr_t( 47 | new Dichotomy(distance, steeringMethod, maxPathLength)); 48 | } 49 | 50 | static DichotomyPtr_t create(const ProblemConstPtr_t& problem, 51 | value_type maxPathLength) { 52 | return create(problem->distance(), problem->steeringMethod(), 53 | maxPathLength); 54 | } 55 | 56 | protected: 57 | bool impl_apply(const PathPtr_t& path, PathPtr_t& projection) const; 58 | 59 | Dichotomy(const DistancePtr_t& distance, 60 | const SteeringMethodPtr_t& steeringMethod, 61 | value_type maxPathLength); 62 | 63 | bool applyToStraightPath(const StraightPathPtr_t& path, 64 | PathPtr_t& projection) const; 65 | 66 | private: 67 | value_type maxPathLength_; 68 | }; 69 | } // namespace pathProjector 70 | } // namespace core 71 | } // namespace hpp 72 | #endif // HPP_CORE_PATHPROJECTOR_DICHOTOMY_HH 73 | -------------------------------------------------------------------------------- /include/hpp/core/path-projector/progressive.hh: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2014, LAAS-CNRS 2 | // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) 3 | // 4 | 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above copyright 13 | // notice, this list of conditions and the following disclaimer in the 14 | // documentation and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 17 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 18 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 19 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 20 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 21 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 22 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 23 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 24 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 26 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 27 | // DAMAGE. 28 | 29 | #ifndef HPP_CORE_PATHPROJECTOR_PROGRESSIVE_HH 30 | #define HPP_CORE_PATHPROJECTOR_PROGRESSIVE_HH 31 | 32 | #include 33 | 34 | namespace hpp { 35 | namespace core { 36 | namespace pathProjector { 37 | class HPP_CORE_DLLAPI Progressive : public PathProjector { 38 | public: 39 | typedef hpp::core::StraightPath StraightPath; 40 | typedef hpp::core::StraightPathPtr_t StraightPathPtr_t; 41 | 42 | /// \todo See todo of pathProjector::Global::create 43 | static ProgressivePtr_t create(const DistancePtr_t& distance, 44 | const SteeringMethodPtr_t& steeringMethod, 45 | value_type step); 46 | 47 | static ProgressivePtr_t create(const ProblemConstPtr_t& problem, 48 | const value_type& step); 49 | 50 | protected: 51 | bool impl_apply(const PathPtr_t& path, PathPtr_t& projection) const; 52 | 53 | Progressive(const DistancePtr_t& distance, 54 | const SteeringMethodPtr_t& steeringMethod, value_type step, 55 | value_type threshold, value_type hessianBound); 56 | 57 | bool project(const PathPtr_t& path, PathPtr_t& proj) const; 58 | 59 | private: 60 | value_type step_; 61 | const value_type thresholdMin_; 62 | const value_type hessianBound_; 63 | const bool withHessianBound_; 64 | }; 65 | } // namespace pathProjector 66 | } // namespace core 67 | } // namespace hpp 68 | 69 | #endif // HPP_CORE_PATHPROJECTOR_PROGRESSIVE_HH 70 | -------------------------------------------------------------------------------- /include/hpp/core/path-projector/recursive-hermite.hh: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016, LAAS-CNRS 2 | // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) 3 | // 4 | 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above copyright 13 | // notice, this list of conditions and the following disclaimer in the 14 | // documentation and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 17 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 18 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 19 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 20 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 21 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 22 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 23 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 24 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 26 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 27 | // DAMAGE. 28 | 29 | #ifndef HPP_CORE_PATHPROJECTOR_RECURSIVE_HERMITE_HH 30 | #define HPP_CORE_PATHPROJECTOR_RECURSIVE_HERMITE_HH 31 | 32 | #include "hpp/core/config.hh" 33 | #include "hpp/core/fwd.hh" 34 | #include "hpp/core/path-projector.hh" 35 | 36 | namespace hpp { 37 | namespace core { 38 | namespace pathProjector { 39 | /// Implements 40 | /// "Fast Interpolation and Time-Optimization on Implicit Contact Submanifolds" 41 | /// from Kris Hauser. 42 | class HPP_CORE_DLLAPI RecursiveHermite : public PathProjector { 43 | public: 44 | typedef hpp::core::path::Hermite Hermite; 45 | typedef hpp::core::path::HermitePtr_t HermitePtr_t; 46 | 47 | static RecursiveHermitePtr_t create(const DistancePtr_t& distance, 48 | const SteeringMethodPtr_t& steeringMethod, 49 | value_type step); 50 | 51 | static RecursiveHermitePtr_t create(const ProblemConstPtr_t& problem, 52 | const value_type& step); 53 | 54 | protected: 55 | bool impl_apply(const PathPtr_t& path, PathPtr_t& projection) const; 56 | 57 | RecursiveHermite(const DistancePtr_t& distance, 58 | const SteeringMethodPtr_t& steeringMethod, 59 | const value_type& M, const value_type& beta); 60 | 61 | bool project(const PathPtr_t& path, PathPtr_t& proj) const; 62 | 63 | private: 64 | bool recurse(const HermitePtr_t& path, PathVectorPtr_t& proj, 65 | const value_type& thr) const; 66 | value_type M_, beta_; 67 | }; 68 | } // namespace pathProjector 69 | } // namespace core 70 | } // namespace hpp 71 | 72 | #endif // HPP_CORE_PATHPROJECTOR_RECURSIVE_HERMITE_HH 73 | -------------------------------------------------------------------------------- /include/hpp/core/path-validation-report.hh: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2014 CNRS 3 | // Authors: Florent Lamiraux 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #ifndef HPP_CORE_PATH_VALIDATION_REPORT_HH 31 | #define HPP_CORE_PATH_VALIDATION_REPORT_HH 32 | 33 | #include 34 | #include 35 | 36 | namespace hpp { 37 | namespace core { 38 | /// \addtogroup validation 39 | /// \{ 40 | 41 | /// Abstraction of path validation report 42 | /// 43 | /// This class is aimed at being derived to store information relative to 44 | /// various PathValidation derived classes. 45 | struct HPP_CORE_DLLAPI PathValidationReport : public ValidationReport { 46 | PathValidationReport() : ValidationReport(), configurationReport() {} 47 | PathValidationReport(const value_type& param, 48 | const ValidationReportPtr_t& report) 49 | : parameter(param), configurationReport(report) {} 50 | 51 | virtual ~PathValidationReport() {}; 52 | 53 | virtual std::ostream& print(std::ostream& os) const { 54 | os << "Invalid configuration at parameter " << parameter << iendl 55 | << incindent; 56 | if (!configurationReport) 57 | os << "No ValidationReport"; 58 | else 59 | os << *configurationReport; 60 | return os << decindent; 61 | } 62 | 63 | value_type getParameter() { return parameter; } 64 | 65 | void setParameter(value_type p) { parameter = p; } 66 | 67 | /// Parameter of the path where a invalid configuration has been found 68 | value_type parameter; 69 | ValidationReportPtr_t configurationReport; 70 | }; // class PathValidationReport 71 | /// \} 72 | } // namespace core 73 | } // namespace hpp 74 | 75 | #endif // HPP_CORE_PATH_VALIDATION_REPORT_HH 76 | -------------------------------------------------------------------------------- /include/hpp/core/path-validation.hh: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2014 CNRS 3 | // Authors: Florent Lamiraux 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #ifndef HPP_CORE_PATH_VALIDATION_HH 31 | #define HPP_CORE_PATH_VALIDATION_HH 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | namespace hpp { 38 | namespace core { 39 | /// \addtogroup validation 40 | /// \{ 41 | 42 | /// Abstraction of path validation 43 | /// 44 | /// Instances of this class compute the latest valid configuration along 45 | /// a path. 46 | /// 47 | /// Method \code validate(ConfigurationIn_t q) \endcode is provided to 48 | /// validate single configurations. It is particularly useful to test 49 | /// the initial and goal configurations of a path planning problem using 50 | /// this path validation. 51 | class HPP_CORE_DLLAPI PathValidation { 52 | public: 53 | /// Compute the largest valid interval starting from the path beginning 54 | /// 55 | /// \param path the path to check for validity, 56 | /// \param reverse if true check from the end, 57 | /// \retval the extracted valid part of the path, pointer to path if 58 | /// path is valid. 59 | /// \retval report information about the validation process. A report 60 | /// is allocated if the path is not valid. 61 | /// \return whether the whole path is valid. 62 | virtual bool validate(const PathPtr_t& path, bool reverse, 63 | PathPtr_t& validPart, 64 | PathValidationReportPtr_t& report) = 0; 65 | 66 | /// Validate a single configuration 67 | /// \param q input configuration, 68 | /// \retval report validation report. 69 | /// The default implementation builds a straight path of length 0 70 | /// with the input configuration and validates the path. 71 | virtual bool validate(ConfigurationIn_t q, ValidationReportPtr_t& report); 72 | virtual ~PathValidation() {}; 73 | 74 | protected: 75 | PathValidation() {} 76 | }; // class PathValidation 77 | /// \} 78 | } // namespace core 79 | } // namespace hpp 80 | 81 | #endif // HPP_CORE_PATH_VALIDATION_HH 82 | -------------------------------------------------------------------------------- /include/hpp/core/path-validation/discretized-collision-checking.hh: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2018 CNRS 3 | // Authors: Florent Lamiraux, Joseph Mirabel 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #ifndef HPP_CORE_PATH_VALIDATION_DISCRETIZED_COLLISION_CHECKING_HH 31 | #define HPP_CORE_PATH_VALIDATION_DISCRETIZED_COLLISION_CHECKING_HH 32 | 33 | #include 34 | 35 | namespace hpp { 36 | namespace core { 37 | namespace pathValidation { 38 | /// \addtogroup validation 39 | /// \{ 40 | 41 | /// Validation of path by collision checking at discretized parameter values 42 | DiscretizedPtr_t createDiscretizedCollisionChecking(const DevicePtr_t& robot, 43 | const value_type& stepSize); 44 | 45 | /// \} 46 | } // namespace pathValidation 47 | } // namespace core 48 | } // namespace hpp 49 | 50 | #endif // HPP_CORE_PATH_VALIDATION_DISCRETIZED_COLLISION_CHECKING_HH 51 | -------------------------------------------------------------------------------- /include/hpp/core/path-validation/discretized-joint-bound.hh: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2018 CNRS 3 | // Authors: Florent Lamiraux, Joseph Mirabel 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #ifndef HPP_CORE_PATH_VALIDATION_DISCRETIZED_JOINT_BOUND_HH 31 | #define HPP_CORE_PATH_VALIDATION_DISCRETIZED_JOINT_BOUND_HH 32 | 33 | #include 34 | 35 | namespace hpp { 36 | namespace core { 37 | namespace pathValidation { 38 | /// \addtogroup validation 39 | /// \{ 40 | 41 | /// Validation of path by checking joint bounds at discretized parameter values 42 | DiscretizedPtr_t createDiscretizedJointBound(const DevicePtr_t& robot, 43 | const value_type& stepSize); 44 | 45 | /// \} 46 | } // namespace pathValidation 47 | } // namespace core 48 | } // namespace hpp 49 | 50 | #endif // HPP_CORE_PATH_VALIDATION_DISCRETIZED_JOINT_BOUND_HH 51 | -------------------------------------------------------------------------------- /include/hpp/core/path-validations.hh: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2017 CNRS 3 | // Authors: Diane Bury 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #ifndef HPP_CORE_PATH_VALIDATIONS_HH 31 | #define HPP_CORE_PATH_VALIDATIONS_HH 32 | 33 | #include 34 | #include 35 | 36 | namespace hpp { 37 | namespace core { 38 | /// \addtogroup validation 39 | /// \{ 40 | 41 | /// Validation of a path with multiple path validation methods 42 | /// 43 | /// Apply several path validation methods to the path parameter 44 | class HPP_CORE_DLLAPI PathValidations 45 | : public PathValidation, 46 | public ObstacleUserVector { 47 | public: 48 | static PathValidationsPtr_t create(); 49 | 50 | /// Compute the largest valid interval starting from the path beginning 51 | /// 52 | /// \param path the path to check for validity, 53 | /// \param reverse if true check from the end, 54 | /// \retval the extracted valid part of the path, pointer to path if 55 | /// path is valid. 56 | /// \retval report information about the validation process. A report 57 | /// is allocated if the path is not valid. 58 | /// \return whether the whole path is valid. 59 | virtual bool validate(const PathPtr_t& path, bool reverse, 60 | PathPtr_t& validPart, 61 | PathValidationReportPtr_t& report); 62 | 63 | /// Validate a single configuration 64 | /// \param q input configuration, 65 | /// \retval report validation report. 66 | /// The default implementation builds a straight path of length 0 67 | /// with the input configuration and validates the path. 68 | virtual bool validate(ConfigurationIn_t q, ValidationReportPtr_t& report); 69 | 70 | /// Add a path validation object 71 | virtual void addPathValidation(const PathValidationPtr_t& pathValidation); 72 | 73 | virtual ~PathValidations() {}; 74 | 75 | protected: 76 | PathValidations(); 77 | }; // class PathValidations 78 | /// \} 79 | } // namespace core 80 | } // namespace hpp 81 | 82 | #endif // HPP_CORE_PATH_VALIDATIONS_HH 83 | -------------------------------------------------------------------------------- /include/hpp/core/path/math.hh: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2017, Joseph Mirabel 2 | // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) 3 | // 4 | 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above copyright 13 | // notice, this list of conditions and the following disclaimer in the 14 | // documentation and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 17 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 18 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 19 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 20 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 21 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 22 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 23 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 24 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 26 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 27 | // DAMAGE. 28 | 29 | #ifndef HPP_CORE_PATH_MATH_HH 30 | #define HPP_CORE_PATH_MATH_HH 31 | 32 | #include 33 | #include 34 | 35 | namespace hpp { 36 | namespace core { 37 | namespace path { 38 | template 39 | struct binomials { 40 | typedef Eigen::Matrix Factorials_t; 41 | 42 | static inline const Factorials_t& factorials() { 43 | static Factorials_t ret(Factorials_t::Zero()); 44 | if (ret(0) == 0) { 45 | ret(0) = 1; 46 | for (size_type i = 1; i < N; ++i) ret(i) = ret(i - 1) * i; 47 | } 48 | return ret; 49 | } 50 | 51 | static inline size_type binomial(size_type n, size_type k) { 52 | const Factorials_t& factors = factorials(); 53 | assert(n >= k && k >= 0); 54 | assert(n < N); 55 | size_type denom = factors(k) * factors(n - k); 56 | return factors(n) / denom; 57 | } 58 | }; 59 | } // namespace path 60 | } // namespace core 61 | } // namespace hpp 62 | 63 | #endif // HPP_CORE_PATH_MATH_HH 64 | -------------------------------------------------------------------------------- /include/hpp/core/plan-and-optimize.hh: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2014 CNRS 3 | // Authors: Florent Lamiraux 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #ifndef HPP_CORE_PLAN_AND_OPTIMIZE_HH 31 | #define HPP_CORE_PLAN_AND_OPTIMIZE_HH 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | namespace hpp { 38 | namespace core { 39 | /// \addtogroup path_planning 40 | /// \{ 41 | 42 | /// Path planner and optimizer 43 | /// 44 | /// Plans a path and iteratively applies a series of optimizer on 45 | /// the result. 46 | class HPP_CORE_DLLAPI PlanAndOptimize : public PathPlanner { 47 | public: 48 | /// Return shared pointer to new object. 49 | static PlanAndOptimizePtr_t create(const PathPlannerPtr_t& pathPlanner); 50 | /// Call internal path planner implementation 51 | virtual void startSolve(); 52 | /// One iteration of path planning or path optimization 53 | virtual void oneStep(); 54 | /// Optimize planned path 55 | virtual PathVectorPtr_t finishSolve(const PathVectorPtr_t& path); 56 | void addPathOptimizer(const PathOptimizerPtr_t& optimizer); 57 | 58 | protected: 59 | PlanAndOptimize(const PathPlannerPtr_t& pathPlanner); 60 | 61 | private: 62 | typedef std::vector Optimizers_t; 63 | const PathPlannerPtr_t pathPlanner_; 64 | Optimizers_t optimizers_; 65 | }; // class PlanAndOptimize 66 | /// \} 67 | } // namespace core 68 | } // namespace hpp 69 | 70 | #endif // HPP_CORE_PLAN_AND_OPTIMIZE_HH 71 | -------------------------------------------------------------------------------- /include/hpp/core/problem-target.hh: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2016 CNRS 3 | // Authors: Joseph Mirabel 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #ifndef HPP_CORE_PROBLEM_TARGET_HH 31 | #define HPP_CORE_PROBLEM_TARGET_HH 32 | 33 | #include 34 | #include 35 | 36 | namespace hpp { 37 | namespace core { 38 | /// \addtogroup path_planning 39 | /// \{ 40 | 41 | /// Problem target 42 | /// 43 | /// This abstract class defines the goal to be reached by a planning 44 | /// algorithm. 45 | class HPP_CORE_DLLAPI ProblemTarget { 46 | public: 47 | virtual ~ProblemTarget() {}; 48 | 49 | /// Check if the problem target is well specified. 50 | virtual void check(const RoadmapPtr_t& roadmap) const = 0; 51 | 52 | /// Check whether the problem is solved. 53 | virtual bool reached(const RoadmapPtr_t& roadmap) const = 0; 54 | 55 | /// Returns the solution path found. 56 | /// Should be called when reached() returns true. 57 | virtual PathVectorPtr_t computePath(const RoadmapPtr_t& roadmap) const = 0; 58 | 59 | /// Set the problem 60 | void problem(const ProblemPtr_t& problem) { problem_ = problem; } 61 | 62 | protected: 63 | /// Constructor 64 | ProblemTarget(const ProblemPtr_t& problem) : problem_(problem) {} 65 | 66 | /// Store weak pointer to itself 67 | void init(const ProblemTargetWkPtr_t& weak) { weakPtr_ = weak; } 68 | 69 | /// Reference to the planner for access to problem and roadmap 70 | ProblemWkPtr_t problem_; 71 | 72 | /// Store weak pointer to itself 73 | ProblemTargetWkPtr_t weakPtr_; 74 | 75 | private: 76 | }; // class ProblemTarget 77 | /// \} 78 | } // namespace core 79 | } // namespace hpp 80 | #endif // HPP_CORE_PROBLEM_TARGET_HH 81 | -------------------------------------------------------------------------------- /include/hpp/core/problem-target/goal-configurations.hh: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2016 CNRS 3 | // Authors: Joseph Mirabel 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #ifndef HPP_CORE_PROBLEM_TARGET_GOAL_CONFIGURATIONS_HH 31 | #define HPP_CORE_PROBLEM_TARGET_GOAL_CONFIGURATIONS_HH 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | namespace hpp { 38 | namespace core { 39 | namespace problemTarget { 40 | /// \addtogroup path_planning 41 | /// \{ 42 | 43 | /// Goal configurations 44 | /// 45 | /// This class defines a goal as a list of goal configurations. 46 | class HPP_CORE_DLLAPI GoalConfigurations : public ProblemTarget { 47 | public: 48 | static GoalConfigurationsPtr_t create(const ProblemPtr_t& problem); 49 | 50 | /// Check if the problem target is well specified. 51 | void check(const RoadmapPtr_t& roadmap) const; 52 | 53 | /// Check if the initial configuration and one of the goal are 54 | /// in the same connected component. 55 | /// \note This test takes into account nodes of the roadmap that 56 | /// are tagged as goal nodes, not the configurations stored 57 | /// in this class. Be careful that those two sets are the 58 | /// same. 59 | bool reached(const RoadmapPtr_t& roadmap) const; 60 | 61 | PathVectorPtr_t computePath(const RoadmapPtr_t& roadmap) const; 62 | /// Get goal configurations. 63 | const Configurations_t& configurations() const; 64 | /// Add goal configuration. 65 | void addConfiguration(ConfigurationIn_t config); 66 | /// Reset the set of goal configurations 67 | void resetConfigurations(); 68 | 69 | protected: 70 | /// Constructor 71 | GoalConfigurations(const ProblemPtr_t& problem) : ProblemTarget(problem) {} 72 | 73 | private: 74 | /// Shared pointer to goal configuration. 75 | Configurations_t configurations_; 76 | 77 | }; // class GoalConfigurations 78 | /// \} 79 | } // namespace problemTarget 80 | } // namespace core 81 | } // namespace hpp 82 | #endif // HPP_CORE_PROBLEM_TARGET_GOAL_CONFIGURATIONS_HH 83 | -------------------------------------------------------------------------------- /include/hpp/core/problem-target/task-target.hh: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2016 CNRS 3 | // Authors: Joseph Mirabel 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #ifndef HPP_CORE_PROBLEM_TARGET_TASK_TARGET_HH 31 | #define HPP_CORE_PROBLEM_TARGET_TASK_TARGET_HH 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | namespace hpp { 38 | namespace core { 39 | namespace problemTarget { 40 | /// \addtogroup path_planning 41 | /// \{ 42 | 43 | /// Task target 44 | /// 45 | /// This class defines a goal using constraints. The set of goal 46 | /// configurations is a submanifold of the full configuration space. 47 | /// \warning So far, this feature is not taken into account by 48 | /// most planners. The supported planners are: 49 | /// - DiffusingPlanner 50 | class HPP_CORE_DLLAPI TaskTarget : public ProblemTarget { 51 | public: 52 | static TaskTargetPtr_t create(const ProblemPtr_t& problem); 53 | 54 | /// Check if the problem target is well specified. 55 | void check(const RoadmapPtr_t& roadmap) const; 56 | 57 | /// Check whether the problem is solved. 58 | bool reached(const RoadmapPtr_t& roadmap) const; 59 | 60 | PathVectorPtr_t computePath(const RoadmapPtr_t& roadmap) const; 61 | 62 | void constraints(const ConstraintSetPtr_t& c) { constraints_ = c; } 63 | 64 | /// Return the vector of numerical constraints that define the goal. 65 | NumericalConstraints_t constraints() const; 66 | 67 | protected: 68 | /// Constructor 69 | TaskTarget(const ProblemPtr_t& problem) : ProblemTarget(problem) {} 70 | 71 | private: 72 | ConstraintSetPtr_t constraints_; 73 | }; // class TaskTarget 74 | /// \} 75 | } // namespace problemTarget 76 | } // namespace core 77 | } // namespace hpp 78 | #endif // HPP_CORE_PROBLEM_TARGET_TASK_TARGET_HH 79 | -------------------------------------------------------------------------------- /include/hpp/core/projection-error.hh: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2015 CNRS 2 | // Authors: Joseph Mirabel 3 | // 4 | 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above copyright 13 | // notice, this list of conditions and the following disclaimer in the 14 | // documentation and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 17 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 18 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 19 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 20 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 21 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 22 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 23 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 24 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 26 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 27 | // DAMAGE. 28 | 29 | #ifndef HPP_CORE_PROJECTION_ERROR_HH 30 | #define HPP_CORE_PROJECTION_ERROR_HH 31 | 32 | #include 33 | #include 34 | 35 | namespace hpp { 36 | namespace core { 37 | struct HPP_CORE_DLLAPI projection_error : public std::exception { 38 | projection_error() : msg_() {} 39 | 40 | projection_error(const std::string& msg) : msg_(msg) {} 41 | 42 | projection_error(const projection_error& other) 43 | : std::exception(other), msg_(other.msg_) {} 44 | 45 | virtual ~projection_error() noexcept {}; 46 | virtual const char* what() const noexcept { return msg_.c_str(); }; 47 | 48 | std::string msg_; 49 | }; 50 | 51 | /// \addtogroup validation 52 | /// \{ 53 | 54 | /// Handles projection errors when evaluating a path. 55 | struct HPP_CORE_DLLAPI ProjectionError : public ValidationReport { 56 | ProjectionError(const std::string& msg = "") : ValidationReport(), msg(msg) {} 57 | 58 | virtual ~ProjectionError() {}; 59 | 60 | virtual std::ostream& print(std::ostream& os) const { 61 | os << "Projection error"; 62 | if (!msg.empty()) os << " (" << msg << ')'; 63 | return os; 64 | } 65 | 66 | std::string msg; 67 | }; // class ProjectionError 68 | /// \} 69 | } // namespace core 70 | } // namespace hpp 71 | 72 | #endif // HPP_CORE_PROJECTION_ERROR_HH 73 | -------------------------------------------------------------------------------- /include/hpp/core/steering-method/car-like.hh: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2017 CNRS 3 | // Authors: Florent Lamiraux 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #ifndef HPP_CORE_STEERING_METHOD_CAR_LIKE_HH 31 | #define HPP_CORE_STEERING_METHOD_CAR_LIKE_HH 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | namespace hpp { 41 | namespace core { 42 | namespace steeringMethod { 43 | /// \addtogroup steering_method 44 | /// \{ 45 | 46 | /// Abstract class that implements various type of trajectories for 47 | /// carlike vehicles 48 | /// 49 | class HPP_CORE_DLLAPI CarLike : public SteeringMethod { 50 | public: 51 | virtual ~CarLike() {} 52 | 53 | /// Set the wheels 54 | void setWheelJoints(const std::vector wheels) { 55 | wheels_ = wheels; 56 | } 57 | 58 | void turningRadius(const value_type& rho); 59 | 60 | inline value_type turningRadius() const { return rho_; } 61 | 62 | protected: 63 | /// Constructor 64 | CarLike(const ProblemConstPtr_t& problem); 65 | 66 | /// Constructor 67 | CarLike(const ProblemConstPtr_t& problem, const value_type turningRadius, 68 | JointPtr_t xyJoint, JointPtr_t rzJoint, 69 | std::vector wheels); 70 | 71 | /// Copy constructor 72 | CarLike(const CarLike& other); 73 | 74 | /// Store weak pointer to itself 75 | void init(CarLikeWkPtr_t weak) { 76 | core::SteeringMethod::init(weak); 77 | weak_ = weak; 78 | } 79 | 80 | DeviceWkPtr_t device_; 81 | /// Turning radius 82 | value_type rho_; 83 | JointPtr_t xy_, rz_; 84 | size_type xyId_, rzId_; 85 | std::vector wheels_; 86 | 87 | private: 88 | CarLikeWkPtr_t weak_; 89 | }; // CarLike 90 | std::vector getWheelsFromParameter(const ProblemConstPtr_t& problem, 91 | const JointPtr_t& rz); 92 | 93 | /// \} 94 | } // namespace steeringMethod 95 | } // namespace core 96 | } // namespace hpp 97 | #endif // HPP_CORE_STEERING_METHOD_CAR_LIKE_HH 98 | -------------------------------------------------------------------------------- /include/hpp/core/steering-method/fwd.hh: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2017 CNRS 3 | // Authors: Joseph Mirabel 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #ifndef HPP_CORE_STEERING_METHOD_FWD_HH 31 | #define HPP_CORE_STEERING_METHOD_FWD_HH 32 | 33 | namespace hpp { 34 | namespace core { 35 | namespace steeringMethod { 36 | HPP_PREDEF_CLASS(Straight); 37 | typedef shared_ptr StraightPtr_t; 38 | HPP_PREDEF_CLASS(Interpolated); 39 | typedef shared_ptr InterpolatedPtr_t; 40 | HPP_PREDEF_CLASS(CarLike); 41 | typedef shared_ptr CarLikePtr_t; 42 | HPP_PREDEF_CLASS(ConstantCurvature); 43 | typedef shared_ptr ConstantCurvaturePtr_t; 44 | HPP_PREDEF_CLASS(Dubins); 45 | typedef shared_ptr DubinsPtr_t; 46 | HPP_PREDEF_CLASS(ReedsShepp); 47 | typedef shared_ptr ReedsSheppPtr_t; 48 | HPP_PREDEF_CLASS(Snibud); 49 | typedef shared_ptr SnibudPtr_t; 50 | template 51 | class Spline; 52 | HPP_PREDEF_CLASS(Hermite); 53 | typedef shared_ptr HermitePtr_t; 54 | } // namespace steeringMethod 55 | } // namespace core 56 | } // namespace hpp 57 | 58 | #endif // HPP_CORE_STEERING_METHOD_FWD_HH 59 | -------------------------------------------------------------------------------- /include/hpp/core/steering-method/straight.hh: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2014 CNRS 3 | // Authors: Florent Lamiraux 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #ifndef HPP_CORE_STEERING_METHOD_STRAIGHT_HH 31 | #define HPP_CORE_STEERING_METHOD_STRAIGHT_HH 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | namespace hpp { 39 | namespace core { 40 | namespace steeringMethod { 41 | /// \addtogroup steering_method 42 | /// \{ 43 | 44 | /// Steering method that creates StraightPath instances 45 | /// 46 | class HPP_CORE_DLLAPI Straight : public SteeringMethod { 47 | public: 48 | /// Create instance and return shared pointer 49 | static StraightPtr_t create(const ProblemConstPtr_t& problem) { 50 | Straight* ptr = new Straight(problem); 51 | StraightPtr_t shPtr(ptr); 52 | ptr->init(shPtr); 53 | return shPtr; 54 | } 55 | /// Copy instance and return shared pointer 56 | static StraightPtr_t createCopy(const StraightPtr_t& other) { 57 | Straight* ptr = new Straight(*other); 58 | StraightPtr_t shPtr(ptr); 59 | ptr->init(shPtr); 60 | return shPtr; 61 | } 62 | /// Copy instance and return shared pointer 63 | virtual SteeringMethodPtr_t copy() const { return createCopy(weak_.lock()); } 64 | 65 | /// create a path between two configurations 66 | virtual PathPtr_t impl_compute(ConfigurationIn_t q1, 67 | ConfigurationIn_t q2) const; 68 | 69 | protected: 70 | /// Constructor with robot 71 | /// Weighed distance is created from robot 72 | Straight(const ProblemConstPtr_t& problem) 73 | : SteeringMethod(problem), weak_() {} 74 | /// Copy constructor 75 | Straight(const Straight& other) : SteeringMethod(other), weak_() {} 76 | 77 | /// Store weak pointer to itself 78 | void init(StraightWkPtr_t weak) { 79 | SteeringMethod::init(weak); 80 | weak_ = weak; 81 | } 82 | 83 | private: 84 | StraightWkPtr_t weak_; 85 | }; // Straight 86 | } // namespace steeringMethod 87 | /// \} 88 | } // namespace core 89 | } // namespace hpp 90 | #endif // HPP_CORE_STEERING_METHOD_STRAIGHT_HH 91 | -------------------------------------------------------------------------------- /include/hpp/core/time-parameterization.hh: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2017, Joseph Mirabel 2 | // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) 3 | // 4 | 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above copyright 13 | // notice, this list of conditions and the following disclaimer in the 14 | // documentation and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 17 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 18 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 19 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 20 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 21 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 22 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 23 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 24 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 26 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 27 | // DAMAGE. 28 | 29 | #ifndef HPP_CORE_TIME_PARAMETERIZATION_HH 30 | #define HPP_CORE_TIME_PARAMETERIZATION_HH 31 | 32 | #include 33 | #include 34 | 35 | namespace hpp { 36 | namespace core { 37 | class HPP_CORE_DLLAPI TimeParameterization { 38 | public: 39 | virtual value_type value(const value_type& t) const = 0; 40 | virtual value_type derivative(const value_type& t, 41 | const size_type& order) const = 0; 42 | virtual value_type derivativeBound(const value_type& low, 43 | const value_type& up) const { 44 | (void)low; 45 | (void)up; 46 | throw std::logic_error("not implemented"); 47 | } 48 | 49 | virtual TimeParameterizationPtr_t copy() const = 0; 50 | }; // class TimeParameterization 51 | } // namespace core 52 | } // namespace hpp 53 | #endif // HPP_CORE_TIME_PARAMETERIZATION_HH 54 | -------------------------------------------------------------------------------- /include/hpp/core/validation-report.hh: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2014 CNRS 3 | // Authors: Florent Lamiraux 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #ifndef HPP_CORE_VALIDATION_REPORT_HH 31 | #define HPP_CORE_VALIDATION_REPORT_HH 32 | 33 | #include 34 | #include 35 | 36 | namespace hpp { 37 | namespace core { 38 | /// \addtogroup validation 39 | /// \{ 40 | 41 | /// Abstraction of validation report for paths and configurations 42 | /// 43 | /// This class is aimed at being derived to store information relative to 44 | /// various Validation derived classes 45 | /// \li CollisionValidation, 46 | /// \li collision related PathValidation classes. 47 | class HPP_CORE_DLLAPI ValidationReport { 48 | public: 49 | virtual ~ValidationReport() {} 50 | /// Write report in a stream 51 | virtual std::ostream& print(std::ostream& os) const = 0; 52 | }; // class ValidationReport 53 | inline std::ostream& operator<<(std::ostream& os, 54 | const ValidationReport& report) { 55 | return report.print(os); 56 | } 57 | /// \} 58 | } // namespace core 59 | } // namespace hpp 60 | 61 | #endif // HPP_CORE_VALIDATION_REPORT_HH 62 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hpp-core 4 | 6.0.0 5 | Implement basic classes for canonical path planning for kinematic chains. 6 | 7 | Joseph Mirabel 8 | LGPL 9 | 10 | 11 | 12 | Joseph Mirabel 13 | Florent Lamiraux et al. 14 | 15 | catkin 16 | 17 | hpp-pinocchio 18 | hpp-util 19 | hpp-statistics 20 | hpp-constraints 21 | eigen3 22 | urdfdom 23 | coal 24 | proxsuite 25 | 26 | romeo_description 27 | 28 | -------------------------------------------------------------------------------- /plugins/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2019, 2020 Joseph Mirabel, Guilhem Saurel, CNRS Authors: Joseph 2 | # Mirabel (joseph.mirabel@laas.fr) 3 | # 4 | 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # 1. Redistributions of source code must retain the above copyright notice, this 9 | # list of conditions and the following disclaimer. 10 | # 11 | # 1. Redistributions in binary form must reproduce the above copyright notice, 12 | # this list of conditions and the following disclaimer in the documentation 13 | # and/or other materials provided with the distribution. 14 | # 15 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 19 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 20 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 21 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 22 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 23 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 24 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | include(hpp-core-plugin) 27 | 28 | hpp_add_plugin( 29 | spline-gradient-based 30 | EXPORT 31 | spline-gradient-basedTargets 32 | SOURCES 33 | spline-gradient-based.cc 34 | LINK_DEPENDENCIES 35 | PUBLIC 36 | ${PROJECT_NAME} 37 | ${PROJECT_NAME}-gpl) 38 | 39 | project_install_component(spline-gradient-based) 40 | 41 | option(BUILD_TOPPRA_PLUGIN "Build toppra plugin" OFF) 42 | if(BUILD_TOPPRA_PLUGIN) 43 | add_project_dependency(toppra FOR_COMPONENT toppra) 44 | message(STATUS "Found toppra ${toppra_VERSION}") 45 | hpp_add_plugin( 46 | toppra 47 | EXPORT 48 | toppraTargets 49 | SOURCES 50 | toppra 51 | LINK_DEPENDENCIES 52 | PUBLIC 53 | ${PROJECT_NAME} 54 | toppra::toppra) 55 | 56 | project_install_component(toppra) 57 | install(FILES toppra.hh 58 | DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/hpp/core/path-optimization/) 59 | endif() 60 | -------------------------------------------------------------------------------- /plugins/spline-gradient-based.cc: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019, Joseph Mirabel 2 | // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) 3 | // 4 | 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above copyright 13 | // notice, this list of conditions and the following disclaimer in the 14 | // documentation and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 17 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 18 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 19 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 20 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 21 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 22 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 23 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 24 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 26 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 27 | // DAMAGE. 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | namespace hpp { 34 | namespace core { 35 | class SplineGradientBasedPlugin : public ProblemSolverPlugin { 36 | public: 37 | SplineGradientBasedPlugin() 38 | : ProblemSolverPlugin("SplineGradientBasedPlugin", "0.0") {} 39 | 40 | protected: 41 | virtual bool impl_initialize(ProblemSolverPtr_t ps) { 42 | // ps->pathOptimizers.add 43 | // ("SplineGradientBased_cannonical1",pathOptimization::SplineGradientBased::create); ps->pathOptimizers.add 45 | // ("SplineGradientBased_cannonical2",pathOptimization::SplineGradientBased::create); ps->pathOptimizers.add 47 | // ("SplineGradientBased_cannonical3",pathOptimization::SplineGradientBased::create); 49 | ps->pathOptimizers.add( 50 | "SplineGradientBased_bezier1", 51 | pathOptimization::SplineGradientBased::create); 52 | // ps->pathOptimizers.add 53 | // ("SplineGradientBased_bezier2",pathOptimization::SplineGradientBased::create); 55 | ps->pathOptimizers.add( 56 | "SplineGradientBased_bezier3", 57 | pathOptimization::SplineGradientBased::create); 58 | ps->pathOptimizers.add( 59 | "SplineGradientBased_bezier5", 60 | pathOptimization::SplineGradientBased::create); 61 | return true; 62 | } 63 | }; 64 | } // namespace core 65 | } // namespace hpp 66 | 67 | HPP_CORE_DEFINE_PLUGIN(hpp::core::SplineGradientBasedPlugin) 68 | -------------------------------------------------------------------------------- /plugins/toppra.hh: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Joseph Mirabel 2 | // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) 3 | // Olivier Roussel (olivier.roussel@laas.fr) 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #ifndef HPP_CORE_PATH_OPTIMIZATION_TOPPRA_HH 31 | #define HPP_CORE_PATH_OPTIMIZATION_TOPPRA_HH 32 | 33 | #include 34 | 35 | namespace hpp { 36 | namespace core { 37 | namespace pathOptimization { 38 | 39 | class TOPPRA; 40 | typedef shared_ptr TOPPRAPtr_t; 41 | 42 | class TOPPRA : public PathOptimizer { 43 | public: 44 | static TOPPRAPtr_t create(const ProblemConstPtr_t &p) { 45 | return TOPPRAPtr_t(new TOPPRA(p)); 46 | } 47 | 48 | PathVectorPtr_t optimize(const PathVectorPtr_t &path); 49 | 50 | protected: 51 | using PathOptimizer::PathOptimizer; 52 | }; // class TOPPRA 53 | 54 | } // namespace pathOptimization 55 | } // namespace core 56 | } // namespace hpp 57 | 58 | #endif // HPP_CORE_PATH_OPTIMIZATION_TOPPRA_HH 59 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [build-system] 2 | build-backend = "cmeel.build" 3 | requires = [ 4 | "cmeel-boost ~= 1.83.0", 5 | "cmeel[build]", 6 | "hpp-constraints[build]", 7 | "hpp-pinocchio[build]", 8 | "hpp-statistics[build]", 9 | "hpp-util[build]" 10 | ] 11 | 12 | [project] 13 | dependencies = [ 14 | "cmeel-boost ~= 1.83.0", 15 | "hpp-constraints", 16 | "hpp-pinocchio", 17 | "hpp-statistics", 18 | "hpp-util" 19 | ] 20 | description = "Utility classes for testing (robust) equilibrium of a system in contact with the environment, and other centroidal dynamics methods." 21 | license = "BSD-2-Clause" 22 | name = "hpp-core" 23 | version = "6.0.0" 24 | 25 | [tool.ruff] 26 | extend-exclude = ["cmake"] 27 | 28 | [tool.ruff.lint] 29 | extend-select = ["I", "NPY", "RUF", "UP", "W"] 30 | 31 | [tool.tomlsort] 32 | all = true 33 | -------------------------------------------------------------------------------- /src/config-validations.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2014 CNRS 3 | // Authors: Florent Lamiraux 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #include 31 | #include 32 | 33 | namespace hpp { 34 | namespace core { 35 | ConfigValidationsPtr_t ConfigValidations::create() { 36 | ConfigValidations* ptr = new ConfigValidations; 37 | return ConfigValidationsPtr_t(ptr); 38 | } 39 | 40 | bool ConfigValidations::validate(const Configuration_t& config, 41 | ValidationReportPtr_t& validationReport) { 42 | for (std::vector::iterator it = validations_.begin(); 43 | it != validations_.end(); ++it) { 44 | if ((*it)->validate(config, validationReport) == false) { 45 | return false; 46 | } 47 | } 48 | return true; 49 | } 50 | 51 | void ConfigValidations::add(const ConfigValidationPtr_t& configValidation) { 52 | validations_.push_back(configValidation); 53 | } 54 | 55 | size_type ConfigValidations::numberConfigValidations() const { 56 | return (size_type)validations_.size(); 57 | } 58 | } // namespace core 59 | } // namespace hpp 60 | -------------------------------------------------------------------------------- /src/configuration-shooter/uniform-tpl.cc: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Eureka Robotics 2 | // Authors: Joseph Mirabel 3 | 4 | #include 5 | #include 6 | 7 | namespace hpp { 8 | namespace core { 9 | namespace configurationShooter { 10 | 11 | template class UniformTpl; 12 | 13 | } // namespace configurationShooter 14 | } // namespace core 15 | } // namespace hpp 16 | -------------------------------------------------------------------------------- /src/configuration-shooter/uniform.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2018 CNRS 3 | // Authors: Florent Lamiraux 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | namespace hpp { 35 | namespace core { 36 | namespace configurationShooter { 37 | 38 | void Uniform::impl_shoot(Configuration_t& config) const { 39 | size_type extraDim = robot_->extraConfigSpace().dimension(); 40 | size_type offset = robot_->configSize() - extraDim; 41 | 42 | config.resize(robot_->configSize()); 43 | config.head(offset) = ::pinocchio::randomConfiguration(robot_->model()); 44 | 45 | if (sampleExtraDOF_) { 46 | // Shoot extra configuration variables 47 | for (size_type i = 0; i < extraDim; ++i) { 48 | value_type lower = robot_->extraConfigSpace().lower(i); 49 | value_type upper = robot_->extraConfigSpace().upper(i); 50 | value_type range = upper - lower; 51 | if ((range < 0) || (range == std::numeric_limits::infinity())) { 52 | std::ostringstream oss("Cannot uniformy sample extra config variable "); 53 | oss << i << ". min = " << lower << ", max = " << upper << std::endl; 54 | throw std::runtime_error(oss.str()); 55 | } 56 | config[offset + i] = lower + (upper - lower) * rand() / RAND_MAX; 57 | } 58 | } else { 59 | config.tail(extraDim).setZero(); 60 | } 61 | } 62 | 63 | } // namespace configurationShooter 64 | } // namespace core 65 | } // namespace hpp 66 | -------------------------------------------------------------------------------- /src/constraint.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2014 CNRS 3 | // Authors: Florent Lamiraux 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | namespace hpp { 35 | namespace core { 36 | bool Constraint::apply(ConfigurationOut_t configuration) { 37 | return impl_compute(configuration); 38 | } 39 | 40 | template 41 | void Constraint::serialize(Archive& ar, const unsigned int version) { 42 | (void)version; 43 | ar& BOOST_SERIALIZATION_NVP(name_); 44 | ar& BOOST_SERIALIZATION_NVP(weak_); 45 | } 46 | 47 | HPP_SERIALIZATION_IMPLEMENT(Constraint); 48 | } // namespace core 49 | } // namespace hpp 50 | -------------------------------------------------------------------------------- /src/continuous-validation/helper.hh: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2018 CNRS 3 | // Authors: Joseph Mirabel 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #include 31 | #include 32 | 33 | namespace hpp { 34 | namespace core { 35 | namespace continuousValidation { 36 | inline const value_type& first(const interval_t& I, bool reverse) { 37 | return reverse ? I.second : I.first; 38 | } 39 | inline const value_type& second(const interval_t& I, bool reverse) { 40 | return first(I, !reverse); 41 | } 42 | 43 | inline const interval_t& begin(const std::list& I, bool reverse) { 44 | return reverse ? I.back() : I.front(); 45 | } 46 | inline const interval_t& end(const std::list& I, bool reverse) { 47 | return begin(I, !reverse); 48 | } 49 | inline const interval_t& Nth(const std::list& I, int N, 50 | bool reverse) { 51 | if (reverse) { 52 | std::list::const_reverse_iterator it(I.rbegin()); 53 | std::advance(it, N); 54 | return *it; 55 | } else { 56 | std::list::const_iterator it(I.begin()); 57 | std::advance(it, N); 58 | return *it; 59 | } 60 | } 61 | } // namespace continuousValidation 62 | } // namespace core 63 | } // namespace hpp 64 | -------------------------------------------------------------------------------- /src/distance-between-objects.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2014 CNRS 3 | // Authors: Florent Lamiraux 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #include 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | namespace hpp { 40 | namespace core { 41 | void DistanceBetweenObjects::addObstacle( 42 | const CollisionObjectConstPtr_t& object) { 43 | using pinocchio::DISTANCE; 44 | for (size_type i = 0; i < robot_->nbJoints(); ++i) { 45 | JointConstPtr_t joint = robot_->jointAt(i); 46 | BodyPtr_t body = joint->linkedBody(); 47 | if (body) { 48 | for (size_type j = 0; j < body->nbInnerObjects(); ++j) { 49 | collisionPairs_.emplace_back(body->innerObjectAt(j), object); 50 | } 51 | } 52 | } 53 | } 54 | 55 | void DistanceBetweenObjects::obstacles(const ObjectStdVector_t& obstacles) { 56 | for (ObjectStdVector_t::const_iterator itObj = obstacles.begin(); 57 | itObj != obstacles.end(); ++itObj) { 58 | addObstacle(*itObj); 59 | } 60 | } 61 | 62 | void DistanceBetweenObjects::computeDistances() { 63 | distanceResults_.resize(collisionPairs_.size()); 64 | std::size_t rank = 0; 65 | coal::DistanceRequest distanceRequest(true, 0, 0); 66 | for (CollisionPairs_t::const_iterator itCol = collisionPairs_.begin(); 67 | itCol != collisionPairs_.end(); ++itCol) { 68 | const CollisionObjectConstPtr_t& obj1 = itCol->first; 69 | const CollisionObjectConstPtr_t& obj2 = itCol->second; 70 | distanceResults_[rank].clear(); 71 | coal::distance(obj1->geometry().get(), obj1->getFclTransform(), 72 | obj2->geometry().get(), obj2->getFclTransform(), 73 | distanceRequest, distanceResults_[rank]); 74 | ++rank; 75 | } 76 | } 77 | 78 | DistanceBetweenObjects::DistanceBetweenObjects(const DevicePtr_t& robot) 79 | : robot_(robot) {} 80 | } // namespace core 81 | } // namespace hpp 82 | -------------------------------------------------------------------------------- /src/distance/serialization.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2020 CNRS 3 | // Authors: Joseph Mirabel 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include // For serialization of Device 37 | #include 38 | #include 39 | 40 | BOOST_CLASS_EXPORT(hpp::core::WeighedDistance) 41 | BOOST_CLASS_EXPORT(hpp::core::distance::ReedsShepp) 42 | 43 | namespace hpp { 44 | namespace core { 45 | 46 | template 47 | inline void Distance::serialize(Archive& ar, const unsigned int version) { 48 | (void)version; 49 | (void)ar; 50 | } 51 | 52 | template 53 | inline void WeighedDistance::serialize(Archive& ar, 54 | const unsigned int version) { 55 | (void)version; 56 | ar& boost::serialization::make_nvp( 57 | "base", boost::serialization::base_object(*this)); 58 | ar& BOOST_SERIALIZATION_NVP(robot_); 59 | ar& BOOST_SERIALIZATION_NVP(weights_); 60 | ar& BOOST_SERIALIZATION_NVP(weak_); 61 | } 62 | 63 | HPP_SERIALIZATION_IMPLEMENT(WeighedDistance); 64 | 65 | namespace distance { 66 | 67 | template 68 | inline void ReedsShepp::serialize(Archive& ar, const unsigned int version) { 69 | (void)version; 70 | ar& boost::serialization::make_nvp( 71 | "base", boost::serialization::base_object(*this)); 72 | ar& BOOST_SERIALIZATION_NVP(weighedDistance_); 73 | ar& BOOST_SERIALIZATION_NVP(device_); 74 | ar& BOOST_SERIALIZATION_NVP(rho_); 75 | ar& BOOST_SERIALIZATION_NVP(xy_); 76 | ar& BOOST_SERIALIZATION_NVP(rz_); 77 | ar& BOOST_SERIALIZATION_NVP(xyId_); 78 | ar& BOOST_SERIALIZATION_NVP(rzId_); 79 | ar& BOOST_SERIALIZATION_NVP(wheels_); 80 | ar& BOOST_SERIALIZATION_NVP(weak_); 81 | } 82 | 83 | HPP_SERIALIZATION_IMPLEMENT(ReedsShepp); 84 | 85 | } // namespace distance 86 | 87 | } // namespace core 88 | } // namespace hpp 89 | -------------------------------------------------------------------------------- /src/dubins.hh: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2008-2014, Andrew Walker 2 | // 3 | // Permission is hereby granted, free of charge, to any person obtaining a copy 4 | // of this software and associated documentation files (the "Software"), to deal 5 | // in the Software without restriction, including without limitation the rights 6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | // copies of the Software, and to permit persons to whom the Software is 8 | // furnished to do so, subject to the following conditions: 9 | // 10 | // The above copyright notice and this permission notice shall be included in 11 | // all copies or substantial portions of the Software. 12 | // 13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 19 | // THE SOFTWARE. 20 | #ifndef DUBINS_H 21 | #define DUBINS_H 22 | 23 | // Path types 24 | #define LSL (0) 25 | #define LSR (1) 26 | #define RSL (2) 27 | #define RSR (3) 28 | #define RLR (4) 29 | #define LRL (5) 30 | 31 | // The three segment types a path can be made up of 32 | #define L_SEG (0) 33 | #define S_SEG (1) 34 | #define R_SEG (2) 35 | 36 | // Error codes 37 | #define EDUBOK (0) // No error 38 | #define EDUBCOCONFIGS (1) // Colocated configurations 39 | #define EDUBPARAM (2) // Path parameterisitation error 40 | #define EDUBBADRHO (3) // the rho value is invalid 41 | #define EDUBNOPATH (4) // no connection between configurations with this word 42 | 43 | // The segment types for each of the Path types 44 | const int DIRDATA[][3] = {{L_SEG, S_SEG, L_SEG}, {L_SEG, S_SEG, R_SEG}, 45 | {R_SEG, S_SEG, L_SEG}, {R_SEG, S_SEG, R_SEG}, 46 | {R_SEG, L_SEG, R_SEG}, {L_SEG, R_SEG, L_SEG}}; 47 | 48 | #define UNPACK_INPUTS(alpha, beta) \ 49 | double sa = sin(alpha); \ 50 | double sb = sin(beta); \ 51 | double ca = cos(alpha); \ 52 | double cb = cos(beta); \ 53 | double c_ab = cos(alpha - beta); 54 | 55 | #define PACK_OUTPUTS(outputs) \ 56 | outputs[0] = t; \ 57 | outputs[1] = p; \ 58 | outputs[2] = q; 59 | 60 | // The various types of solvers for each of the path types 61 | typedef int (*DubinsWord)(double, double, double, double*); 62 | 63 | // A complete list of the possible solvers that could give optimal paths 64 | extern DubinsWord dubins_words[]; 65 | int dubins_LSL(double alpha, double beta, double d, double* outputs); 66 | int dubins_RSR(double alpha, double beta, double d, double* outputs); 67 | int dubins_LSR(double alpha, double beta, double d, double* outputs); 68 | int dubins_RSL(double alpha, double beta, double d, double* outputs); 69 | int dubins_LRL(double alpha, double beta, double d, double* outputs); 70 | int dubins_RLR(double alpha, double beta, double d, double* outputs); 71 | 72 | #endif // DUBINS_H 73 | -------------------------------------------------------------------------------- /src/extracted-path.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2020 CNRS 3 | // Authors: Joseph Mirabel 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #include "extracted-path.hh" 31 | 32 | #include 33 | #include 34 | 35 | namespace hpp { 36 | namespace core { 37 | template 38 | void ExtractedPath::serialize(Archive& ar, const unsigned int version) { 39 | using namespace boost::serialization; 40 | (void)version; 41 | ar& make_nvp("base", base_object(*this)); 42 | ar& BOOST_SERIALIZATION_NVP(original_); 43 | ar& BOOST_SERIALIZATION_NVP(reversed_); 44 | ar& BOOST_SERIALIZATION_NVP(weak_); 45 | } 46 | 47 | HPP_SERIALIZATION_IMPLEMENT(ExtractedPath); 48 | } // namespace core 49 | } // namespace hpp 50 | 51 | BOOST_CLASS_EXPORT(hpp::core::ExtractedPath) 52 | -------------------------------------------------------------------------------- /src/nearest-neighbor/serialization.cc: -------------------------------------------------------------------------------- 1 | #include "basic.hh" 2 | // #include "k-d-tree.hh" 3 | 4 | #include 5 | #include 6 | 7 | BOOST_CLASS_EXPORT(hpp::core::nearestNeighbor::Basic) 8 | // BOOST_CLASS_EXPORT(hpp::core::nearestNeighbor::KDTree) 9 | 10 | namespace hpp { 11 | namespace core { 12 | 13 | template 14 | inline void NearestNeighbor::serialize(Archive& ar, 15 | const unsigned int version) { 16 | (void)version; 17 | (void)ar; 18 | } 19 | 20 | HPP_SERIALIZATION_IMPLEMENT(NearestNeighbor); 21 | 22 | namespace nearestNeighbor { 23 | 24 | template 25 | inline void Basic::serialize(Archive& ar, const unsigned int version) { 26 | (void)version; 27 | ar& boost::serialization::make_nvp( 28 | "base", boost::serialization::base_object(*this)); 29 | ar& boost::serialization::make_nvp("distance_", 30 | const_cast(distance_)); 31 | } 32 | 33 | HPP_SERIALIZATION_IMPLEMENT(Basic); 34 | 35 | /* 36 | template 37 | inline void KDTree::serialize(Archive& ar, const unsigned int version) 38 | { 39 | throw std::logic_error("nearestNeighbor::KDTree not serializable."); 40 | (void) version; 41 | ar & boost::serialization::make_nvp("base", 42 | boost::serialization::base_object(*this)); ar & 43 | BOOST_SERIALIZATION_NVP(distance_); 44 | } 45 | 46 | HPP_SERIALIZATION_IMPLEMENT(KDTree); 47 | */ 48 | 49 | } // namespace nearestNeighbor 50 | } // namespace core 51 | } // namespace hpp 52 | -------------------------------------------------------------------------------- /src/path-validation.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2022 CNRS Airbus S.A.S. 3 | // Authors: Florent Lamiraux 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | namespace hpp { 35 | namespace core { 36 | bool PathValidation::validate(ConfigurationIn_t q, 37 | ValidationReportPtr_t& report) { 38 | // Create a straight path of length 0 with the configuration. 39 | // The output space does not matter here since no Liegroup operation 40 | // will be performed. Thus, we use Rn. 41 | StraightPathPtr_t p( 42 | StraightPath::create(LiegroupSpace::Rn(q.size()), q, q, 43 | std::make_pair(0, 0))); 44 | PathPtr_t unused; 45 | PathValidationReportPtr_t r; 46 | bool res(this->validate(p, false, unused, r)); 47 | if (r) report = r->configurationReport; 48 | return res; 49 | } 50 | } // namespace core 51 | } // namespace hpp 52 | -------------------------------------------------------------------------------- /src/path-validation/discretized-collision-checking.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2014 CNRS 3 | // Authors: Florent Lamiraux 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | namespace hpp { 35 | namespace core { 36 | namespace pathValidation { 37 | DiscretizedPtr_t createDiscretizedCollisionChecking( 38 | const DevicePtr_t& robot, const value_type& stepSize) { 39 | DiscretizedPtr_t pv(Discretized::create(stepSize)); 40 | CollisionValidationPtr_t cv(CollisionValidation::create(robot)); 41 | pv->add(cv); 42 | return pv; 43 | } 44 | } // namespace pathValidation 45 | } // namespace core 46 | } // namespace hpp 47 | -------------------------------------------------------------------------------- /src/path-validation/discretized-joint-bound.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2014 CNRS 3 | // Authors: Florent Lamiraux 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | namespace hpp { 35 | namespace core { 36 | namespace pathValidation { 37 | DiscretizedPtr_t createDiscretizedJointBound(const DevicePtr_t& robot, 38 | const value_type& stepSize) { 39 | DiscretizedPtr_t pv(Discretized::create(stepSize)); 40 | JointBoundValidationPtr_t jbv(JointBoundValidation::create(robot)); 41 | pv->add(jbv); 42 | return pv; 43 | } 44 | } // namespace pathValidation 45 | } // namespace core 46 | } // namespace hpp 47 | -------------------------------------------------------------------------------- /src/path-validation/no-validation.hh: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2019 CNRS 3 | // Authors: Florent Lamiraux 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | namespace hpp { 39 | namespace core { 40 | namespace pathValidation { 41 | HPP_PREDEF_CLASS(NoValidation); 42 | typedef shared_ptr NoValidationPtr_t; 43 | class NoValidation : public PathValidation { 44 | public: 45 | // Validate all paths 46 | virtual bool validate(const PathPtr_t& path, bool, PathPtr_t& validPart, 47 | PathValidationReportPtr_t&) { 48 | validPart = path; 49 | return true; 50 | } 51 | static NoValidationPtr_t create(const DevicePtr_t&, const value_type&) { 52 | NoValidation* ptr = new NoValidation(); 53 | NoValidationPtr_t shPtr(ptr); 54 | return shPtr; 55 | } 56 | 57 | protected: 58 | NoValidation() {} 59 | }; // class NoValidation 60 | } // namespace pathValidation 61 | } // namespace core 62 | } // namespace hpp 63 | -------------------------------------------------------------------------------- /src/path-validations.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2017 CNRS 3 | // Authors: Diane Bury 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | namespace hpp { 38 | namespace core { 39 | 40 | PathValidationsPtr_t PathValidations::create() { 41 | PathValidations* ptr = new PathValidations(); 42 | return PathValidationsPtr_t(ptr); 43 | } 44 | 45 | void PathValidations::addPathValidation( 46 | const PathValidationPtr_t& pathValidation) { 47 | validations_.push_back(pathValidation); 48 | } 49 | 50 | bool PathValidations::validate(const PathPtr_t& path, bool reverse, 51 | PathPtr_t& validPart, 52 | PathValidationReportPtr_t& validationReport) { 53 | PathPtr_t tempPath = path; 54 | PathPtr_t tempValidPart; 55 | PathValidationReportPtr_t tempValidationReport; 56 | 57 | bool result = true; 58 | value_type lastValidTime = path->timeRange().second; 59 | value_type t = lastValidTime; 60 | 61 | for (std::vector::iterator it = validations_.begin(); 62 | it != validations_.end(); ++it) { 63 | if ((*it)->validate(tempPath, reverse, tempValidPart, 64 | tempValidationReport) == false) { 65 | t = tempValidationReport->getParameter(); 66 | if (t < lastValidTime) { 67 | lastValidTime = t; 68 | tempPath = tempValidPart; 69 | } 70 | result = false; 71 | } 72 | } 73 | validPart = tempPath; 74 | validationReport->setParameter(lastValidTime); 75 | return result; 76 | } 77 | 78 | bool PathValidations::validate(ConfigurationIn_t q, 79 | ValidationReportPtr_t& report) { 80 | for (auto pv : validations_) { 81 | bool res(pv->validate(q, report)); 82 | if (!res) return false; 83 | } 84 | return true; 85 | } 86 | 87 | PathValidations::PathValidations() {} 88 | } // namespace core 89 | } // namespace hpp 90 | -------------------------------------------------------------------------------- /src/plan-and-optimize.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2014 CNRS 3 | // Authors: Florent Lamiraux 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #include 31 | #include 32 | 33 | namespace hpp { 34 | namespace core { 35 | 36 | void PlanAndOptimize::oneStep() { pathPlanner_->oneStep(); } 37 | 38 | void PlanAndOptimize::startSolve() { pathPlanner_->startSolve(); } 39 | 40 | PathVectorPtr_t PlanAndOptimize::finishSolve(const PathVectorPtr_t& path) { 41 | PathVectorPtr_t result = path; 42 | for (Optimizers_t::iterator itOpt = optimizers_.begin(); 43 | itOpt != optimizers_.end(); ++itOpt) { 44 | result = (*itOpt)->optimize(result); 45 | } 46 | return result; 47 | } 48 | 49 | void PlanAndOptimize::addPathOptimizer(const PathOptimizerPtr_t& optimizer) { 50 | optimizers_.push_back(optimizer); 51 | } 52 | 53 | PlanAndOptimizePtr_t PlanAndOptimize::create( 54 | const PathPlannerPtr_t& pathPlanner) { 55 | PlanAndOptimize* ptr = new PlanAndOptimize(pathPlanner); 56 | return PlanAndOptimizePtr_t(ptr); 57 | } 58 | 59 | PlanAndOptimize::PlanAndOptimize(const PathPlannerPtr_t& pathPlanner) 60 | : PathPlanner(pathPlanner->problem(), pathPlanner->roadmap()), 61 | pathPlanner_(pathPlanner), 62 | optimizers_() {} 63 | 64 | } // namespace core 65 | } // namespace hpp 66 | -------------------------------------------------------------------------------- /src/steering-method/dubins.cc: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2017, CNRS 2 | // Authors: Florent Lamiraux 3 | // 4 | 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above copyright 13 | // notice, this list of conditions and the following disclaimer in the 14 | // documentation and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 17 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 18 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 19 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 20 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 21 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 22 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 23 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 24 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 26 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 27 | // DAMAGE. 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | namespace hpp { 37 | namespace core { 38 | namespace steeringMethod { 39 | PathPtr_t Dubins::impl_compute(ConfigurationIn_t q1, 40 | ConfigurationIn_t q2) const { 41 | Configuration_t qEnd(q2); 42 | qEnd.segment<2>(xyId_) = q1.segment<2>(xyId_); 43 | qEnd.segment<2>(rzId_) = q1.segment<2>(rzId_); 44 | // Do not take into account wheel joints in additional distance. 45 | for (std::vector::const_iterator it = wheels_.begin(); 46 | it != wheels_.end(); ++it) { 47 | size_type i = (*it)->rankInConfiguration(); 48 | qEnd[i] = q1[i]; 49 | } 50 | // The length corresponding to the non RS DoF 51 | DistancePtr_t d(problem()->distance()); 52 | value_type extraL = (*d)(q1, qEnd); 53 | DubinsPathPtr_t path = 54 | DubinsPath::create(device_.lock(), q1, q2, extraL, rho_, xyId_, rzId_, 55 | wheels_, constraints()); 56 | return path; 57 | } 58 | 59 | Dubins::Dubins(const ProblemConstPtr_t& problem) : CarLike(problem), weak_() {} 60 | 61 | Dubins::Dubins(const ProblemConstPtr_t& problem, const value_type turningRadius, 62 | JointPtr_t xyJoint, JointPtr_t rzJoint, 63 | std::vector wheels) 64 | : CarLike(problem, turningRadius, xyJoint, rzJoint, wheels) {} 65 | 66 | /// Copy constructor 67 | Dubins::Dubins(const Dubins& other) : CarLike(other) {} 68 | 69 | } // namespace steeringMethod 70 | } // namespace core 71 | } // namespace hpp 72 | -------------------------------------------------------------------------------- /src/steering-method/snibud.cc: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2017, CNRS 2 | // Authors: Florent Lamiraux 3 | // 4 | 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above copyright 13 | // notice, this list of conditions and the following disclaimer in the 14 | // documentation and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 17 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 18 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 19 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 20 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 21 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 22 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 23 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 24 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 26 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 27 | // DAMAGE. 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | namespace hpp { 37 | namespace core { 38 | namespace steeringMethod { 39 | PathPtr_t Snibud::impl_compute(ConfigurationIn_t q1, 40 | ConfigurationIn_t q2) const { 41 | Configuration_t qEnd(q2); 42 | qEnd.segment<2>(xyId_) = q1.segment<2>(xyId_); 43 | qEnd.segment<2>(rzId_) = q1.segment<2>(rzId_); 44 | // Do not take into account wheel joints in additional distance. 45 | for (std::vector::const_iterator it = wheels_.begin(); 46 | it != wheels_.end(); ++it) { 47 | size_type i = (*it)->rankInConfiguration(); 48 | qEnd[i] = q1[i]; 49 | } 50 | // The length corresponding to the non RS DoF 51 | DistancePtr_t d(problem()->distance()); 52 | value_type extraL = (*d)(q1, qEnd); 53 | DubinsPathPtr_t path = 54 | DubinsPath::create(device_.lock(), q2, q1, extraL, rho_, xyId_, rzId_, 55 | wheels_, constraints()); 56 | 57 | return path->reverse(); 58 | } 59 | 60 | Snibud::Snibud(const ProblemConstPtr_t& problem) : CarLike(problem), weak_() {} 61 | 62 | Snibud::Snibud(const ProblemConstPtr_t& problem, const value_type turningRadius, 63 | JointPtr_t xyJoint, JointPtr_t rzJoint, 64 | std::vector wheels) 65 | : CarLike(problem, turningRadius, xyJoint, rzJoint, wheels) {} 66 | 67 | /// Copy constructor 68 | Snibud::Snibud(const Snibud& other) : CarLike(other) {} 69 | 70 | } // namespace steeringMethod 71 | } // namespace core 72 | } // namespace hpp 73 | -------------------------------------------------------------------------------- /src/steering-method/straight.cc: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 CNRS 2 | // Authors: Joseph Mirabel 3 | // 4 | 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above copyright 13 | // notice, this list of conditions and the following disclaimer in the 14 | // documentation and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 17 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 18 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 19 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 20 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 21 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 22 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 23 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 24 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 26 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 27 | // DAMAGE. 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | namespace hpp { 36 | namespace core { 37 | namespace steeringMethod { 38 | PathPtr_t Straight::impl_compute(ConfigurationIn_t q1, 39 | ConfigurationIn_t q2) const { 40 | value_type length = (*problem()->distance())(q1, q2); 41 | ConstraintSetPtr_t c; 42 | if (constraints() && constraints()->configProjector()) { 43 | c = HPP_STATIC_PTR_CAST(ConstraintSet, constraints()->copy()); 44 | c->configProjector()->rightHandSideFromConfig(q1); 45 | c->configProjector()->lineSearchType(ConfigProjector::Backtracking); 46 | } else { 47 | c = constraints(); 48 | } 49 | PathPtr_t path = StraightPath::create(problem()->robot(), q1, q2, length, c); 50 | return path; 51 | } 52 | } // namespace steeringMethod 53 | } // namespace core 54 | } // namespace hpp 55 | -------------------------------------------------------------------------------- /tests/containers.cc: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016, Joseph Mirabel 2 | // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) 3 | // 4 | 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above copyright 13 | // notice, this list of conditions and the following disclaimer in the 14 | // documentation and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 17 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 18 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 19 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 20 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 21 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 22 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 23 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 24 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 26 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 27 | // DAMAGE. 28 | 29 | #define BOOST_TEST_MODULE containers 30 | #include 31 | #include 32 | 33 | typedef std::string Type1; 34 | typedef std::shared_ptr Type2; 35 | 36 | using hpp::core::Container; 37 | 38 | BOOST_AUTO_TEST_SUITE(test_hpp_core_container) 39 | 40 | BOOST_AUTO_TEST_CASE(containers) { 41 | Container c1; 42 | Container c2; 43 | c1.add("key1", Type1("type1_1")); 44 | c2.add("key1", Type2(new std::string("type2_1"))); 45 | 46 | c1.print(std::cout); 47 | c2.print(std::cout); 48 | 49 | c2.add("key1", Type2(new std::string("type2_2"))); 50 | c2.print(std::cout); 51 | } 52 | 53 | BOOST_AUTO_TEST_SUITE_END() 54 | -------------------------------------------------------------------------------- /tests/data/empty.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /tests/plugin-test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2019, Joseph Mirabel Authors: Joseph Mirabel 2 | # (joseph.mirabel@laas.fr) 3 | # 4 | 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # 1. Redistributions of source code must retain the above copyright notice, this 9 | # list of conditions and the following disclaimer. 10 | # 11 | # 1. Redistributions in binary form must reproduce the above copyright notice, 12 | # this list of conditions and the following disclaimer in the documentation 13 | # and/or other materials provided with the distribution. 14 | # 15 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 19 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 20 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 21 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 22 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 23 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 24 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | add_library(example MODULE EXCLUDE_FROM_ALL example.cc) 27 | set_target_properties( 28 | example PROPERTIES PREFIX "" # BUILD_WITH_INSTALL_RPATH TRUE 29 | ) 30 | 31 | target_link_libraries(example ${PROJECT_NAME}) 32 | 33 | # INSTALL(TARGETS example DESTINATION lib/hppPlugins) 34 | -------------------------------------------------------------------------------- /tests/plugin-test/example.cc: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019, Joseph Mirabel 2 | // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) 3 | // 4 | 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above copyright 13 | // notice, this list of conditions and the following disclaimer in the 14 | // documentation and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 17 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 18 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 19 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 20 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 21 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 22 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 23 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 24 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 26 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 27 | // DAMAGE. 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | namespace foo { 34 | class ExamplePlugin : public hpp::core::ProblemSolverPlugin { 35 | public: 36 | ExamplePlugin() : ProblemSolverPlugin("ExamplePlugin", "0.0") {} 37 | 38 | protected: 39 | virtual bool impl_initialize(hpp::core::ProblemSolverPtr_t ps) { 40 | ps->distances.add("WeighedDuplicate", 41 | hpp::core::WeighedDistance::createFromProblem); 42 | return true; 43 | } 44 | }; 45 | } // namespace foo 46 | 47 | HPP_CORE_DEFINE_PLUGIN(foo::ExamplePlugin) 48 | -------------------------------------------------------------------------------- /tests/plugin.cc.in: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019, Joseph Mirabel 2 | // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are 6 | // met: 7 | // 8 | // 1. Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 11 | // 2. Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 16 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 17 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 18 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 21 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 22 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 23 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 26 | // DAMAGE. 27 | 28 | #define BOOST_TEST_MODULE plugin 29 | #include 30 | #include 31 | 32 | #include 33 | #include 34 | 35 | using namespace hpp::core; 36 | 37 | BOOST_AUTO_TEST_CASE (load_plugin) 38 | { 39 | ProblemSolverPtr_t ps = ProblemSolver::create(); 40 | 41 | std::string lib ("@CMAKE_BINARY_DIR@/tests/plugin-test/example.so"); 42 | BOOST_CHECK (plugin::loadPlugin (lib, ps)); 43 | 44 | BOOST_CHECK (ps->distances.has ("WeighedDuplicate")); 45 | 46 | delete ps; 47 | } 48 | -------------------------------------------------------------------------------- /tests/test-config-validations.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2018 CNRS 3 | // Authors: Diane Bury 4 | // 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright 14 | // notice, this list of conditions and the following disclaimer in the 15 | // documentation and/or other materials provided with the distribution. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | // DAMAGE. 29 | 30 | #define BOOST_TEST_MODULE test_config_validations 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | using namespace hpp::core; 39 | 40 | typedef std::vector ConfigValidationTypes_t; 41 | 42 | BOOST_AUTO_TEST_CASE(config_validations) { 43 | ProblemSolverPtr_t ps = ProblemSolver::create(); 44 | ps->robot(ps->createRobot("robot")); 45 | ConfigValidationTypes_t configTypes = ps->configValidationTypes(); 46 | BOOST_CHECK_MESSAGE(std::find(configTypes.begin(), configTypes.end(), 47 | "CollisionValidation") != configTypes.end(), 48 | "CollisionValidation is not present"); 49 | BOOST_CHECK_MESSAGE(std::find(configTypes.begin(), configTypes.end(), 50 | "JointBoundValidation") != configTypes.end(), 51 | "JointBoundValidation is not present"); 52 | 53 | ProblemPtr_t problem = ps->problem(); 54 | ConfigValidationsPtr_t configValidations = problem->configValidations(); 55 | BOOST_CHECK_MESSAGE(configValidations->numberConfigValidations() == 2, 56 | "ConfigValidations not set right in Problem"); 57 | } 58 | 59 | BOOST_AUTO_TEST_CASE(test_add_config_validation) { 60 | ProblemSolverPtr_t ps = ProblemSolver::create(); 61 | ps->robot(ps->createRobot("robot")); 62 | ProblemPtr_t problem = ps->problem(); 63 | 64 | ps->clearConfigValidations(); 65 | BOOST_CHECK_MESSAGE( 66 | problem->configValidations()->numberConfigValidations() == 0, 67 | "Clearing ConfigValidations did not work"); 68 | 69 | ps->addConfigValidation("CollisionValidation"); 70 | BOOST_CHECK_MESSAGE( 71 | problem->configValidations()->numberConfigValidations() == 1, 72 | "Adding CollisionValidation to the ProblemSolver did not work"); 73 | } 74 | --------------------------------------------------------------------------------