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 (316)
216 additional commits have been omitted to prevent performance issues.
Hide whitespace changes
Inline Side-by-side
Showing
with 490 additions and 1034 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)
INCLUDE(cmake2/FindqpOASES.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(LIBRARY_OUTPUT_PATH "${PROJECT_SOURCE_DIR}/lib/")
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${PROJECT_SOURCE_DIR}/bin/")
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()
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" OFF)
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)
# Declare Headers
SET(${PROJECT_NAME}_HEADERS
include/bezier-com-traj/data.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/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()
#SEARCH_FOR_QPOASES()
ADD_REQUIRED_DEPENDENCY("qpOASES")
add_subdirectory (src)
add_subdirectory (test)
IF(BUILD_PYTHON_INTERFACE)
add_subdirectory (python)
ENDIF(BUILD_PYTHON_INTERFACE)
SETUP_PROJECT_FINALIZE()
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(tests)
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 @ 77f875ef
# - 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/FindCLP.cmake deleted 100644 → 0
View file @ 77f875ef
# - Try to find libcdd
# Once done this will define
# CLP_FOUND - System has CLP
# CLP_INCLUDE_DIRS - The CLP include directories
# CLP_LIBRARIES - The libraries needed to use CLP
# CLP_DEFINITIONS - Compiler switches required for using CLP
# /usr/include/coin, /usr/lib/libClp.so
find_path(CLP_INCLUDE_DIR coin/ClpSimplex.hpp
HINTS ${CLP_INCLUDEDIR}
PATH_SUFFIXES CLP )
find_library(CLP_LIBRARY NAMES libclp.so
HINTS ${CLP_LIBDIR} ${CLP_LIBRARY_DIRS} )
set(CLP_LIBRARIES ${CLP_LIBRARY} )
set(CLP_INCLUDE_DIRS ${CLP_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(CLP DEFAULT_MSG
CLP_LIBRARY CLP_INCLUDE_DIR)
mark_as_advanced(CLP_INCLUDE_DIR CLP_LIBRARY )
cmake2/FindEigen2.cmake deleted 100644 → 0
View file @ 77f875ef
# - 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 @ 77f875ef
# - 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/FindqpOASES.cmake deleted 100644 → 0
View file @ 77f875ef
#.rst:
# FindqpOASES
# -----------
#
# Try to find the qpOASES library.
# Once done this will define the following variables::
#
# qpOASES_FOUND - System has qpOASES
# qpOASES_INCLUDE_DIR - qpOASES include directory
# qpOASES_LIBRARY - qpOASES libraries
#
# qpOASES does not have an "install" step, and the includes are in the source
# tree, while the libraries are in the build tree.
# Therefore the environment and cmake variables `qpOASES_SOURCE_DIR` and
# `qpOASES_BINARY_DIR` will be used to locate the includes and libraries.
#=============================================================================
# Copyright 2014 iCub Facility, Istituto Italiano di Tecnologia
# Authors: Daniele E. Domenichelli <daniele.domenichelli@iit.it>
#
# Distributed under the OSI-approved BSD License (the "License");
# see accompanying file Copyright.txt for details.
#
# This software is distributed WITHOUT ANY WARRANTY; without even the
# implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
# See the License for more information.
#=============================================================================
# (To distribute this file outside of YCM, substitute the full
# License text for the above reference.)
MACRO(SEARCH_FOR_QPOASES)
include(FindPackageHandleStandardArgs)
find_path(qpOASES_INCLUDEDIR
NAMES qpOASES.hpp
HINTS "${qpOASES_SOURCE_DIR}"
ENV qpOASES_SOURCE_DIR
PATH_SUFFIXES include)
find_library(qpOASES_LIB
NAMES qpOASES
HINTS "${qpOASES_BINARY_DIR}"
ENV qpOASES_BINARY_DIR
PATH_SUFFIXES lib
libs)
set(qpOASES_INCLUDE_DIR ${qpOASES_INCLUDEDIR})
set(qpOASES_LIBRARY ${qpOASES_LIB})
find_package_handle_standard_args(qpOASES DEFAULT_MSG qpOASES_LIBRARY
qpOASES_INCLUDE_DIR)
set(qpOASES_FOUND ${QPOASES_FOUND})
ENDMACRO(SEARCH_FOR_QPOASES)
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 @ 77f875ef
/*
* Copyright 2015, LAAS-CNRS
* Author: Andrea Del Prete
*/
#ifndef BEZIER_COM_TRAJ_LIB_COMMON_SOLVE_H
#define BEZIER_COM_TRAJ_LIB_COMMON_SOLVE_H
#include <Eigen/Dense>
#include <bezier-com-traj/config.hh>
#include <bezier-com-traj/data.hh>
namespace bezier_com_traj
{
typedef Matrix63 matrix6_t;
typedef Vector6 point6_t;
typedef Matrix3 matrix3_t;
typedef Vector3 point3_t;
/**
* @brief waypoint_t a waypoint is composed of a 6*3 matrix that depend
* on the variable x, and of a 6d vector independent of x, such that
* each control point of the target bezier curve is given by pi = wix * x + wis
*/
typedef std::pair<matrix6_t, point6_t> waypoint6_t;
typedef std::pair<matrix3_t, point3_t> waypoint3_t;
typedef const Eigen::Ref<const point_t>& point_t_tC;
typedef spline::bezier_curve <double, double, 6, true, point6_t> bezier6_t;
BEZIER_COM_TRAJ_DLLAPI Matrix3 skew(point_t_tC x);
template<typename T> T initwp();
int Normalize(Ref_matrixXX A, Ref_vectorX b);
/**
* @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);
} // end namespace bezier_com_traj
#endif
include/bezier-com-traj/data.hh deleted 100644 → 0
View file @ 77f875ef
/*
* Copyright 2015, LAAS-CNRS
* Author: Andrea Del Prete
*/
#ifndef BEZIER_COM_TRAJ_LIB_DATA_H
#define BEZIER_COM_TRAJ_LIB_DATA_H
#include <Eigen/Dense>
#include <bezier-com-traj/config.hh>
#include <spline/bezier_curve.h>
#include <centroidal-dynamics-lib/centroidal_dynamics.hh>
#include <vector>
namespace bezier_com_traj
{
typedef double value_type;
typedef Eigen::Matrix <value_type, 3, 3> Matrix3;
typedef Eigen::Matrix <value_type, 6, 3> Matrix63;
typedef Eigen::Matrix <value_type, Eigen::Dynamic, 3> MatrixX3;
typedef Eigen::Matrix <value_type, Eigen::Dynamic, Eigen::Dynamic> MatrixXX;
typedef centroidal_dynamics::Vector3 Vector3;
typedef centroidal_dynamics::Vector6 Vector6;
typedef centroidal_dynamics::VectorX VectorX;
typedef Eigen::Ref<Vector3> Ref_vector3;
typedef Eigen::Ref<VectorX> Ref_vectorX;
typedef Eigen::Ref<MatrixX3> Ref_matrixX3;
typedef Eigen::Ref<MatrixXX> Ref_matrixXX;
typedef const Eigen::Ref<const Vector3> & Cref_vector3;
typedef const Eigen::Ref<const Vector6> & Cref_vector6;
typedef const Eigen::Ref<const VectorX> & Cref_vectorX;
typedef const Eigen::Ref<const MatrixXX> & Cref_matrixXX;
typedef const Eigen::Ref<const MatrixX3> & Cref_matrixX3;
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_;
VectorX kin_;
MatrixX3 Ang_;
VectorX ang_;
};
struct BEZIER_COM_TRAJ_DLLAPI ProblemData
{
ProblemData()
: c0_(Vector3::Zero())
,dc0_(Vector3::Zero())
,ddc0_(Vector3::Zero())
, c1_(Vector3::Zero())
,dc1_(Vector3::Zero())
,ddc1_(Vector3::Zero())
,useAngularMomentum_(false) {}
std::vector<ContactData> contacts_;
Vector3 c0_,dc0_,ddc0_,c1_,dc1_,ddc1_;
Vector3 l0_;
bool useAngularMomentum_;
};
typedef Eigen::Vector3d point_t;
typedef spline::bezier_curve <double, double, 3, true, point_t > bezier_t;
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);
}
bool success_;
double cost_;
VectorX x;
};
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_;
bezier_t dL_of_t_;
point_t dc1_;
point_t ddc1_;
};
} // end namespace bezier_com_traj
#endif
include/bezier-com-traj/solve.hh deleted 100644 → 0
View file @ 77f875ef
/*
* Copyright 2015, LAAS-CNRS
* Author: Andrea Del Prete
*/
#ifndef BEZIER_COM_TRAJ_LIB_SOLVE_H
#define BEZIER_COM_TRAJ_LIB_SOLVE_H
#include <bezier-com-traj/config.hh>
#include <bezier-com-traj/data.hh>
#include <Eigen/Dense>
namespace bezier_com_traj
{
/**
* @brief solve0step Tries to solve the 0-step capturability problem. Given the current contact phase,
* a COM position, and an initial velocity, tries to compute a feasible COM trajectory that
* stops the character without falling.
* @param pData problem Data. Should contain only one contact phase.
* @param Ts timelength of each contact phase. Should only contain one value
* @param timeStep time that the solver has to stop.
* @return ResultData a struct containing the resulting trajectory, if success is true.
*/
BEZIER_COM_TRAJ_DLLAPI ResultDataCOMTraj solve0step(const ProblemData& pData, const std::vector<double>& Ts, const double timeStep = -1);
std::vector<spline::Bern<double> > ComputeBersteinPolynoms(int degree);
/// Methods for transition test :
/**
* @brief solveIntersection Solve the QP problem, that find a point inside the constraints Ab that minimise the cost Hg
* @param Ab s.t. Ax <= b
* @param Hg min x'Hx + g'x
* @param init x_init
* @return ResultData
*/
BEZIER_COM_TRAJ_DLLAPI ResultData solveIntersection(const std::pair<MatrixXX, VectorX>& Ab,const std::pair<MatrixXX, VectorX>& Hg, const Vector3& init);
/**
* @brief solveOnestep Tries to solve the one step problem : Given two contact phases, an initial and final com position and velocity,
* try to compute the CoM trajectory (as a Bezier curve) that connect them
* @param pData problem Data. Should contain only two contact phase.
* @param Ts timelength of each contact phase. Should only contain two value
* @param timeStep time step used by the discretization
* @return ResultData a struct containing the resulting trajectory, if success is true.
*/
BEZIER_COM_TRAJ_DLLAPI ResultDataCOMTraj solveOnestep(const ProblemData& pData, const VectorX& Ts, const Vector3& init_guess,const int pointsPerPhase = 3 ,const double feasability_treshold = 0.);
} // end namespace bezier_com_traj
#endif
include/bezier-com-traj/solve_end_effector.hh deleted 100644 → 0
View file @ 77f875ef
/*
* Copyright 2017, LAAS-CNRS
* Author: Pierre Fernbach
*/
#include <bezier-com-traj/solve.hh>
#include <bezier-com-traj/common_solve_methods.hh>
#include <limits>
using namespace bezier_com_traj;
namespace bezier_com_traj
{
typedef waypoint3_t waypoint_t;
typedef std::pair<double,point3_t> coefs_t;
const int DIM_POINT=3;
const int NUM_DISCRETIZATION = 11;
/**
* @brief solveEndEffector Tries to produce a trajectory represented as a bezier curve
* that satisfy position, velocity and acceleration constraint for the initial and final point
* and that follow as close as possible the input trajectory
* @param pData problem Data.
* @param path the path to follow, the class Path must implement the operator (double t) , t \in [0,1] return a Vector3
* that give the position on the path for a given time
* @param T time lenght of the trajectory
* @param timeStep time that the solver has to stop
* @return ResultData a struct containing the resulting trajectory, if success is true.
*/
template<typename Path>
ResultDataCOMTraj solveEndEffector(const ProblemData& pData,const Path& path, const double T, const double timeStep);
coefs_t initCoefs(){
coefs_t c;
c.first=0;
c.second=point3_t::Zero();
return c;
}
void computeConstantWaypoints(const ProblemData& pData,double T,double n,point_t& p0,point_t& p1, point_t& p2, point_t& p3, point_t& p5, point_t& p6,point_t& p7,point_t& p8){
p0 = pData.c0_;
p1 = (pData.dc0_ * T / n )+ pData.c0_;
p2 = (pData.ddc0_*T*T/(n*(n-1))) + (2*pData.dc0_ *T / n) + pData.c0_; // * T because derivation make a T appear
p3 = (3*pData.ddc0_*T*T/(n*(n-1))) + (3*pData.dc0_ *T / n) + pData.c0_;
p8 = pData.c1_;
p7 = (-pData.dc1_ * T / n) + pData.c1_; // * T ?
p6 = (pData.ddc1_ *T*T / (n*(n-1))) - (2 * pData.dc1_ *T / n) + pData.c1_ ; // * T ??
p5 = (3*pData.ddc1_ *T*T / (n*(n-1))) - (3 * pData.dc1_ *T / n) + pData.c1_ ; // * T ??
}
std::vector<bezier_t::point_t> computeConstantWaypoints(const ProblemData& pData,double T,double n){
point_t p0,p1,p2,p3,p4,p5,p6,p7;
computeConstantWaypoints(pData,T,n,p0,p1,p2,p3,p4,p5,p6,p7);
std::vector<bezier_t::point_t> pts;
pts.push_back(p0);
pts.push_back(p1);
pts.push_back(p2);
pts.push_back(p3);
pts.push_back(p4);
pts.push_back(p5);
pts.push_back(p6);
pts.push_back(p7);
return pts;
}
std::vector<waypoint_t> createEndEffectorAccelerationWaypoints(double T,const ProblemData& pData){
// create the waypoint from the analytical expressions :
std::vector<waypoint_t> wps;
point_t p0,p1,p2,p3,p5,p6,p7,p8;
computeConstantWaypoints(pData,T,8,p0,p1,p2,p3,p5,p6,p7,p8);
std::cout<<"Create end eff waypoints, constant waypoints = :"<<std::endl<<
"p0 = "<<p0.transpose()<<std::endl<<"p1 = "<<p1.transpose()<<std::endl<<"p2 = "<<p2.transpose()<<std::endl<<
"p3 = "<<p3.transpose()<<std::endl<<"p5 = "<<p5.transpose()<<std::endl<<"p6 = "<<p6.transpose()<<std::endl<<"p7 = "<<p7.transpose()<<"p8 = "<<p8.transpose()<<std::endl;
double alpha = 1. / (T*T);
waypoint_t w = initwp<waypoint_t>();
// assign w0:
w.second= 56*alpha*(p0 - 2*p1 + p2);
wps.push_back(w);
w = initwp<waypoint_t>();
// assign w1:
w.second= 56*alpha*(p1 - 2*p2 + p3);
wps.push_back(w);
w = initwp<waypoint_t>();
// assign w2:
w.first = 56*alpha*Matrix3::Identity();
w.second = (56*p2 - 112*p3)*alpha;
wps.push_back(w);
w = initwp<waypoint_t>();
// assign w3:
w.first = -112*alpha*Matrix3::Identity();
w.second = (56*p3 +56*p8)*alpha;
wps.push_back(w);
w = initwp<waypoint_t>();
// assign w4:
w.first = 56*alpha*Matrix3::Identity();
w.second = (-112*p5 + 56*p6)*alpha;
wps.push_back(w);
w = initwp<waypoint_t>();
// assign w5:
w.second=56*alpha*(p5-2*p6+p7);
wps.push_back(w);
w = initwp<waypoint_t>();
// assign w5:
w.second=56*alpha*(p6-2*p7+p8);
wps.push_back(w);
return wps;
}
std::vector<waypoint_t> createEndEffectorVelocityWaypoints(double T,const ProblemData& pData){
// create the waypoint from the analytical expressions :
std::vector<waypoint_t> wps;
point_t p0,p1,p2,p3,p5,p6,p7,p8;
computeConstantWaypoints(pData,T,8,p0,p1,p2,p3,p5,p6,p7,p8);
/* std::cout<<"Create end eff waypoints, constant waypoints = :"<<std::endl<<
"p0 = "<<p0.transpose()<<std::endl<<"p1 = "<<p1.transpose()<<std::endl<<"p2 = "<<p2.transpose()<<std::endl<<
"p4 = "<<p4.transpose()<<std::endl<<"p5 = "<<p5.transpose()<<std::endl<<"p6 = "<<p6.transpose()<<std::endl;*/
double alpha = 1. / (T);
waypoint_t w = initwp<waypoint_t>();
// assign w0:
w.second= alpha*8*(-p0+p1);
wps.push_back(w);
w = initwp<waypoint_t>();
// assign w1:
w.second = alpha*8*(-p1+p2);
wps.push_back(w);
w = initwp<waypoint_t>();
// assign w2:
w.second = alpha*8*(-p2+p3);
wps.push_back(w);
w = initwp<waypoint_t>();
// assign w3:
w.first = 8*alpha*Matrix3::Identity();
w.second = alpha*-8*p3;
wps.push_back(w);
w = initwp<waypoint_t>();
// assign w4:
w.first = -8*alpha*Matrix3::Identity();
w.second = alpha*8*p5;
wps.push_back(w);
w = initwp<waypoint_t>();
// assign w5:
w.second=alpha*8*(-p5+p6);
wps.push_back(w);
w = initwp<waypoint_t>();
// assign w6:
w.second=alpha*8*(-p6+p7);
wps.push_back(w);
w = initwp<waypoint_t>();
// assign w7:
w.second=alpha*8*(-p7+p8);
wps.push_back(w);
return wps;
}
void computeConstraintsMatrix(const ProblemData& pData,const std::vector<waypoint_t>& wps_acc,const std::vector<waypoint_t>& wps_vel,const VectorX& acc_bounds,const VectorX& vel_bounds,MatrixXX& A,VectorX& b){
assert(acc_bounds.size() == DIM_POINT && "Acceleration bounds should have the same dimension as the points");
assert(vel_bounds.size() == DIM_POINT && "Velocity bounds should have the same dimension as the points");
int empty_acc=0;
int empty_vel=0;
for (std::vector<waypoint_t>::const_iterator wpcit = wps_acc.begin(); wpcit != wps_acc.end(); ++wpcit)
{
if(wpcit->first.isZero(std::numeric_limits<double>::epsilon()))
empty_acc++;
}
for (std::vector<waypoint_t>::const_iterator wpcit = wps_vel.begin(); wpcit != wps_vel.end(); ++wpcit)
{
if(wpcit->first.isZero(std::numeric_limits<double>::epsilon()))
empty_vel++;
}
A = MatrixXX::Zero(2*DIM_POINT*(wps_acc.size()-empty_acc+wps_vel.size()-empty_vel)+DIM_POINT,DIM_POINT); // *2 because we have to put the lower and upper bound for each one, +DIM_POINT for the constraint on x[z]
b = VectorX::Zero(A.rows());
int i = 0;
//upper acc bounds
for (std::vector<waypoint_t>::const_iterator wpcit = wps_acc.begin(); wpcit != wps_acc.end(); ++wpcit)
{
if(! wpcit->first.isZero(std::numeric_limits<double>::epsilon())){
A.block<DIM_POINT,DIM_POINT>(i*DIM_POINT,0) = wpcit->first;
b.segment<DIM_POINT>(i*DIM_POINT) = acc_bounds - wpcit->second;
++i;
}
}
//lower acc bounds
for (std::vector<waypoint_t>::const_iterator wpcit = wps_acc.begin(); wpcit != wps_acc.end(); ++wpcit)
{
if(! wpcit->first.isZero(std::numeric_limits<double>::epsilon())){
A.block<DIM_POINT,DIM_POINT>(i*DIM_POINT,0) = -wpcit->first;
b.segment<DIM_POINT>(i*DIM_POINT) = acc_bounds + wpcit->second;
++i;
}
}
//upper velocity bounds
for (std::vector<waypoint_t>::const_iterator wpcit = wps_vel.begin(); wpcit != wps_vel.end(); ++wpcit)
{
if(! wpcit->first.isZero(std::numeric_limits<double>::epsilon())){
A.block<DIM_POINT,DIM_POINT>(i*DIM_POINT,0) = wpcit->first;
b.segment<DIM_POINT>(i*DIM_POINT) = vel_bounds - wpcit->second;
++i;
}
}
//lower velocity bounds
for (std::vector<waypoint_t>::const_iterator wpcit = wps_vel.begin(); wpcit != wps_vel.end(); ++wpcit)
{
if(! wpcit->first.isZero(std::numeric_limits<double>::epsilon())){
A.block<DIM_POINT,DIM_POINT>(i*DIM_POINT,0) = -wpcit->first;
b.segment<DIM_POINT>(i*DIM_POINT) = vel_bounds + wpcit->second;
++i;
}
}
// test : constraint x[z] to be always higher than init[z] and goal[z].
// TODO replace z with the direction of the contact normal ... need to change the API
MatrixXX mxz = MatrixXX::Zero(DIM_POINT,DIM_POINT);
mxz(DIM_POINT-1,DIM_POINT-1) = -1;
VectorX nxz = VectorX::Zero(DIM_POINT);
nxz[2]= - std::min(pData.c0_[2],pData.c1_[2]);
A.block<DIM_POINT,DIM_POINT>(i*DIM_POINT,0) = mxz;
b.segment<DIM_POINT>(i*DIM_POINT) = nxz;
//TEST :
/* A.block<DIM_POINT,DIM_POINT>(i*DIM_POINT,0) = Matrix3::Identity();
b.segment<DIM_POINT>(i*DIM_POINT) = Vector3(10,10,10);
i++;
A.block<DIM_POINT,DIM_POINT>(i*DIM_POINT,0) = -Matrix3::Identity();
b.segment<DIM_POINT>(i*DIM_POINT) = Vector3(10,10,10);*/
}
/*std::vector<coefs_t> createDiscretizationPoints(const ProblemData& pData){
// equation found with sympy (for 11 points)
std::vector<coefs_t> cks; // one element for each discretization points :
//.first is the term that depend on x and . second is the constant term
coefs_t ck = initCoefs();
// 0
ck.first = 0.;
ck.second = pData.c0_;
cks.push_back(ck);
ck = initCoefs();
// 1
ck.first = 729./50000.;
ck.second = 19683.*pData.c0_/20000. + 127.*pData.c1_/100000. + 45927.*pData.dc0_/500000. - 207.*pData.dc1_/500000. + 6561.*pData.ddc0_/2000000. + 81.*pData.ddc1_/2000000.;
cks.push_back(ck);
ck = initCoefs();
// 2
ck.first = 256./3125.;
ck.second = 2816.*pData.c0_/3125. + 53.*pData.c1_/3125. + 2304.*pData.dc0_/15625. - 84.*pData.dc1_/15625. + 128.*pData.ddc0_/15625. + 8.*pData.ddc1_/15625. ;
cks.push_back(ck);
ck = initCoefs();
// 3
ck.first = 9261./50000. ;
ck.second = 74431.*pData.c0_/100000. + 7047.*pData.c1_/100000. + 79233.*pData.dc0_/500000. - 10773.*pData.dc1_/500000. + 21609.*pData.ddc0_/2000000. + 3969.*pData.ddc1_/2000000.;
cks.push_back(ck);
ck = initCoefs();
// 4
ck.first = 864./3125.;
ck.second = 1701.*pData.c0_/3125. + 112.*pData.c1_/625.+ 2106.*pData.dc0_/15625. - 816.*pData.dc1_/15625. + 162.*pData.ddc0_/15625. + 72.*pData.ddc1_/15625.;
cks.push_back(ck);
ck = initCoefs();
// 5
ck.first = 5./16.;
ck.second = 11.*pData.c0_/32. + 11.*pData.c1_/32. + 3.*pData.dc0_/32. - 3.*pData.dc1_/32. + pData.ddc0_/128. + pData.ddc1_/128. ;
cks.push_back(ck);
ck = initCoefs();
// 6
ck.first = 864./3125.;
ck.second = 112.*pData.c0_/625. + 1701.*pData.c1_/3125. + 816.*pData.dc0_/15625. - 2106.*pData.dc1_/15625. + 72.*pData.ddc0_/15625. + 162.*pData.ddc1_/15625. ;
cks.push_back(ck);
ck = initCoefs();
// 7
ck.first = 9261./50000.;
ck.second = 704699999999999.*pData.c0_/10000000000000000. + 74431.*pData.c1_/100000. + 10773.*pData.dc0_/500000. - 79233.*pData.dc1_/500000. + 3969.*pData.ddc0_/2000000. + 21609.*pData.ddc1_/2000000.;
cks.push_back(ck);
ck = initCoefs();
// 8
ck.first = 256./3125.;
ck.second = 53.*pData.c0_/3125. + 2816.*pData.c1_/3125. + 84.*pData.dc0_/15625. - 2304.*pData.dc1_/15625. + 8.*pData.ddc0_/15625. + 128.*pData.ddc1_/15625.;
cks.push_back(ck);
ck = initCoefs();
// 9
ck.first = 729./50000.;
ck.second = 127.*pData.c0_/100000. + 19683.*pData.c1_/20000. + 207.*pData.dc0_/500000. - 45927.*pData.dc1_/500000. + 81.*pData.ddc0_/2000000. + 6561.*pData.ddc1_/2000000. ;
cks.push_back(ck);
ck = initCoefs();
// 10
ck.first = 0.;
ck.second = pData.c1_ ;
cks.push_back(ck);
return cks;
}*/
/**
* @brief evaluateCurve Evaluate the curve at a given parameter
* @param pData
* @param T
* @param t Normalized : between 0 and 1
* @return
*/
coefs_t evaluateCurve(const ProblemData& pData,double T, double t){
point_t p0,p1,p2,p3,p5,p6,p7,p8;
computeConstantWaypoints(pData,T,8,p0,p1,p2,p3,p5,p6,p7,p8);
coefs_t coefs;
coefs.first = 70.0*pow(t,8) - 280.0*pow(t,7) + 420.0*pow(t,6) - 280.0*pow(t,5) + 70.0*pow(t,4);
coefs.second = 1.0*p0*pow(t,8) - 8.0*p0*pow(t,7) + 28.0*p0*pow(t,6) - 56.0*p0*pow(t,5) + 70.0*p0*pow(t,4) - 56.0*p0*pow(t,3) + 28.0*p0*pow(t,2) - 8.0*p0*t + 1.0*p0 - 8.0*p1*pow(t,8) + 56.0*p1*pow(t,7) - 168.0*p1*pow(t,6) + 280.0*p1*pow(t,5) - 280.0*p1*pow(t,4) + 168.0*p1*pow(t,3) - 56.0*p1*pow(t,2) + 8.0*p1*t + 28.0*p2*pow(t,8) - 168.0*p2*pow(t,7) + 420.0*p2*pow(t,6) - 560.0*p2*pow(t,5) + 420.0*p2*pow(t,4) - 168.0*p2*pow(t,3) + 28.0*p2*pow(t,2) - 56.0*p3*pow(t,8 )+ 280.0*p3*pow(t,7) - 560.0*p3*pow(t,6) + 560.0*p3*pow(t,5) - 280.0*p3*pow(t,4) + 56.0*p3*pow(t,3) - 56.0*p5*pow(t,8) + 168.0*p5*pow(t,7) - 168.0*p5*pow(t,6) + 56.0*p5*pow(t,5 )+ 28.0*p6*pow(t,8) - 56.0*p6*pow(t,7) + 28.0*p6*pow(t,6) - 8.0*p7*pow(t,8) + 8.0*p7*pow(t,7) + 1.0*p8*pow(t,8);
return coefs;
}
/**
* @brief evaluateAccCurve Evaluate the acceleration at a given parameter
* @param pData
* @param T
* @param param Normalized : between 0 and 1
* @return
*/
coefs_t evaluateAccCurve(const ProblemData& pData, double T, double t){
point_t p0,p1,p2,p3,p5,p6,p7,p8;
computeConstantWaypoints(pData,T,8,p0,p1,p2,p3,p5,p6,p7,p8);
coefs_t coefs;
double alpha = 1./(T*T);
//equations found with sympy
coefs.first= ((3920.0*pow(t,6) - 11760.0*pow(t,5) + 12600.0*pow(t,4) - 5600.0*pow(t,3) + 840.0*pow(t,2)))*alpha;
coefs.second=(56.0*p0*pow(t,6) - 336.0*p0*pow(t,5) + 840.0*p0*pow(t,4) - 1120.0*p0*pow(t,3) + 840.0*p0*pow(t,2) - 336.0*p0*t + 56.0*p0 - 448.0*p1*pow(t,6) + 2352.0*p1*pow(t,5) - 5040.0*p1*pow(t,4) + 5600.0*p1*pow(t,3) - 3360.0*p1*pow(t,2) + 1008.0*p1*t - 112.0*p1 + 1568.0*p2*pow(t,6) - 7056.0*p2*pow(t,5) + 12600.0*p2*pow(t,4) - 11200.0*p2*pow(t,3) + 5040.0*p2*pow(t,2) - 1008.0*p2*t + 56.0*p2 - 3136.0*p3*pow(t,6) + 11760.0*p3*pow(t,5) - 16800.0*p3*pow(t,4) + 11200.0*p3*pow(t,3) - 3360.0*p3*pow(t,2)+ 336.0*p3*t - 3136.0*p5*pow(t,6) + 7056.0*p5*pow(t,5) - 5040.0*p5*pow(t,4) + 1120.0*p5*pow(t,3) + 1568.0*p6*pow(t,6) - 2352.0*p6*pow(t,5) + 840.0*p6*pow(t,4) - 448.0*p7*pow(t,6) + 336.0*p7*pow(t,5) + 56.0*p8*pow(t,6))*alpha;
return coefs;
}
template <typename Path>
void computeDistanceCostFunction(int numPoints,const ProblemData& pData, double T,const Path& path, MatrixXX& H,VectorX& g){
H = MatrixXX::Zero(DIM_POINT,DIM_POINT);
g = VectorX::Zero(DIM_POINT);
double step = 1./(numPoints-1);
std::vector<coefs_t> cks;
for(size_t i = 0 ; i < numPoints ; ++i){
cks.push_back(evaluateCurve(pData,T,i*step));
}
point3_t pk;
size_t i = 0;
for (std::vector<coefs_t>::const_iterator ckcit = cks.begin(); ckcit != cks.end(); ++ckcit){
pk=path(i*step);
// std::cout<<"pk = "<<pk.transpose()<<std::endl;
// std::cout<<"coef First : "<<ckcit->first<<std::endl;
// std::cout<<"coef second : "<<ckcit->second.transpose()<<std::endl;
H += (ckcit->first * ckcit->first * Matrix3::Identity());
g += (ckcit->first * ckcit->second) - (pk * ckcit->first);
i++;
}
}
void computeC_of_T (const ProblemData& pData,double T, ResultDataCOMTraj& res){
std::vector<Vector3> wps;
point_t p0,p1,p2,p3,p5,p6,p7,p8;
computeConstantWaypoints(pData,T,8,p0,p1,p2,p3,p5,p6,p7,p8);
wps.push_back(p0);
wps.push_back(p1);
wps.push_back(p2);
wps.push_back(p3);
wps.push_back(res.x);
wps.push_back(p5);
wps.push_back(p6);
wps.push_back(p7);
wps.push_back(p8);
res.c_of_t_ = bezier_t (wps.begin(), wps.end(),T);
std::cout<<"bezier curve created, size = "<<res.c_of_t_.size_<<std::endl;
}
void computeAccelerationCostFunction(int numPoints,const ProblemData& pData,double T, MatrixXX& H,VectorX& g){
double step = 1./(numPoints-1);
H = MatrixXX::Zero(DIM_POINT,DIM_POINT);
g = VectorX::Zero(DIM_POINT);
std::vector<coefs_t> cks;
for(int i = 0 ; i < numPoints ; ++i){
cks.push_back(evaluateAccCurve(pData,T,i*step));
}
for (std::vector<coefs_t>::const_iterator ckcit = cks.begin(); ckcit != cks.end(); ++ckcit){
H+=(ckcit->first * ckcit->first * Matrix3::Identity());
g+=ckcit->first*ckcit->second;
}
//TEST : don't consider z axis for minimum acceleration cost
H(2,2) = 1e-6;
g[2] = 1e-6 ;
//normalize :
// double norm=H(0,0); // because H is always diagonal
// H /= norm;
// g /= norm;
}
template <typename Path>
ResultDataCOMTraj solveEndEffector(const ProblemData& pData,const Path& path, const double T, const double weightDistance){
std::cout<<"solve end effector, T = "<<T<<std::endl;
assert (weightDistance>=0. && weightDistance<=1. && "WeightDistance must be between 0 and 1");
double weightAcc = 1. - weightDistance;
std::vector<waypoint_t> wps_acc=createEndEffectorAccelerationWaypoints(T,pData);
std::vector<waypoint_t> wps_vel=createEndEffectorVelocityWaypoints(T,pData);
// stack the constraint for each waypoint :
MatrixXX A;
VectorX b;
Vector3 acc_bounds(100,100,100);
Vector3 vel_bounds(50,50,50);
computeConstraintsMatrix(pData,wps_acc,wps_vel,acc_bounds,vel_bounds,A,b);
// std::cout<<"End eff A = "<<std::endl<<A<<std::endl;
// std::cout<<"End eff b = "<<std::endl<<b<<std::endl;
// compute cost function (discrete integral under the curve defined by 'path')
MatrixXX H_rrt,H_acc,H;
VectorX g_rrt,g_acc,g;
computeDistanceCostFunction<Path>(20,pData,T,path,H_rrt,g_rrt);
computeAccelerationCostFunction(50,pData,T,H_acc,g_acc);
/* std::cout<<"End eff H_rrt = "<<std::endl<<H_rrt<<std::endl;
std::cout<<"End eff g_rrt = "<<std::endl<<g_rrt<<std::endl;
std::cout<<"End eff H_acc = "<<std::endl<<H_acc<<std::endl;
std::cout<<"End eff g_acc = "<<std::endl<<g_acc<<std::endl;
*/
// add the costs :
H = MatrixXX::Zero(DIM_POINT,DIM_POINT);
g = VectorX::Zero(DIM_POINT);
H = weightAcc*H_acc + weightDistance*H_rrt;
g = weightAcc*g_acc + weightDistance*g_rrt;
std::cout<<"End eff H = "<<std::endl<<H<<std::endl;
std::cout<<"End eff g = "<<std::endl<<g<<std::endl;
// call the solver
VectorX init = VectorX(DIM_POINT);
init = (pData.c0_ + pData.c1_)/2.;
// init =pData.c0_;
std::cout<<"Init = "<<std::endl<<init.transpose()<<std::endl;
ResultData resQp = solve(A,b,H,g, init);
ResultDataCOMTraj res;
if(resQp.success_)
{
res.success_ = true;
res.x = resQp.x;
// computeRealCost(pData, res);
computeC_of_T (pData,T,res);
// computedL_of_T(pData,Ts,res);
}
std::cout<<"Solved, success = "<<res.success_<<" x = "<<res.x.transpose()<<std::endl;
std::cout<<"Final cost : "<<resQp.cost_<<std::endl;
return res;
}
}//namespace bezier_com_traj