Skip to content
Snippets Groups Projects

Compare revisions

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

Source

Select target project
No results found

Target

Select target project
  • jchemin/hpp-bezier-com-traj
  • pfernbac/hpp-bezier-com-traj
  • gsaurel/hpp-bezier-com-traj
  • humanoid-path-planner/hpp-bezier-com-traj
4 results
Show changes
Commits on Source (263)
163 additional commits have been omitted to prevent performance issues.
Hide whitespace changes
Inline Side-by-side
Showing
with 487 additions and 649 deletions
.git-blame-ignore-revs 0 → 100644
View file @ 02786ee6
# format (Guilhem Saurel, 2022-04-05)
6e4505ec181290ffad5bf832b51dec30f04abf52
# [Python] allow Python 3 & fix format (Guilhem Saurel, 2019-09-25)
c9d5314ca82524d5aab0dffd75c40564262495b1
# pre-commit run -a (Guilhem Saurel, 2022-10-05)
5bac51b3232c27d606faeaa6746de9a04de149d6
.github/workflows/nix.yml 0 → 100644
View file @ 02786ee6
name: "CI - Nix"
on:
push:
jobs:
tests:
name: "Nix build on ${{ matrix.os }}"
runs-on: "${{ matrix.os }}-latest"
strategy:
matrix:
os: [ubuntu]
steps:
- uses: actions/checkout@v4
- uses: cachix/install-nix-action@v27
- uses: cachix/cachix-action@v15
with:
name: gepetto
authToken: '${{ secrets.CACHIX_AUTH_TOKEN }}'
- run: nix build -L
.gitlab-ci.yml 0 → 100644
View file @ 02786ee6
include: https://rainboard.laas.fr/project/hpp-bezier-com-traj/.gitlab-ci.yml
.mergify.yml 0 → 100644
View file @ 02786ee6
pull_request_rules:
- name: merge automatically when CI passes and PR is approved
conditions:
- check-success = "gitlab-ci"
- check-success = "Nix build on ubuntu"
- check-success = "pre-commit.ci - pr"
- or:
- author = pre-commit-ci[bot]
- author = dependabot[bot]
actions:
merge:
.pre-commit-config.yaml 0 → 100644
View file @ 02786ee6
ci:
autoupdate_branch: devel
repos:
- repo: https://github.com/astral-sh/ruff-pre-commit
rev: v0.8.1
hooks:
- id: ruff
args:
- --fix
- --exit-non-zero-on-fix
- id: ruff-format
- repo: https://github.com/cheshirekow/cmake-format-precommit
rev: v0.6.13
hooks:
- id: cmake-format
- repo: https://github.com/pappasam/toml-sort
rev: v0.24.2
hooks:
- id: toml-sort-fix
exclude: poetry.lock
- repo: https://github.com/pre-commit/mirrors-clang-format
rev: v19.1.4
hooks:
- id: clang-format
args:
- --style=Google
- repo: https://github.com/pre-commit/pre-commit-hooks
rev: v5.0.0
hooks:
- id: check-added-large-files
- id: check-ast
- id: check-executables-have-shebangs
- id: check-json
- id: check-merge-conflict
- id: check-symlinks
- id: check-toml
- id: check-yaml
- id: debug-statements
- id: destroyed-symlinks
- id: detect-private-key
- id: end-of-file-fixer
- id: fix-byte-order-marker
- id: mixed-line-ending
- id: trailing-whitespace
CMakeLists.txt
View file @ 02786ee6
cmake_minimum_required(VERSION 2.6)
INCLUDE(cmake/base.cmake)
INCLUDE(cmake/python.cmake)
SET(PROJECT_NAME bezier-com-traj)
SET(PROJECT_DESCRIPTION
"Multi contact trajectory generation for the COM using Bezier curves"
)
SET(PROJECT_URL "")
set(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake/;${PROJECT_SOURCE_DIR}/cmake2/")
set(SRC_DIR "${PROJECT_SOURCE_DIR}/src")
set(INCLUDE_DIR "${PROJECT_SOURCE_DIR}/include")
set(EXTERNAL_LIBRARY_DIR "${PROJECT_SOURCE_DIR}/external/lib")
if ( MSVC )
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DNOMINMAX")
endif()
# Where to compile shared objects
SET(LIBRARY_OUTPUT_PATH ${PROJECT_BINARY_DIR}/lib)
SETUP_PROJECT()
# Inhibit all warning messages.
#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w")
# remove flag that makes all warnings into errors
string (REPLACE "-Werror" "" CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS})
MESSAGE( STATUS "CMAKE_CXX_FLAGS: " ${CMAKE_CXX_FLAGS} )
OPTION (BUILD_PYTHON_INTERFACE "Build the python binding" ON)
IF(BUILD_PYTHON_INTERFACE)
# search for python
FINDPYTHON(2.7 REQUIRED)
find_package( PythonLibs 2.7 REQUIRED )
include_directories( ${PYTHON_INCLUDE_DIRS} )
find_package( Boost COMPONENTS python REQUIRED )
include_directories( ${Boost_INCLUDE_DIR} )
ENDIF(BUILD_PYTHON_INTERFACE)
find_package (centroidal-dynamics-lib REQUIRED)
find_package (Spline REQUIRED)
# Declare Headers
SET(${PROJECT_NAME}_HEADERS
include/bezier-com-traj/data.hh
include/bezier-com-traj/utils.hh
include/bezier-com-traj/flags.hh
include/bezier-com-traj/definitions.hh
include/bezier-com-traj/config.hh
include/bezier-com-traj/solve.hh
include/bezier-com-traj/solve_end_effector.hh
include/bezier-com-traj/common_solve_methods.hh
include/bezier-com-traj/common_solve_methods.inl
include/bezier-com-traj/cost/costfunction_definition.hh
include/bezier-com-traj/waypoints/waypoints_definition.hh
include/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh
include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1.hh
include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh
include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh
include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh
include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh
include/solver/eiquadprog-fast.hpp
)
find_package(Eigen3)
if (EIGEN3_INCLUDE_DIR)
message(STATUS "Found Eigen3")
cmake_minimum_required(VERSION 3.10)
# Project properties
set(PROJECT_NAME hpp-bezier-com-traj)
set(PROJECT_DESCRIPTION
"Multi contact trajectory generation for the COM using Bezier curves")
# Project options
option(BUILD_PYTHON_INTERFACE "Build the python bindings" ON)
option(USE_GLPK "Use sparse lp solver" OFF)
option(PRINT_QHULL_INEQ
"generate text file containing last inequality computed" OFF)
# Project configuration
set(PROJECT_USE_CMAKE_EXPORT TRUE)
set(CUSTOM_HEADER_DIR "hpp/bezier-com-traj")
set(CXX_DISABLE_WERROR TRUE)
# Check if the submodule cmake have been initialized
set(JRL_CMAKE_MODULES "${CMAKE_CURRENT_LIST_DIR}/cmake")
if(EXISTS "${JRL_CMAKE_MODULES}/base.cmake")
message(STATUS "JRL cmakemodules found in 'cmake/' git submodule")
else()
message(STATUS "Eigen3 not found looking for Eigen2")
find_package(Eigen2 REQUIRED)
find_package(jrl-cmakemodules QUIET CONFIG)
if(jrl-cmakemodules_FOUND)
get_property(
JRL_CMAKE_MODULES
TARGET jrl-cmakemodules::jrl-cmakemodules
PROPERTY INTERFACE_INCLUDE_DIRECTORIES)
message(STATUS "JRL cmakemodules found on system at ${JRL_CMAKE_MODULES}")
elseif(${CMAKE_VERSION} VERSION_LESS "3.14.0")
message(
FATAL_ERROR
"\nCan't find jrl-cmakemodules. Please either:\n"
" - use git submodule: 'git submodule update --init'\n"
" - or install https://github.com/jrl-umi3218/jrl-cmakemodules\n"
" - or upgrade your CMake version to >= 3.14 to allow automatic fetching\n"
)
else()
message(STATUS "JRL cmakemodules not found. Let's fetch it.")
include(FetchContent)
FetchContent_Declare(
"jrl-cmakemodules"
GIT_REPOSITORY "https://github.com/jrl-umi3218/jrl-cmakemodules.git")
FetchContent_MakeAvailable("jrl-cmakemodules")
FetchContent_GetProperties("jrl-cmakemodules" SOURCE_DIR JRL_CMAKE_MODULES)
endif()
endif()
include("${JRL_CMAKE_MODULES}/hpp.cmake")
include("${JRL_CMAKE_MODULES}/boost.cmake")
# Project definition
compute_project_args(PROJECT_ARGS LANGUAGES CXX)
project(${PROJECT_NAME} ${PROJECT_ARGS})
if(BUILD_PYTHON_INTERFACE)
string(REGEX REPLACE "-" "_" PY_NAME ${PROJECT_NAME})
add_project_dependency(eigenpy 2.7.12 REQUIRED)
endif(BUILD_PYTHON_INTERFACE)
# Project dependencies
add_project_dependency(hpp-centroidal-dynamics REQUIRED)
add_project_dependency(ndcurves REQUIRED)
if(USE_GLPK)
add_project_dependency(glpk REQUIRED FIND_EXTERNAL glpk)
add_definitions(-DUSE_GLPK_SOLVER=1)
endif(USE_GLPK)
# Main Library
set(${PROJECT_NAME}_HEADERS
include/${CUSTOM_HEADER_DIR}/common_solve_methods.hh
include/${CUSTOM_HEADER_DIR}/common_solve_methods.inl
include/${CUSTOM_HEADER_DIR}/cost/costfunction_definition.hh
include/${CUSTOM_HEADER_DIR}/data.hh
include/${CUSTOM_HEADER_DIR}/definitions.hh
include/${CUSTOM_HEADER_DIR}/flags.hh
include/${CUSTOM_HEADER_DIR}/local_config.hh
include/${CUSTOM_HEADER_DIR}/solve_end_effector.hh
include/${CUSTOM_HEADER_DIR}/solve.hh
include/${CUSTOM_HEADER_DIR}/solver/eiquadprog-fast.hpp # TODO: use
# stack-of-task/eiquadprog
include/${CUSTOM_HEADER_DIR}/solver/glpk-wrapper.hpp
include/${CUSTOM_HEADER_DIR}/solver/solver-abstract.hpp
include/${CUSTOM_HEADER_DIR}/utils.hh
include/${CUSTOM_HEADER_DIR}/waypoints/waypoints_c0_dc0_c1.hh
include/${CUSTOM_HEADER_DIR}/waypoints/waypoints_c0_dc0_dc1_c1.hh
include/${CUSTOM_HEADER_DIR}/waypoints/waypoints_c0_dc0_dc1.hh
include/${CUSTOM_HEADER_DIR}/waypoints/waypoints_c0_dc0_ddc0_c1.hh
include/${CUSTOM_HEADER_DIR}/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh
include/${CUSTOM_HEADER_DIR}/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh
include/${CUSTOM_HEADER_DIR}/waypoints/waypoints_c0_dc0_ddc0.hh
include/${CUSTOM_HEADER_DIR}/waypoints/waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh
include/${CUSTOM_HEADER_DIR}/waypoints/waypoints_c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1.hh
include/${CUSTOM_HEADER_DIR}/waypoints/waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh
include/${CUSTOM_HEADER_DIR}/waypoints/waypoints_definition.hh)
set(${PROJECT_NAME}_SOURCES
src/common_solve_methods.cpp
src/costfunction_definition.cpp
src/solver-abstract.cpp
src/eiquadprog-fast.cpp
src/computeCOMTraj.cpp
src/solve_0_step.cpp
src/utils.cpp
src/waypoints_definition.cpp)
if(USE_GLPK)
set(${PROJECT_NAME}_SOURCES ${${PROJECT_NAME}_SOURCES} src/glpk-wrapper.cpp)
endif(USE_GLPK)
add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SOURCES}
${${PROJECT_NAME}_HEADERS})
target_include_directories(
${PROJECT_NAME} PUBLIC $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>)
target_link_libraries(${PROJECT_NAME} ndcurves::ndcurves
hpp-centroidal-dynamics::hpp-centroidal-dynamics)
if(USE_GLPK)
target_include_directories(${PROJECT_NAME} PUBLIC ${glpk_INCLUDE_DIR})
target_link_libraries(${PROJECT_NAME} ${glpk_LIBRARY})
endif(USE_GLPK)
install(
TARGETS ${PROJECT_NAME}
EXPORT ${TARGETS_EXPORT_NAME}
DESTINATION lib)
add_subdirectory(src)
add_subdirectory(tests)
IF(BUILD_PYTHON_INTERFACE)
add_subdirectory (python)
ENDIF(BUILD_PYTHON_INTERFACE)
SETUP_PROJECT_FINALIZE()
if(BUILD_PYTHON_INTERFACE)
add_subdirectory(python)
endif(BUILD_PYTHON_INTERFACE)
install(FILES package.xml DESTINATION share/${PROJECT_NAME})
LICENSE 0 → 100644
View file @ 02786ee6
BSD 2-Clause License
Copyright (c) 2018, t steve
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
README.md 0 → 100644
View file @ 02786ee6
# bezier_COM_Traj
[![Pipeline status](https://gitlab.laas.fr/humanoid-path-planner/hpp-bezier-com-traj/badges/master/pipeline.svg)](https://gitlab.laas.fr/humanoid-path-planner/hpp-bezier-com-traj/commits/master)
[![Coverage report](https://gitlab.laas.fr/humanoid-path-planner/hpp-bezier-com-traj/badges/master/coverage.svg?job=doc-coverage)](https://gepettoweb.laas.fr/doc/humanoid-path-planner/hpp-bezier-com-traj/master/coverage/)
[![Code style: black](https://img.shields.io/badge/code%20style-black-000000.svg)](https://github.com/psf/black)
[![pre-commit.ci status](https://results.pre-commit.ci/badge/github/humanoid-path-planner/hpp-bezier-com-traj/master.svg)](https://results.pre-commit.ci/latest/github/humanoid-path-planner/hpp-bezier-com-traj)
Copyright 2018-2020 LAAS-CNRS
Authors: Pierre Fernbach and Steve Tonneau
## Description
bezier_COM_Traj implements tools to compute Bezier trajectories given various sets of constraints: initial and terminal conditions (position, velocities, acceleration), additional linear constraints on the complete trajectory, and, most interestingly, constraints related to the center of mass dynamics.
The trajectories are genererated through the resolution of convex optimization (Quadratic Programms), and thus allow to specify a cost functional to minimize.
The library is implemented in C++, but also provides Python bindings.
Two types of applications can be used so far:
- First, zero step capturability: Given the centroidal state of a robot, determines whether it is possible for the robot to come to a stop without violating frictional constraints. In this formulation, the problem can be solved continuously, and angular momentum constraints can be used.
- Second, the general case (which encompasses zero step capturability):
Given a sequence of discrete contact configurations, and given the current state of the robot, and a desired target state, compute a kinematically and dynamically accurate trajectory for the center of mass of the robot. In this general case, the trajectory is checked at discrete intervals (the verification is not continuous). Furthermore, at this point angular momentum is not handled (this is a TODO and not a limitation of the approach).
More details can be found in the preprint paper:
CROC: Convex Resolution Of Centroidal dynamics trajectories to provide a feasibility criterion for the multi contact planning problem, by Fernbach et al.
https://hal.archives-ouvertes.fr/hal-01726155v1
## Dependencies
* [centroidal-dynamics-lib](https://github.com/stonneau/centroidal-dynamics-lib) Centroidal dynamics computation library
* [ndcurves](https://github.com/loco-3d/ndcurves) Bezier curves library
* [glpk](https://www.gnu.org/software/glpk/) GNU Linear Programming Kit
## Additional dependencies for python bindings
* [Boost.Python](http://www.boost.org/doc/libs/1_63_0/libs/python/doc/html/index.html)
* [eigenpy](https://github.com/stack-of-tasks/eigenpy)
* Additionally you will need to activate the python bindings for the above libraries
## Installation on ubuntu-14.04 64 bit
Once the required libraries are installed you can clone this repository using ssh:
```
git clone --recursive git@gitlab.com:stonneau/bezier_COM_traj.git $BEZIER_COM_DIR
```
or using http:
```
git clone --recursive https://gitlab.com/stonneau/bezier_COM_traj.git $BEZIER_COM_DIR
```
And you can build this library using CMake:
```
mkdir $BEZIER_COM_DIR/build
cd $BEZIER_COM_DIR/build
cmake -DCMAKE_INSTALL_PREFIX=${DEVEL_DIR}/install ..
make install
```
### Optional: Python bindings installation
To install the Python bindings, in the CMakeLists.txt file, first enable the BUILD_PYTHON_INTERFACE option:
```
OPTION (BUILD_PYTHON_INTERFACE "Build the python binding" ON)
```
Then rebuild the library:
```
cd $BEZIER_COM_DIR/build
cmake -DCMAKE_INSTALL_PREFIX=${DEVEL_DIR}/install ..
make install
```
The python bindings should then be accessible through the package bezier_com_traj.
To see all the possible uses, you can refer to the [test file](https://gitlab.com/stonneau/bezier_COM_traj/blob/master/python/test/binding_tests.py)
In spite of an exhaustive documentation, please refer to the C++ documentation, which mostly applies
to python.
## Python example : Zero step capturability
For the zero step capturability, we will first define a contact phase using the objects from centroidal_dynamics:
```
#importing the libraries of interest
import ndcurves # noqa - necessary to register ndcurves::bezier_curve
import numpy as np
from numpy import array
from hpp_centroidal_dynamics import Equilibrium, EquilibriumAlgorithm, SolverLP
from hpp_bezier_com_traj import (SOLVER_QUADPROG, ConstraintFlag, Constraints, ContactData, ProblemData,
computeCOMTraj, zeroStepCapturability)
# create an Equilibrium solver, for a robot of 54 kilos. We linearize the friction cone to four generating rays
eq = Equilibrium("test", 54., 4)
# Now define some contact points ...
P = array([[x, y, 0] for x in [-0.05, 0.05] for y in [-0.1, 0.1]])
#and normals
z = array([0., 0., 1.])
N = array([[0., 0., 1.]] * 4)
#setting contact positions and normals, as well as friction coefficient of 0.3
#EQUILIBRIUM_ALGORITHM_PP is the algorithm that will always be used for our problems
eq.setNewContacts(P, N, 0.3, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP)
```
Then, we will define the initial state of our robot:
```
#c0 is the initial center of mass position
c0 = array([0., 0., 1.])
#we set the inital speed dc0 to a rather slow 10 cm / s along the x axis
dc0 = array([0.1, 0., 0.])
l0 = array([0., 0., 0.])
```
And finally, some optimization parameters:
The total duration of the trajectory, as well as
the discretization step. If the discretization step is < 0,
then the continuous formulation is used
```
#trajectory duration of 1.2 seconds
T = 1.2
#continuous resolution of the trajectory
tstep = -1.
```
We can now solve the problem:
```
# the boolean value indicates whether to use or not angular momentum
result = zeroStepCapturability(eq, c0, dc0, l0, False, T, tstep)
print(result.success)
#True the problem was feasible, and a trajectory was successfully computed
```
The found centroidal trajectory is accessible from the returned object, only if the problem
was feasible
```
result.c_of_t # a bezier curve object describing the com trajectory
#We can check that the end velocity is indeed zero:
dc_of_t = result.c_of_t.compute_derivate(1) # computing first derivative
print(np.linalg.norm(dc_of_t(dc_of_t.max())))
# 0.0
```
refer to the [test file](https://gitlab.com/stonneau/bezier_COM_traj/blob/master/python/test/binding_tests.py) for more advanced problems, including kinematic constraints,
mutiple contact phases handling and angular momentum
cmake @ 29c0eb4e
Compare 7e56da01...29c0eb4e
Subproject commit 7e56da01a62eb83631ed263fa3057b15b37c0c8c
Subproject commit 29c0eb4e659304f44d55a0389e2749812d858659
cmake2/FindCDD.cmake deleted 100644 → 0
View file @ ff92675e
# - Try to find libcdd
# Once done this will define
# CDD_FOUND - System has CDD
# CDD_INCLUDE_DIRS - The CDD include directories
# CDD_LIBRARIES - The libraries needed to use CDD
# CDD_DEFINITIONS - Compiler switches required for using CDD
find_path(CDD_INCLUDE_DIR cdd/cdd.h
HINTS ${CDD_INCLUDEDIR} /usr/include
PATH_SUFFIXES CDD )
find_library(CDD_LIBRARY NAMES libcdd.so
HINTS ${CDD_LIBDIR} ${CDD_LIBRARY_DIRS} /usr/lib/libcdd.so )
set(CDD_LIBRARIES ${CDD_LIBRARY} )
set(CDD_INCLUDE_DIRS ${CDD_INCLUDE_DIR} )
include(FindPackageHandleStandardArgs)
# handle the QUIETLY and REQUIRED arguments and set CDD_FOUND to TRUE
# if all listed variables are TRUE
find_package_handle_standard_args(CDD DEFAULT_MSG
CDD_LIBRARY CDD_INCLUDE_DIR)
mark_as_advanced(CDD_INCLUDE_DIR CDD_LIBRARY )
cmake2/FindEigen2.cmake deleted 100644 → 0
View file @ ff92675e
# - Try to find Eigen2 lib
#
# This module supports requiring a minimum version, e.g. you can do
# find_package(Eigen2 2.0.3)
# to require version 2.0.3 to newer of Eigen2.
#
# Once done this will define
#
# EIGEN2_FOUND - system has eigen lib with correct version
# EIGEN2_INCLUDE_DIR - the eigen include directory
# EIGEN2_VERSION - eigen version
# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
# Redistribution and use is allowed according to the terms of the BSD license.
if(NOT Eigen2_FIND_VERSION)
if(NOT Eigen2_FIND_VERSION_MAJOR)
set(Eigen2_FIND_VERSION_MAJOR 2)
endif(NOT Eigen2_FIND_VERSION_MAJOR)
if(NOT Eigen2_FIND_VERSION_MINOR)
set(Eigen2_FIND_VERSION_MINOR 0)
endif(NOT Eigen2_FIND_VERSION_MINOR)
if(NOT Eigen2_FIND_VERSION_PATCH)
set(Eigen2_FIND_VERSION_PATCH 0)
endif(NOT Eigen2_FIND_VERSION_PATCH)
set(Eigen2_FIND_VERSION "${Eigen2_FIND_VERSION_MAJOR}.${Eigen2_FIND_VERSION_MINOR}.${Eigen2_FIND_VERSION_PATCH}")
endif(NOT Eigen2_FIND_VERSION)
macro(_eigen2_check_version)
file(READ "${EIGEN2_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen2_version_header)
string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen2_world_version_match "${_eigen2_version_header}")
set(EIGEN2_WORLD_VERSION "${CMAKE_MATCH_1}")
string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen2_major_version_match "${_eigen2_version_header}")
set(EIGEN2_MAJOR_VERSION "${CMAKE_MATCH_1}")
string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen2_minor_version_match "${_eigen2_version_header}")
set(EIGEN2_MINOR_VERSION "${CMAKE_MATCH_1}")
set(EIGEN2_VERSION ${EIGEN2_WORLD_VERSION}.${EIGEN2_MAJOR_VERSION}.${EIGEN2_MINOR_VERSION})
if((${EIGEN2_WORLD_VERSION} NOTEQUAL 2) OR (${EIGEN2_MAJOR_VERSION} GREATER 10) OR (${EIGEN2_VERSION} VERSION_LESS ${Eigen2_FIND_VERSION}))
set(EIGEN2_VERSION_OK FALSE)
else()
set(EIGEN2_VERSION_OK TRUE)
endif()
if(NOT EIGEN2_VERSION_OK)
message(STATUS "Eigen2 version ${EIGEN2_VERSION} found in ${EIGEN2_INCLUDE_DIR}, "
"but at least version ${Eigen2_FIND_VERSION} is required")
endif(NOT EIGEN2_VERSION_OK)
endmacro(_eigen2_check_version)
if (EIGEN2_INCLUDE_DIR)
# in cache already
_eigen2_check_version()
set(EIGEN2_FOUND ${EIGEN2_VERSION_OK})
else (EIGEN2_INCLUDE_DIR)
find_path(EIGEN2_INCLUDE_DIR NAMES Eigen/Core
PATHS
${INCLUDE_INSTALL_DIR}
${KDE4_INCLUDE_DIR}
PATH_SUFFIXES eigen2
)
if(EIGEN2_INCLUDE_DIR)
_eigen2_check_version()
endif(EIGEN2_INCLUDE_DIR)
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(Eigen2 DEFAULT_MSG EIGEN2_INCLUDE_DIR EIGEN2_VERSION_OK)
mark_as_advanced(EIGEN2_INCLUDE_DIR)
endif(EIGEN2_INCLUDE_DIR)
cmake2/FindEigen3.cmake deleted 100644 → 0
View file @ ff92675e
# - Try to find Eigen3 lib
#
# This module supports requiring a minimum version, e.g. you can do
# find_package(Eigen3 3.1.2)
# to require version 3.1.2 or newer of Eigen3.
#
# Once done this will define
#
# EIGEN3_FOUND - system has eigen lib with correct version
# EIGEN3_INCLUDE_DIR - the eigen include directory
# EIGEN3_VERSION - eigen version
# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
if(NOT Eigen3_FIND_VERSION)
if(NOT Eigen3_FIND_VERSION_MAJOR)
set(Eigen3_FIND_VERSION_MAJOR 2)
endif(NOT Eigen3_FIND_VERSION_MAJOR)
if(NOT Eigen3_FIND_VERSION_MINOR)
set(Eigen3_FIND_VERSION_MINOR 91)
endif(NOT Eigen3_FIND_VERSION_MINOR)
if(NOT Eigen3_FIND_VERSION_PATCH)
set(Eigen3_FIND_VERSION_PATCH 0)
endif(NOT Eigen3_FIND_VERSION_PATCH)
set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}")
endif(NOT Eigen3_FIND_VERSION)
macro(_eigen3_check_version)
file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")
string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}")
string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}")
set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
set(EIGEN3_VERSION_OK FALSE)
else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
set(EIGEN3_VERSION_OK TRUE)
endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
if(NOT EIGEN3_VERSION_OK)
message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, "
"but at least version ${Eigen3_FIND_VERSION} is required")
endif(NOT EIGEN3_VERSION_OK)
endmacro(_eigen3_check_version)
if (EIGEN3_INCLUDE_DIR)
# in cache already
_eigen3_check_version()
set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})
else (EIGEN3_INCLUDE_DIR)
find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
PATHS
${CMAKE_INSTALL_PREFIX}/include
${KDE4_INCLUDE_DIR}
PATH_SUFFIXES eigen3 eigen
)
if(EIGEN3_INCLUDE_DIR)
_eigen3_check_version()
endif(EIGEN3_INCLUDE_DIR)
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)
mark_as_advanced(EIGEN3_INCLUDE_DIR)
endif(EIGEN3_INCLUDE_DIR)
cmake2/FindSpline.cmake deleted 100644 → 0
View file @ ff92675e
# - Try to find spline
# Once done this will define
# SPLINE_FOUND - System has SPLINE
# SPLINE_INCLUDE_DIRS - The SPLINE include directories
# SPLINE_DEFINITIONS - Compiler switches required for using SPLINE
# /usr/include/coin, /usr/lib/libSPLINE.so
find_path(SPLINE_INCLUDE_DIR spline/bezier_curve.h
HINTS ${SPLINE_INCLUDEDIR}
PATH_SUFFIXES SPLINE )
#~ find_library(SPLINE_LIBRARY NAMES libSPLINE.so
#~ HINTS ${SPLINE_LIBDIR} ${SPLINE_LIBRARY_DIRS} )
set(SPLINE_INCLUDE_DIRS ${SPLINE_INCLUDE_DIR} )
include(FindPackageHandleStandardArgs)
# handle the QUIETLY and REQUIRED arguments and set CDD_FOUND to TRUE
# if all listed variables are TRUE
find_package_handle_standard_args(SPLINE DEFAULT_MSG
SPLINE_INCLUDE_DIR)
mark_as_advanced(SPLINE_INCLUDE_DIR)
cmake2/Findcentroidal-dynamics-lib.cmake deleted 100644 → 0
View file @ ff92675e
# - Try to find centroidal-dynamics-lib
# Once done this will define
# CDL_FOUND - System has CDL
# CDL_INCLUDE_DIRS - The CDL include directories
# CDL_LIBRARIES - The libraries needed to use CDL
# CDL_DEFINITIONS - Compiler switches required for using CDL
find_path(CDL_INCLUDE_DIR centroidal-dynamics-lib/centroidal_dynamics.hh
HINTS ${CDL_INCLUDEDIR} /usr/include
PATH_SUFFIXES CDL )
find_library(CDL_LIBRARY NAMES libcentroidal-dynamics-lib.so
HINTS ${CDL_LIBDIR} ${CDL_LIBRARY_DIRS} /usr/lib/libcentroidal-dynamics-lib.so )
set(CDL_LIBRARIES ${CDL_LIBRARY} )
set(CDL_INCLUDE_DIRS ${CDL_INCLUDE_DIR} )
include(FindPackageHandleStandardArgs)
# handle the QUIETLY and REQUIRED arguments and set CDL_FOUND to TRUE
# if all listed variables are TRUE
find_package_handle_standard_args(CDL DEFAULT_MSG
CDL_LIBRARY CDL_INCLUDE_DIR)
mark_as_advanced(CDL_INCLUDE_DIR CDL_LIBRARY )
flake.lock 0 → 100644
View file @ 02786ee6
{
"nodes": {
"flake-parts": {
"inputs": {
"nixpkgs-lib": [
"nixpkgs"
]
},
"locked": {
"lastModified": 1719877454,
"narHash": "sha256-g5N1yyOSsPNiOlFfkuI/wcUjmtah+nxdImJqrSATjOU=",
"owner": "hercules-ci",
"repo": "flake-parts",
"rev": "4e3583423212f9303aa1a6337f8dffb415920e4f",
"type": "github"
},
"original": {
"owner": "hercules-ci",
"repo": "flake-parts",
"type": "github"
}
},
"nixpkgs": {
"locked": {
"lastModified": 1727174734,
"narHash": "sha256-xa3TynMF5vaWonmTOg/Ejc1Fmo0GkQnCaVRVkBc3z2I=",
"owner": "gepetto",
"repo": "nixpkgs",
"rev": "0ad139a0e4372abc12320c8c92ee90e0e5e296e1",
"type": "github"
},
"original": {
"owner": "gepetto",
"repo": "nixpkgs",
"type": "github"
}
},
"root": {
"inputs": {
"flake-parts": "flake-parts",
"nixpkgs": "nixpkgs"
}
}
},
"root": "root",
"version": 7
}
flake.nix 0 → 100644
View file @ 02786ee6
{
description = "Multi contact trajectory generation for the COM using Bezier curves";
nixConfig = {
extra-substituters = [ "https://gepetto.cachix.org" ];
extra-trusted-public-keys = [ "gepetto.cachix.org-1:toswMl31VewC0jGkN6+gOelO2Yom0SOHzPwJMY2XiDY=" ];
};
inputs = {
nixpkgs.url = "github:gepetto/nixpkgs";
flake-parts = {
url = "github:hercules-ci/flake-parts";
inputs.nixpkgs-lib.follows = "nixpkgs";
};
};
outputs =
inputs:
inputs.flake-parts.lib.mkFlake { inherit inputs; } {
systems = [
"x86_64-linux"
"aarch64-linux"
"aarch64-darwin"
"x86_64-darwin"
];
perSystem =
{ pkgs, self', ... }:
{
devShells.default = pkgs.mkShell { inputsFrom = [ self'.packages.default ]; };
packages = {
default = self'.packages.hpp-bezier-com-traj;
hpp-bezier-com-traj = pkgs.hpp-bezier-com-traj.overrideAttrs (_: {
src = pkgs.lib.fileset.toSource {
root = ./.;
fileset = pkgs.lib.fileset.unions [
./CMakeLists.txt
./include
./package.xml
./python
./src
./tests
];
};
});
};
};
};
}
include/bezier-com-traj/common_solve_methods.hh deleted 100644 → 0
View file @ ff92675e
/*
* Copyright 2018, LAAS-CNRS
* Author: Steve Tonneau
*/
#ifndef BEZIER_COM_TRAJ_LIB_COMMON_SOLVE_H
#define BEZIER_COM_TRAJ_LIB_COMMON_SOLVE_H
#include <bezier-com-traj/config.hh>
#include <bezier-com-traj/data.hh>
#include <bezier-com-traj/waypoints/waypoints_definition.hh>
#include <Eigen/Dense>
namespace bezier_com_traj
{
/**
* @brief ComputeDiscretizedWaypoints Given the waypoints defining a bezier curve,
* computes a discretization of the curve
* @param wps original waypoints
* @param bernstein berstein polynoms for
* @param numSteps desired number of wayoints
* @return a vector of waypoint representing the discretization of the curve
*/
BEZIER_COM_TRAJ_DLLAPI std::vector<waypoint6_t> ComputeDiscretizedWaypoints(
const std::vector<waypoint6_t>& wps, const std::vector<spline::Bern<double> >& bernstein, int numSteps);
/**
* @brief compute6dControlPointInequalities Given linear and angular control waypoints,
* compute the inequality matrices A and b, A x <= b that constrain the desired control point x.
* @param cData data for the current contact phase
* @param wps waypoints or the linear part of the trajectory
* @param wpL waypoints or the angular part of the trajectory
* @param useAngMomentum whether the angular momentum is consider or equal to 0
* @param fail set to true if problem is found infeasible
* @return
*/
BEZIER_COM_TRAJ_DLLAPI std::pair<MatrixXX, VectorX> compute6dControlPointInequalities(
const ContactData& cData, const std::vector<waypoint6_t>& wps,
const std::vector<waypoint6_t>& wpL, const bool useAngMomentum, bool& fail);
/**
* @brief solve x' h x + 2 g' x, subject to A*x <= b using quadprog
* @param A Inequality matrix
* @param b Inequality vector
* @param H Cost matrix
* @param g cost Vector
* @return
*/
BEZIER_COM_TRAJ_DLLAPI ResultData solve(Cref_matrixXX A, Cref_vectorX b, Cref_matrixXX H,
Cref_vectorX g, Cref_vectorX initGuess);
/**
* @brief solve x' h x + 2 g' x, subject to A*x <= b using quadprog, with x of fixed dimension 3
* @param Ab Inequality matrix and vector
* @param Hg Cost matrix and vector
* @return
*/
BEZIER_COM_TRAJ_DLLAPI ResultData solve(const std::pair<MatrixXX, VectorX>& Ab,
const std::pair<MatrixXX, VectorX>& Hg, const Vector3& init);
template <typename Point>
BEZIER_COM_TRAJ_DLLAPI std::vector< std::pair<double,Point> > computeDiscretizedWaypoints
(const ProblemData& pData, double T,const T_time& timeArray);
template <typename Point>
BEZIER_COM_TRAJ_DLLAPI std::vector< std::pair<double,Point> > computeDiscretizedAccelerationWaypoints
(const ProblemData& pData, double T,const T_time& timeArray);
} // end namespace bezier_com_traj
#include "common_solve_methods.inl"
#endif
include/bezier-com-traj/common_solve_methods.inl deleted 100644 → 0
View file @ ff92675e
namespace bezier_com_traj
{
template <typename Point>
std::vector< std::pair<double,Point> > computeDiscretizedWaypoints
(const ProblemData& pData, double T,const T_time& timeArray)
{
typedef std::pair<double,Point> coefs_t;
std::vector<coefs_t> wps;
std::vector<Point> pi = computeConstantWaypoints(pData,T);
// evaluate curve work with normalized time !
for (CIT_time cit = timeArray.begin(); cit != timeArray.end(); ++cit)
{
double t = std::min(cit->first / T, 1.);
wps.push_back(evaluateCurveAtTime(pData,pi,t));
}
return wps;
}
template <typename Point>
std::vector< std::pair<double,Point> > computeDiscretizedAccelerationWaypoints
(const ProblemData& pData, double T,const T_time& timeArray)
{
typedef std::pair<double,Point> coefs_t;
std::vector<coefs_t> wps;
std::vector<Point> pi = computeConstantWaypoints(pData,T);
// evaluate curve work with normalized time !
for (CIT_time cit = timeArray.begin(); cit != timeArray.end(); ++cit)
{
double t = std::min(cit->first / T, 1.);
wps.push_back(evaluateAccelerationCurveAtTime(pData,pi,T,t));
}
return wps;
}
} // end namespace bezier_com_traj
include/bezier-com-traj/data.hh deleted 100644 → 0
View file @ ff92675e
/*
* Copyright 2018, LAAS-CNRS
* Author: Steve Tonneau
*/
#ifndef BEZIER_COM_TRAJ_LIB_DATA_H
#define BEZIER_COM_TRAJ_LIB_DATA_H
#include <bezier-com-traj/config.hh>
#include <bezier-com-traj/flags.hh>
#include <bezier-com-traj/definitions.hh>
#include <bezier-com-traj/utils.hh>
#include <spline/bezier_curve.h>
#include <centroidal-dynamics-lib/centroidal_dynamics.hh>
#include <Eigen/Dense>
#include <vector>
namespace bezier_com_traj
{
/**
* @brief Contact data contains all the contact information
* relative to a contact phase: contact points and normals
* (within Equilibrium object), as well as any additional
* kinematic and angular constraints.
*/
struct BEZIER_COM_TRAJ_DLLAPI ContactData
{
ContactData()
: contactPhase_(0)
, Kin_(Eigen::Matrix3d::Zero())
, kin_(VectorX::Zero(0))
, Ang_(Eigen::Matrix3d::Zero())
, ang_(VectorX::Zero(0)) {}
~ContactData(){}
centroidal_dynamics::Equilibrium* contactPhase_;
MatrixX3 Kin_; // inequality kinematic constraints
VectorX kin_;
MatrixX3 Ang_; // inequality angular momentum constraints
VectorX ang_;
};
/**
* @brief Used to define the constraints on the trajectory generation problem.
* Flags are used to constrain initial and terminal com positions an derivatives.
* Additionally, the maximum acceleration can be bounded.
*/
struct BEZIER_COM_TRAJ_DLLAPI Constraints
{
Constraints()
: flag_(INIT_POS | INIT_VEL | END_VEL | END_POS)
, constraintAcceleration_(true)
, maxAcceleration_(5.)
, reduce_h_(1e-4) {}
Constraints(ConstraintFlag flag)
: flag_(flag)
, constraintAcceleration_(true)
, maxAcceleration_(5.)
, reduce_h_(1e-4) {}
~Constraints(){}
ConstraintFlag flag_;
bool constraintAcceleration_;
double maxAcceleration_;
double reduce_h_;
};
/**
* @brief Defines all the inputs of the problem:
* Initial and terminal constraints, as well as selected
* cost functions. Also,a list of ContactData defines the
* different phases of the problem. While the method
* can handle any phase greater than one, using more
* than three phases is probably too constraining.
*/
struct BEZIER_COM_TRAJ_DLLAPI ProblemData
{
ProblemData()
: c0_ (point_t::Zero())
,dc0_ (point_t::Zero())
,ddc0_(point_t::Zero())
, c1_ (point_t::Zero())
,dc1_ (point_t::Zero())
,ddc1_(point_t::Zero())
,useAngularMomentum_(false)
,costFunction_(ACCELERATION) {}
std::vector<ContactData> contacts_;
point_t c0_,dc0_,ddc0_,c1_,dc1_,ddc1_;
point_t l0_;
bool useAngularMomentum_;
Constraints constraints_;
CostFunction costFunction_;
};
/**
* @brief Struct used to return the results of the trajectory generation
* problem.
*/
struct BEZIER_COM_TRAJ_DLLAPI ResultData
{
ResultData():
success_(false)
, cost_(-1.)
, x(VectorX::Zero(0)){}
ResultData(const bool success, const double cost, Cref_vectorX x ):
success_(success)
, cost_(cost)
, x(x){}
ResultData(const ResultData& other):
success_(other.success_)
, cost_(other.cost_)
, x(other.x){}
~ResultData(){}
ResultData& operator=(const ResultData& other)
{
success_= (other.success_);
cost_ = (other.cost_);
x = (other.x);
return *this;
}
bool success_; // whether the optimization was successful
double cost_; // cost evaluation for the solved control point
VectorX x; //control point
};
/**
* @brief Specialized ResultData that computes the Bezier curves
* corresponding to the computed trajectory
*/
struct BEZIER_COM_TRAJ_DLLAPI ResultDataCOMTraj : public ResultData
{
ResultDataCOMTraj():
ResultData()
, c_of_t_(bezier_t::zero())
, dL_of_t_(bezier_t::zero())
, dc1_(point_t::Zero())
, ddc1_(point_t::Zero()) {}
~ResultDataCOMTraj(){}
bezier_t c_of_t_; // center of mass trajectory
bezier_t dL_of_t_; // angular momentum derivative trajectory
point_t dc1_; // terminal velocity
point_t ddc1_; //terminal acceleration
};
} // end namespace bezier_com_traj
#endif
include/bezier-com-traj/flags.hh deleted 100644 → 0
View file @ ff92675e
/*
* Copyright 2018, LAAS-CNRS
* Author: Steve Tonneau
*/
#ifndef BEZIER_COM_TRAJ_FLAGS_H
#define BEZIER_COM_TRAJ_FLAGS_H
#include <bezier-com-traj/config.hh>
namespace bezier_com_traj
{
enum BEZIER_COM_TRAJ_DLLAPI CostFunction{
ACCELERATION = 0x00001,
DISTANCE_TRAVELED = 0x00002,
TARGET_END_VELOCITY = 0x00004,
UNKNOWN_COST = 0x00008
};
enum BEZIER_COM_TRAJ_DLLAPI ConstraintFlag{
INIT_POS = 0x00001,
INIT_VEL = 0x00002,
INIT_ACC = 0x00004,
END_POS = 0x00008,
END_VEL = 0x00010,
END_ACC = 0x00020,
UNKNOWN = 0x00040
};
inline ConstraintFlag operator~(ConstraintFlag a)
{return static_cast<ConstraintFlag>(~static_cast<const int>(a));}
inline ConstraintFlag operator|(ConstraintFlag a, ConstraintFlag b)
{return static_cast<ConstraintFlag>(static_cast<const int>(a) | static_cast<const int>(b));}
inline ConstraintFlag operator&(ConstraintFlag a, ConstraintFlag b)
{return static_cast<ConstraintFlag>(static_cast<const int>(a) & static_cast<const int>(b));}
inline ConstraintFlag operator^(ConstraintFlag a, ConstraintFlag b)
{return static_cast<ConstraintFlag>(static_cast<const int>(a) ^ static_cast<const int>(b));}
inline ConstraintFlag& operator|=(ConstraintFlag& a, ConstraintFlag b)
{return (ConstraintFlag&)((int&)(a) |= static_cast<const int>(b));}
inline ConstraintFlag& operator&=(ConstraintFlag& a, ConstraintFlag b)
{return (ConstraintFlag&)((int&)(a) &= static_cast<const int>(b));}
inline ConstraintFlag& operator^=(ConstraintFlag& a, ConstraintFlag b)
{return (ConstraintFlag&)((int&)(a) ^= static_cast<const int>(b));}
} // end namespace bezier_com_traj
#endif