├── .git-blame-ignore-revs ├── .gitignore ├── .gitlab-ci.yml ├── .gitmodules ├── .pre-commit-config.yaml ├── .travis.yml ├── CMakeLists.txt ├── README.md ├── include └── parametric-curves │ ├── MathDefs.h │ ├── abstract-curve.hpp │ ├── bernstein.h │ ├── bezier_curve.h │ ├── constant.hpp │ ├── curve-constraint.hpp │ ├── helpers │ ├── effector_spline.h │ └── effector_spline_rotation.h │ ├── infinite-const-acc.hpp │ ├── infinite-sinusoid.hpp │ ├── linear-chirp.hpp │ ├── minimum-jerk.hpp │ ├── optimization │ └── OptimizeSpline.h │ ├── polynomial.hpp │ ├── serialization │ ├── archive.hpp │ └── eigen-matrix.hpp │ ├── spatial │ └── force-curve.hpp │ ├── spline.hpp │ ├── text-file.hpp │ └── utils │ └── file-io.hpp ├── pyproject.toml ├── python ├── CMakeLists.txt ├── parametric_curves │ └── __init__.py └── spline_python.cpp ├── setup.cfg └── tests ├── CMakeLists.txt ├── Main.cpp └── python ├── CMakeLists.txt ├── quick.py └── test.py /.git-blame-ignore-revs: -------------------------------------------------------------------------------- 1 | # format (Guilhem Saurel, 2022-04-05) 2 | 7e4f60e57398fb63c6ee43d74cc4775e0c5bf435 3 | 4 | # Format: yapf 0.32 (Guilhem Saurel, 2022-02-07) 5 | f3e915be9dfc8132db9dc2dd65443bb80cef2e93 6 | 7 | # [Python] format (Guilhem Saurel, 2019-10-09) 8 | cba6436a362584053e9ebd4f10dcb641299d9017 9 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Build and Release Folders 2 | bin/ 3 | lib/ 4 | build* 5 | _build* 6 | 7 | # temp files 8 | .*~ 9 | *.user 10 | *~ 11 | *.pyc 12 | -------------------------------------------------------------------------------- /.gitlab-ci.yml: -------------------------------------------------------------------------------- 1 | include: http://rainboard.laas.fr/project/parametric-curves/.gitlab-ci.yml 2 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "cmake"] 2 | path = cmake 3 | url = https://github.com/jrl-umi3218/jrl-cmakemodules.git 4 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | ci: 2 | autoupdate_branch: 'devel' 3 | repos: 4 | - repo: https://github.com/pre-commit/mirrors-clang-format 5 | rev: v16.0.6 6 | hooks: 7 | - id: clang-format 8 | args: [--style=Google] 9 | - repo: https://github.com/pre-commit/pre-commit-hooks 10 | rev: v4.4.0 11 | hooks: 12 | - id: check-added-large-files 13 | - id: check-ast 14 | - id: check-executables-have-shebangs 15 | - id: check-json 16 | - id: check-merge-conflict 17 | - id: check-symlinks 18 | - id: check-toml 19 | - id: check-yaml 20 | - id: debug-statements 21 | - id: destroyed-symlinks 22 | - id: detect-private-key 23 | - id: end-of-file-fixer 24 | - id: fix-byte-order-marker 25 | - id: mixed-line-ending 26 | - id: trailing-whitespace 27 | - repo: https://github.com/psf/black 28 | rev: 23.3.0 29 | hooks: 30 | - id: black 31 | - repo: https://github.com/PyCQA/flake8 32 | rev: 6.0.0 33 | hooks: 34 | - id: flake8 35 | - repo: https://github.com/cheshirekow/cmake-format-precommit 36 | rev: v0.6.13 37 | hooks: 38 | - id: cmake-format 39 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: generic 2 | python: 3 | - "2.7" 4 | sudo: required 5 | dist: bionic 6 | compiler: 7 | - gcc 8 | # - clang 9 | env: 10 | global: 11 | - secure: "SnIBG/xLIHX3CSvUbqqsX8xTVqIqQ7fFS6HWO6KZQVBsT6yugTwYHbyhNiU531JejYJ/I3ZrDhXfYH3qFZiYxnH1sifvwV+fnTtMXpPN7qPZwIymkjcmm6gJF51e0C7VOfUbvKFv0ngwj+ul21rgZSMuoEvxPK0WxtE3/ZSfn9c=" 12 | - APT_DEPENDENCIES="doxygen libeigen3-dev " 13 | - DEBSIGN_KEYID=5AE5CD75 14 | - LCOV_IGNORE_RULES="*unittest* /opt/openrobots/*" 15 | - CC=gcc 16 | - DO_COVERAGE_ON_BRANCH="master;release" 17 | - DO_CPPCHECK_ON_BRANCH="" 18 | - DO_INSTALL_DOC_EXCEPT_ON_BRANCH="" 19 | - PYTHONPATH=/opt/openrobots/lib/python2.7/site-packages/ 20 | matrix: 21 | # - BUILDTYPE=Release 22 | - BUILDTYPE=Debug 23 | notifications: 24 | email: 25 | - hpp@laas.fr 26 | branches: 27 | only: 28 | - master 29 | - debian 30 | - devel 31 | matrix: 32 | allow_failures: 33 | - compiler: 34 | before_install: ./travis_custom/custom_before_install 35 | install: 36 | - pip install --user coveralls 37 | - pip install --user numpy 38 | script: 39 | - export CMAKE_ADDITIONAL_OPTIONS="-DCMAKE_BUILD_TYPE=${BUILDTYPE}" 40 | - sudo free -m -t 41 | - ./.travis/run ../travis_custom/custom_build 42 | after_failure: ./.travis/run after_failure 43 | after_success: 44 | - ./.travis/run after_success 45 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.14) 2 | 3 | # Project properties 4 | set(PROJECT_ORG stack-of-tasks) 5 | set(PROJECT_NAME parametric-curves) 6 | set(PROJECT_DESCRIPTION 7 | "Template based classes for creating and manipulating parametric curves. Comes with extra options specific to end-effector trajectories in robotics." 8 | ) 9 | set(PROJECT_URL "http://github.com/${PROJECT_ORG}/${PROJECT_NAME}") 10 | 11 | # Project options 12 | option(BUILD_PYTHON_INTERFACE "Build the python bindings" ON) 13 | option(INSTALL_PYTHON_INTERFACE_ONLY "Install *ONLY* the python bindings" OFF) 14 | 15 | # Project configuration 16 | if(NOT INSTALL_PYTHON_INTERFACE_ONLY) 17 | set(PROJECT_USE_CMAKE_EXPORT TRUE) 18 | endif(NOT INSTALL_PYTHON_INTERFACE_ONLY) 19 | set(CUSTOM_HEADER_DIR ${PROJECT_NAME}) 20 | set(CXX_DISABLE_WERROR TRUE) 21 | set(DOXYGEN_USE_MATHJAX YES) 22 | 23 | # Check if the submodule cmake have been initialized 24 | set(JRL_CMAKE_MODULES "${CMAKE_CURRENT_LIST_DIR}/cmake") 25 | if(NOT EXISTS "${CMAKE_SOURCE_DIR}/cmake/base.cmake") 26 | message(STATUS "JRL cmakemodules not found. Let's fetch it.") 27 | include(FetchContent) 28 | FetchContent_Declare( 29 | "jrl-cmakemodules" 30 | GIT_REPOSITORY "https://github.com/jrl-umi3218/jrl-cmakemodules.git") 31 | FetchContent_MakeAvailable("jrl-cmakemodules") 32 | FetchContent_GetProperties("jrl-cmakemodules" SOURCE_DIR JRL_CMAKE_MODULES) 33 | endif() 34 | 35 | include("${JRL_CMAKE_MODULES}/base.cmake") 36 | include("${JRL_CMAKE_MODULES}/boost.cmake") 37 | 38 | set_default_cmake_build_type("RelWithDebInfo") 39 | 40 | # Project definition 41 | compute_project_args(PROJECT_ARGS LANGUAGES CXX) 42 | project(${PROJECT_NAME} ${PROJECT_ARGS}) 43 | 44 | # Project dependencies 45 | if(BUILD_PYTHON_INTERFACE) 46 | add_project_dependency(eigenpy 2.7.12 REQUIRED) 47 | include("${JRL_CMAKE_MODULES}/python.cmake") 48 | endif(BUILD_PYTHON_INTERFACE) 49 | 50 | add_project_dependency(Eigen3 REQUIRED) 51 | add_project_dependency(Boost REQUIRED COMPONENTS serialization) 52 | 53 | if(BUILD_PYTHON_INTERFACE) 54 | string(REGEX REPLACE "-" "_" PY_NAME ${PROJECT_NAME}) 55 | set(PYWRAP ${PY_NAME}_pywrap) 56 | add_subdirectory(python) 57 | endif(BUILD_PYTHON_INTERFACE) 58 | 59 | # Main Library 60 | set(${PROJECT_NAME}_HEADERS 61 | # include/${PROJECT_NAME}/bernstein.h 62 | include/${PROJECT_NAME}/abstract-curve.hpp 63 | include/${PROJECT_NAME}/MathDefs.h 64 | include/${PROJECT_NAME}/spline.hpp 65 | include/${PROJECT_NAME}/polynomial.hpp 66 | include/${PROJECT_NAME}/infinite-sinusoid.hpp 67 | include/${PROJECT_NAME}/infinite-const-acc.hpp 68 | include/${PROJECT_NAME}/constant.hpp 69 | include/${PROJECT_NAME}/linear-chirp.hpp 70 | include/${PROJECT_NAME}/minimum-jerk.hpp 71 | include/${PROJECT_NAME}/text-file.hpp 72 | # include/${PROJECT_NAME}/bezier_curve.h 73 | include/${PROJECT_NAME}/curve-constraint.hpp 74 | include/${PROJECT_NAME}/serialization/eigen-matrix.hpp 75 | # include/${PROJECT_NAME}/serialization/archive.hpp 76 | include/${PROJECT_NAME}/spatial/force-curve.hpp 77 | include/${PROJECT_NAME}/utils/file-io.hpp) 78 | 79 | add_library(${PROJECT_NAME} INTERFACE) 80 | target_include_directories(${PROJECT_NAME} SYSTEM 81 | INTERFACE ${EIGEN3_INCLUDE_DIR}) 82 | target_include_directories(${PROJECT_NAME} 83 | INTERFACE $) 84 | 85 | if(NOT INSTALL_PYTHON_BINDINGS_ONLY) 86 | install( 87 | TARGETS ${PROJECT_NAME} 88 | EXPORT ${TARGETS_EXPORT_NAME} 89 | DESTINATION lib) 90 | endif(NOT INSTALL_PYTHON_BINDINGS_ONLY) 91 | 92 | if(BUILD_TESTING) 93 | add_subdirectory(tests) 94 | endif(BUILD_TESTING) 95 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Spline 2 | 3 | [![Pipeline status](https://gitlab.laas.fr/stack-of-tasks/parametric-curves/badges/master/pipeline.svg)](https://gitlab.laas.fr/stack-of-tasks/parametric-curves/commits/master) 4 | [![Coverage report](https://gitlab.laas.fr/stack-of-tasks/parametric-curves/badges/master/coverage.svg?job=doc-coverage)](https://gepettoweb.laas.fr/doc/stack-of-tasks/parametric-curves/master/coverage/) 5 | [![Code style: black](https://img.shields.io/badge/code%20style-black-000000.svg)](https://github.com/psf/black) 6 | [![pre-commit.ci status](https://results.pre-commit.ci/badge/github/stack-of-tasks/parametric-curves/master.svg)](https://results.pre-commit.ci/latest/github/stack-of-tasks/parametric-curves) 7 | 8 | A template-based Library for creating curves of arbitrary order and dimension, eventually subject to derivative constraints. The main use of the library is the creation of end-effector trajectories for legged robots. 9 | 10 | To do so, tools are provided to: 11 | > - create **exact** splines of arbitrary order (that pass exactly by an arbitrary number waypoints) 12 | > - constrain initial / end velocities and acceleration for the spline. 13 | > - constrain take-off and landing phases to follow a straight line along a given normal (to avoid undesired collisions between the effector and the contact surface) 14 | > - automatically handle 3d rotation of the effector. 15 | 16 | The library is template-based, thus generic: the curves can be of any dimension, and can be implemented in double, float ... 17 | 18 | While a Bezier curve implementation is provided, the main interest 19 | of this library is to create spline curves of arbitrary order 20 | 21 | ## Example of use for and end-effector trajectory 22 | 23 | The library comes with an helper class to automatically generate end-effector trajectories. 24 | For instance, to create a 2 second long trajectory from the point (0,0,0) to (1,1,0), with a waypoint 25 | at (0.5,0.5,0.5), one can use the following code: 26 | 27 | ``` 28 | typedef std::pair Waypoint; 29 | typedef std::vector T_Waypoint; 30 | 31 | // loading helper class namespace 32 | using namespace spline::helpers; 33 | 34 | // Create waypoints 35 | waypoints.push_back(std::make_pair(0., Eigen::Vector3d(0,0,0))); 36 | waypoints.push_back(std::make_pair(1., Eigen::Vector3d(0.5,0.5,0.5))); 37 | waypoints.push_back(std::make_pair(2., Eigen::Vector3d(1,1,0))); 38 | 39 | exact_cubic_t* eff_traj = effector_spline(waypoints.begin(),waypoints.end()); 40 | 41 | // evaluate spline 42 | (*eff_traj)(0.); // (0,0,0) 43 | (*eff_traj)(2.); // (1,1,0) 44 | ``` 45 | If rotation of the effector must be considered, the code is almost the same: 46 | 47 | ``` 48 | // initial rotation is 0, end rotation is a rotation by Pi around x axis 49 | quat_t init_rot(0,0,0,1), end_rot(1,0,0,0); 50 | effector_spline_rotation eff_traj_rot(waypoints.begin(),waypoints.end(), init_quat, end_quat); 51 | 52 | // evaluate spline 53 | eff_traj_rot(0.); // (0,0,0,0,0,0,1) 54 | eff_traj_rot(1.); // (0.5,0.5,0.5,0.707107,0,0,0.707107) // Pi/2 around x axis 55 | eff_traj_rot(2.); // (0,0,0,1,0,0,0) 56 | ``` 57 | 58 | Additional parameters for the same methods an be used to specify parameters for the take off and 59 | landing phases: height and duration of the phase, and along which normal. 60 | Please refer to the Main.cpp files to see all the unit tests and possibilities offered by the library 61 | 62 | ## Installation 63 | 64 | ### Dependencies 65 | * [Eigen (version >= 3.2.2)](http://eigen.tuxfamily.org/index.php?title=Main_Page) 66 | 67 | ### Additional dependencies for python bindings 68 | * [Boost.Python](http://www.boost.org/doc/libs/1_63_0/libs/python/doc/html/index.html) 69 | * [eigenpy](https://github.com/stack-of-tasks/eigenpy) 70 | 71 | To handle this with cmake, use the recursive option to clone the repository. 72 | For instance, using http: 73 | ``` 74 | git clone --recursive https://github.com/stonneau/spline.git $SPLINE_DIR 75 | ``` 76 | The library is header only, so the build only serves to build the tests and python bindings: 77 | 78 | ``` 79 | cd $SPLINE_DIR && mkdir build && cd build 80 | cmake .. && make 81 | ../bin/tests 82 | ``` 83 | 84 | If everything went fine you should obtain the following output: 85 | ``` 86 | performing tests... 87 | no errors found 88 | ``` 89 | #### Optional: Python bindings installation 90 | To install the Python bindings, in the CMakeLists.txt file, first enable the BUILD_PYTHON_INTERFACE option: 91 | ``` 92 | OPTION (BUILD_PYTHON_INTERFACE "Build the python binding" ON) 93 | ``` 94 | 95 | Then rebuild the library: 96 | ``` 97 | cd $SPLINE_DIR/build 98 | cmake -DCMAKE_INSTALL_PREFIX=${DEVEL_DIR}/install .. 99 | make install 100 | ``` 101 | The python bindings should then be accessible through the package centroidal_dynamics. 102 | To see example of use, you can refer to the [test file](https://github.com/stonneau/spline/blob/master/python/test/test.py) 103 | which is rather self explanatory: 104 | 105 | In spite of an exhaustive documentation, please refer to the C++ documentation, which mostly applies 106 | to python. For the moment, only bezier curves are binded. 107 | -------------------------------------------------------------------------------- /include/parametric-curves/MathDefs.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file Math.h 3 | * \brief Linear algebra and other maths definitions. Based on Eigen 3 or more 4 | * \author Steve T. 5 | * \version 0.1 6 | * \date 06/17/2013 7 | * 8 | * This file contains math definitions used 9 | * used throughout the library. 10 | * Preprocessors definition are used to use either float 11 | * or double values, and 3 dimensional vectors for 12 | * the Point structure. 13 | */ 14 | 15 | #ifndef _SPLINEMATH 16 | #define _SPLINEMATH 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | namespace parametriccurves { 24 | 25 | // REF: boulic et al An inverse kinematics architecture enforcing an arbitrary 26 | // number of strict priority levels 27 | template 28 | void PseudoInverse(_Matrix_Type_& pinvmat) { 29 | Eigen::JacobiSVD<_Matrix_Type_> svd( 30 | pinvmat, Eigen::ComputeFullU | Eigen::ComputeFullV); 31 | _Matrix_Type_ m_sigma = svd.singularValues(); 32 | 33 | double pinvtoler = 1.e-6; // choose your tolerance widely! 34 | 35 | _Matrix_Type_ m_sigma_inv = 36 | _Matrix_Type_::Zero(pinvmat.cols(), pinvmat.rows()); 37 | for (long i = 0; i < m_sigma.rows(); ++i) { 38 | if (m_sigma(i) > pinvtoler) m_sigma_inv(i, i) = 1.0 / m_sigma(i); 39 | } 40 | pinvmat = (svd.matrixV() * m_sigma_inv * svd.matrixU().transpose()); 41 | } 42 | 43 | } // namespace parametriccurves 44 | #endif //_SPLINEMATH 45 | -------------------------------------------------------------------------------- /include/parametric-curves/abstract-curve.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file AbstractCurve.hpp 3 | * \brief interface for a Curve of arbitrary dimension. 4 | * \author Steve T. 5 | * \version 0.1 6 | * \date 06/17/2013 7 | * 8 | * Interface for a curve 9 | */ 10 | #include 11 | #include 12 | 13 | #ifndef _parameteric_curves_abstract_curve_hpp 14 | #define _parameteric_curves_abstract_curve_hpp 15 | 16 | namespace parametriccurves { 17 | /// \struct AbstractCurve 18 | /// \brief Represents a curve of dimension Dim 19 | /// is Safe is false, no verification is made on the evaluation of the curve. 20 | template 21 | struct AbstractCurve { 22 | typedef Point point_t; 23 | typedef Numeric time_t; 24 | typedef Numeric num_t; 25 | 26 | public: 27 | /* Constructors - destructors */ 28 | AbstractCurve(time_t t_min_, time_t t_max_) : t_min(t_min_), t_max(t_max_) {} 29 | AbstractCurve() {} 30 | virtual ~AbstractCurve() {} 31 | 32 | public: 33 | /// \brief Evaluation of the cubic spline at time t. 34 | /// \param t : the time when to evaluate the spine 35 | /// \param return : the value x(t) 36 | virtual const point_t operator()(const time_t& t) const = 0; 37 | 38 | /// \brief Evaluation of the derivative spline at time t. 39 | /// \param t : the time when to evaluate the spline 40 | /// \param order : order of the derivative 41 | /// \param return : the value x(t) 42 | virtual const point_t derivate(const time_t& t, 43 | const std::size_t& order) const = 0; 44 | 45 | public: 46 | /*Getters*/ 47 | virtual const time_t tmin() const { return t_min; } 48 | virtual const time_t tmax() const { return t_max; } 49 | virtual bool checkRange(const time_t t) const { 50 | return (t >= t_min) && (t <= t_max); 51 | } 52 | 53 | /* Setters */ 54 | virtual bool setInitialPoint(const point_t& /*x_init*/) = 0; 55 | virtual bool setInitialPoint(const num_t& /*x_init*/) = 0; 56 | 57 | virtual bool setTimePeriod(const time_t& traj_time_) { 58 | t_min = 0.0; 59 | t_max = traj_time_; 60 | return true; 61 | } 62 | 63 | protected: 64 | time_t t_min; 65 | time_t t_max; 66 | }; 67 | } // namespace parametriccurves 68 | #endif //_STRUCT_CURVE_ABC 69 | -------------------------------------------------------------------------------- /include/parametric-curves/bernstein.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file bezier_curve.h 3 | * \brief class allowing to create a Bezier curve of dimension 1 <= n <= 3. 4 | * \author Steve T. 5 | * \version 0.1 6 | * \date 06/17/2013 7 | */ 8 | 9 | #ifndef _CLASS_BERNSTEIN 10 | #define _CLASS_BERNSTEIN 11 | 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | #include "MathDefs.h" 18 | #include "curve_abc.h" 19 | 20 | namespace spline { 21 | /// 22 | /// \brief Computes factorial of a number 23 | /// 24 | unsigned int fact(const unsigned int n) { 25 | assert(n >= 0); 26 | int res = 1; 27 | for (int i = 2; i <= n; ++i) res *= i; 28 | return res; 29 | } 30 | 31 | /// 32 | /// \brief Computes a binomal coefficient 33 | /// 34 | unsigned int bin(const unsigned int n, const unsigned int k) { 35 | return fact(n) / (fact(k) * fact(n - k)); 36 | } 37 | 38 | /// \class Bernstein 39 | /// \brief Computes a Bernstein polynome 40 | /// 41 | template 42 | struct Bern { 43 | Bern(const unsigned int m, const unsigned int i) 44 | : m_minus_i(m - i), i_(i), bin_m_i_(bin(m, i)) {} 45 | 46 | ~Bern() {} 47 | 48 | Numeric operator()(const Numeric u) const { 49 | assert(u >= 0. && u <= 1.); 50 | return bin_m_i_ * (pow(u, i_)) * pow((1 - u), m_minus_i); 51 | } 52 | 53 | Numeric m_minus_i; 54 | Numeric i_; 55 | Numeric bin_m_i_; 56 | }; 57 | 58 | /// 59 | /// \brief Computes all Bernstein polynomes for a certain degree 60 | /// 61 | template 62 | std::vector > makeBernstein(const unsigned int n) { 63 | std::vector > res; 64 | for (unsigned int i = 0; i <= n; ++i) res.push_back(Bern(n, i)); 65 | return res; 66 | } 67 | } // namespace spline 68 | #endif //_CLASS_BERNSTEIN 69 | -------------------------------------------------------------------------------- /include/parametric-curves/bezier_curve.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file bezier_curve.h 3 | * \brief class allowing to create a Bezier curve of dimension 1 <= n <= 3. 4 | * \author Steve T. 5 | * \version 0.1 6 | * \date 06/17/2013 7 | */ 8 | 9 | #ifndef _CLASS_BEZIERCURVE 10 | #define _CLASS_BEZIERCURVE 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | #include "MathDefs.h" 17 | #include "bernstein.h" 18 | #include "curve_abc.h" 19 | #include "curve_constraint.h" 20 | 21 | namespace spline { 22 | /// \class BezierCurve 23 | /// \brief Represents a Bezier curve of arbitrary dimension and order. 24 | /// For degree lesser than 4, the evaluation is analitycal.Otherwise 25 | /// the bernstein polynoms are used to evaluate the spline at a given location. 26 | /// 27 | template > 29 | struct bezier_curve : public curve_abc { 30 | typedef Point point_t; 31 | typedef Time time_t; 32 | typedef Numeric num_t; 33 | typedef curve_constraints curve_constraints_t; 34 | typedef std::vector > t_point_t; 35 | typedef typename t_point_t::const_iterator cit_point_t; 36 | typedef bezier_curve bezier_curve_t; 37 | 38 | /* Constructors - destructors */ 39 | public: 40 | ///\brief Constructor 41 | ///\param PointsBegin, PointsEnd : the points parametering the Bezier curve 42 | /// 43 | template 44 | bezier_curve(In PointsBegin, In PointsEnd, const time_t minBound = 0, 45 | const time_t maxBound = 1) 46 | : minBound_(minBound), 47 | maxBound_(maxBound), 48 | size_(std::distance(PointsBegin, PointsEnd)), 49 | degree_(size_ - 1), 50 | bernstein_(spline::makeBernstein(degree_)) { 51 | assert(bernstein_.size() == size_); 52 | In it(PointsBegin); 53 | if (Safe && (size_ < 1 || minBound >= maxBound)) 54 | throw std::out_of_range( 55 | "can't create bezier min bound is higher than max bound"); // TODO 56 | for (; it != PointsEnd; ++it) pts_.push_back(*it); 57 | } 58 | 59 | ///\brief Constructor 60 | /// This constructor will add 4 points (2 after the first one, 2 before the 61 | /// last one) to ensure that velocity and acceleration constraints are 62 | /// respected 63 | ///\param PointsBegin, PointsEnd : the points parametering the Bezier curve 64 | ///\param constraints : constraints applying on start / end velocities and 65 | /// acceleration 66 | /// 67 | template 68 | bezier_curve(In PointsBegin, In PointsEnd, 69 | const curve_constraints_t& constraints, 70 | const time_t minBound = 0, const time_t maxBound = 1) 71 | : minBound_(minBound), 72 | maxBound_(maxBound), 73 | size_(std::distance(PointsBegin, PointsEnd) + 4), 74 | degree_(size_ - 1), 75 | bernstein_(spline::makeBernstein(degree_)) { 76 | if (Safe && (size_ < 1 || minBound >= maxBound)) 77 | throw std::out_of_range( 78 | "can't create bezier min bound is higher than max bound"); 79 | t_point_t updatedList = 80 | add_constraints(PointsBegin, PointsEnd, constraints); 81 | for (cit_point_t cit = updatedList.begin(); cit != updatedList.end(); ++cit) 82 | pts_.push_back(*cit); 83 | } 84 | 85 | ///\brief Destructor 86 | ~bezier_curve() { 87 | // NOTHING 88 | } 89 | 90 | private: 91 | // bezier_curve(const bezier_curve&); 92 | // bezier_curve& operator=(const bezier_curve&); 93 | /* Constructors - destructors */ 94 | 95 | /*Operations*/ 96 | public: 97 | /// \brief Evaluation of the cubic spline at time t. 98 | /// \param t : the time when to evaluate the spine 99 | /// \param return : the value x(t) 100 | virtual point_t operator()(const time_t t) const { 101 | num_t nT = (t - minBound_) / (maxBound_ - minBound_); 102 | if (Safe & !(0 <= nT && nT <= 1)) { 103 | throw std::out_of_range( 104 | "can't evaluate bezier curve, out of range"); // TODO 105 | } else { 106 | num_t dt = (1 - nT); 107 | switch (size_) { 108 | case 1: 109 | return pts_[0]; 110 | case 2: 111 | return pts_[0] * dt + nT * pts_[1]; 112 | break; 113 | case 3: 114 | return pts_[0] * dt * dt + 2 * pts_[1] * nT * dt + pts_[2] * nT * nT; 115 | break; 116 | case 4: 117 | return pts_[0] * dt * dt * dt + 3 * pts_[1] * nT * dt * dt + 118 | 3 * pts_[2] * nT * nT * dt + pts_[3] * nT * nT * nT; 119 | default: 120 | return evalHorner(nT); 121 | break; 122 | } 123 | } 124 | } 125 | 126 | /// \brief Computes the derivative curve at order N. 127 | /// \param order : order of the derivative 128 | /// \param return : the value x(t) 129 | bezier_curve_t compute_derivate(const std::size_t order) const { 130 | if (order == 0) return *this; 131 | t_point_t derived_wp; 132 | for (typename t_point_t::const_iterator pit = pts_.begin(); 133 | pit != pts_.end() - 1; ++pit) 134 | derived_wp.push_back(degree_ * (*(pit + 1) - (*pit))); 135 | if (derived_wp.empty()) derived_wp.push_back(point_t::Zero()); 136 | bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(), minBound_, 137 | maxBound_); 138 | return deriv.compute_derivate(order - 1); 139 | } 140 | 141 | /// \brief Computes the primitive of the curve at order N. 142 | /// \param constant : value of the primitive at t = 0 143 | /// \param return : the curve x_1(t) such that d/dt(x_1(t)) = x_1(t) 144 | bezier_curve_t compute_primitive(const std::size_t order) const { 145 | if (order == 0) return *this; 146 | num_t new_degree = (num_t)(degree_ + 1); 147 | t_point_t n_wp; 148 | point_t current_sum = point_t::Zero(); 149 | // recomputing waypoints q_i from derivative waypoints p_i. q_0 is the given 150 | // constant. then q_i = (sum( j = 0 -> j = i-1) p_j) /n+1 151 | n_wp.push_back(current_sum); 152 | for (typename t_point_t::const_iterator pit = pts_.begin(); 153 | pit != pts_.end(); ++pit) { 154 | current_sum += *pit; 155 | n_wp.push_back(current_sum / new_degree); 156 | } 157 | bezier_curve_t integ(n_wp.begin(), n_wp.end(), minBound_, maxBound_); 158 | return integ.compute_primitive(order - 1); 159 | } 160 | 161 | /// \brief Evaluates the derivative at order N of the curve. 162 | /// If the derivative is to be evaluated several times, it is 163 | /// rather recommended to compute the derivative curve using compute_derivate 164 | /// \param order : order of the derivative 165 | /// \param t : the time when to evaluate the spine 166 | /// \param return : the value x(t) 167 | virtual point_t derivate(const time_t t, const std::size_t order) const { 168 | bezier_curve_t deriv = compute_derivate(order); 169 | return deriv(t); 170 | } 171 | 172 | /// 173 | /// \brief Evaluates all Bernstein polynomes for a certain degree 174 | /// 175 | point_t evalBernstein(const Numeric u) const { 176 | point_t res = point_t::Zero(); 177 | typename t_point_t::const_iterator pts_it = pts_.begin(); 178 | for (typename std::vector >::const_iterator cit = 179 | bernstein_.begin(); 180 | cit != bernstein_.end(); ++cit, ++pts_it) 181 | res += cit->operator()(u) * (*pts_it); 182 | return res; 183 | } 184 | 185 | /// 186 | /// \brief Evaluates all Bernstein polynomes for a certain degree using 187 | /// horner's scheme 188 | /// 189 | point_t evalHorner(const Numeric t) const { 190 | typename t_point_t::const_iterator pts_it = pts_.begin(); 191 | Numeric u, bc, tn; 192 | u = 1.0 - t; 193 | bc = 1; 194 | tn = 1; 195 | point_t tmp = (*pts_it) * u; 196 | ++pts_it; 197 | for (int i = 1; i < degree_; i++, ++pts_it) { 198 | tn = tn * t; 199 | bc = bc * (degree_ - i + 1) / i; 200 | tmp = (tmp + tn * bc * (*pts_it)) * u; 201 | } 202 | return (tmp + tn * t * (*pts_it)); 203 | } 204 | 205 | const t_point_t& waypoints() const { return pts_; } 206 | 207 | private: 208 | template 209 | t_point_t add_constraints(In PointsBegin, In PointsEnd, 210 | const curve_constraints_t& constraints) { 211 | t_point_t res; 212 | point_t P0, P1, P2, P_n_2, P_n_1, PN; 213 | P0 = *PointsBegin; 214 | PN = *(PointsEnd - 1); 215 | P1 = P0 + constraints.init_vel / degree_; 216 | P_n_1 = PN - constraints.end_vel / degree_; 217 | P2 = constraints.init_acc / (degree_ * (degree_ - 1)) + 2 * P1 - P0; 218 | P_n_2 = constraints.end_acc / (degree_ * (degree_ - 1)) + 2 * P_n_1 - PN; 219 | 220 | res.push_back(P0); 221 | res.push_back(P1); 222 | res.push_back(P2); 223 | 224 | for (In it = PointsBegin + 1; it != PointsEnd - 1; ++it) res.push_back(*it); 225 | 226 | res.push_back(P_n_2); 227 | res.push_back(P_n_1); 228 | res.push_back(PN); 229 | return res; 230 | } 231 | 232 | /*Operations*/ 233 | 234 | /*Helpers*/ 235 | public: 236 | virtual time_t min() const { return minBound_; } 237 | virtual time_t max() const { return maxBound_; } 238 | /*Helpers*/ 239 | 240 | public: 241 | const time_t minBound_, maxBound_; 242 | const std::size_t size_; 243 | const std::size_t degree_; 244 | const std::vector > bernstein_; 245 | 246 | private: 247 | t_point_t pts_; 248 | 249 | // storing bernstein polynoms, even in low dimension 250 | }; 251 | } // namespace spline 252 | #endif //_CLASS_BEZIERCURVE 253 | -------------------------------------------------------------------------------- /include/parametric-curves/constant.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file sinusoid.hpp 3 | * \brief Generates InfiniteSinusoidal trajectory 4 | * \author Rohan Budhiraja 5 | * \date 2017 6 | * 7 | */ 8 | 9 | #ifndef _parameteric_curves_constant_hpp 10 | #define _parameteric_curves_constant_hpp 11 | 12 | #include 13 | 14 | namespace parametriccurves { 15 | 16 | /// \class InfiniteSinusoid 17 | /// \brief Creates InfiniteSinusoid curve 18 | /// The sinusoid is actually a cosine so that it starts with zero velocity. 19 | /// Returns x = x_init + A*cos(2*pi*f*t) where f is give by 1/(2*traj_time) 20 | template > 22 | struct Constant : public AbstractCurve { 23 | typedef Point point_t; 24 | typedef Numeric time_t; 25 | typedef Numeric num_t; 26 | typedef AbstractCurve curve_abc_t; 27 | 28 | public: 29 | ///\brief Constructor 30 | 31 | Constant(const time_t& traj_time, 32 | const point_t& x_init_ = Eigen::Matrix::Zero()) 33 | : curve_abc_t(0, traj_time), x_init(x_init_) {} 34 | 35 | ///\brief Destructor 36 | ~Constant() {} 37 | 38 | public: 39 | virtual const point_t operator()(const time_t& t) const { return x_init; } 40 | 41 | virtual const point_t derivate(const time_t& t, 42 | const std::size_t& order) const { 43 | return point_t::Zero(); 44 | } 45 | 46 | virtual bool setInitialPoint(const point_t& x_init_) { 47 | if (x_init.size() != x_init_.size()) return false; 48 | x_init = x_init_; 49 | } 50 | 51 | virtual bool setInitialPoint(const double& x_init_) { 52 | if (Dim != 1) return false; 53 | x_init[0] = x_init_; 54 | return true; 55 | } 56 | 57 | protected: 58 | /*Attributes*/ 59 | point_t x_init; 60 | }; 61 | } // namespace parametriccurves 62 | #endif //_CLASS_EXACTCUBIC 63 | -------------------------------------------------------------------------------- /include/parametric-curves/curve-constraint.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file curve_constraint.h 3 | * \brief struct to define constraints on start / end velocities and 4 | * acceleration on a curve \author Steve T. \version 0.1 \date 04/05/2017 5 | * 6 | */ 7 | 8 | #ifndef _CLASS_CURVE_CONSTRAINT 9 | #define _CLASS_CURVE_CONSTRAINT 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | namespace parametriccurves { 17 | template 18 | struct curve_constraints { 19 | typedef Point point_t; 20 | curve_constraints() 21 | : init_vel(point_t::Zero()), 22 | init_acc(init_vel), 23 | end_vel(init_vel), 24 | end_acc(init_vel) {} 25 | 26 | ~curve_constraints() {} 27 | point_t init_vel; 28 | point_t init_acc; 29 | point_t end_vel; 30 | point_t end_acc; 31 | }; 32 | } // namespace parametriccurves 33 | #endif //_CLASS_CUBICZEROVELACC 34 | -------------------------------------------------------------------------------- /include/parametric-curves/helpers/effector_spline.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file exact_cubic.h 3 | * \brief class allowing to create an Exact cubic spline. 4 | * \author Steve T. 5 | * \version 0.1 6 | * \date 06/17/2013 7 | * 8 | * This file contains definitions for the ExactCubic class. 9 | * Given a set of waypoints (x_i*) and timestep (t_i), it provides the unique 10 | * set of cubic splines fulfulling those 4 restrictions : 11 | * - x_i(t_i) = x_i* ; this means that the curve passes trough each waypoint 12 | * - x_i(t_i+1) = x_i+1* ; 13 | * - its derivative is continous at t_i+1 14 | * - its acceleration is continous at t_i+1 15 | * more details in paper "Task-Space Trajectories via Cubic Spline Optimization" 16 | * By J. Zico Kolter and Andrew Y.ng (ICRA 2009) 17 | */ 18 | 19 | #ifndef _CLASS_EFFECTORSPLINE 20 | #define _CLASS_EFFECTORSPLINE 21 | 22 | #include "spline/spline_deriv_constraint.h" 23 | 24 | namespace spline { 25 | namespace helpers { 26 | typedef double Numeric; 27 | typedef double Time; 28 | typedef Eigen::Matrix Point; 29 | typedef std::vector > T_Point; 30 | typedef std::pair Waypoint; 31 | typedef std::vector T_Waypoint; 32 | typedef spline_deriv_constraint 33 | spline_deriv_constraint_t; 34 | typedef spline_deriv_constraint_t::spline_constraints spline_constraints_t; 35 | typedef spline_deriv_constraint_t::exact_cubic_t exact_cubic_t; 36 | typedef spline_deriv_constraint_t::t_spline_t t_spline_t; 37 | typedef spline_deriv_constraint_t::spline_t spline_t; 38 | 39 | Waypoint compute_offset(const Waypoint& source, const Point& normal, 40 | const Numeric offset, const Time time_offset) { 41 | // compute time such that the equation from source to offsetpoint is 42 | // necessarily a line 43 | Numeric norm = normal.norm(); 44 | assert(norm > 0.); 45 | return std::make_pair(source.first + time_offset, 46 | (source.second + normal / norm * offset)); 47 | } 48 | 49 | spline_t make_end_spline(const Point& normal, const Point& from, 50 | const Numeric offset, const Time init_time, 51 | const Time time_offset) { 52 | // compute spline from land way point to end point 53 | // constraints are null velocity and acceleration 54 | Numeric norm = normal.norm(); 55 | assert(norm > 0.); 56 | Point n = normal / norm; 57 | Point d = offset / (time_offset * time_offset * time_offset) * -n; 58 | Point c = -3 * d * time_offset; 59 | Point b = -c * time_offset; 60 | 61 | T_Point points; 62 | points.push_back(from); 63 | points.push_back(b); 64 | points.push_back(c); 65 | points.push_back(d); 66 | 67 | return spline_t(points.begin(), points.end(), init_time, 68 | init_time + time_offset); 69 | } 70 | 71 | spline_constraints_t compute_required_offset_velocity_acceleration( 72 | const spline_t& end_spline, const Time time_offset) { 73 | // computing end velocity: along landing normal and respecting time 74 | spline_constraints_t constraints; 75 | constraints.end_acc = end_spline.derivate(end_spline.min(), 2); 76 | constraints.end_vel = end_spline.derivate(end_spline.min(), 1); 77 | return constraints; 78 | } 79 | 80 | /// \brief Helper method to create a spline typically used to 81 | /// guide the 3d trajectory of a robot end effector. 82 | /// Given a set of waypoints, and the normal vector of the start and 83 | /// ending positions, automatically create the spline such that: 84 | /// + init and end velocities / accelerations are 0. 85 | /// + the effector lifts and lands exactly in the direction of the specified 86 | /// normals \param wayPointsBegin : an iterator pointing to the first element 87 | /// of a waypoint container \param wayPointsEnd : an iterator pointing to 88 | /// the end of a waypoint container \param lift_normal : normal 89 | /// to be followed by end effector at take-off \param land_normal : normal 90 | /// to be followed by end effector at landing \param lift_offset : length 91 | /// of the straight line along normal at take-off \param land_offset : 92 | /// length of the straight line along normal at landing \param 93 | /// lift_offset_duration : time travelled along straight line at take-off \param 94 | /// land_offset_duration : time travelled along straight line at landing 95 | /// 96 | template 97 | exact_cubic_t* effector_spline( 98 | In wayPointsBegin, In wayPointsEnd, 99 | const Point& lift_normal = Eigen::Vector3d::UnitZ(), 100 | const Point& land_normal = Eigen::Vector3d::UnitZ(), 101 | const Numeric lift_offset = 0.02, const Numeric land_offset = 0.02, 102 | const Time lift_offset_duration = 0.02, 103 | const Time land_offset_duration = 0.02) { 104 | T_Waypoint waypoints; 105 | const Waypoint &inPoint = *wayPointsBegin, endPoint = *(wayPointsEnd - 1); 106 | waypoints.push_back(inPoint); 107 | // adding initial offset 108 | waypoints.push_back( 109 | compute_offset(inPoint, lift_normal, lift_offset, lift_offset_duration)); 110 | // inserting all waypoints but last 111 | waypoints.insert(waypoints.end(), wayPointsBegin + 1, wayPointsEnd - 1); 112 | // inserting waypoint to start landing 113 | const Waypoint& landWaypoint = 114 | compute_offset(endPoint, land_normal, land_offset, -land_offset_duration); 115 | waypoints.push_back(landWaypoint); 116 | // specifying end velocity constraint such that landing will be in straight 117 | // line 118 | spline_t end_spline = 119 | make_end_spline(land_normal, landWaypoint.second, land_offset, 120 | landWaypoint.first, land_offset_duration); 121 | spline_constraints_t constraints = 122 | compute_required_offset_velocity_acceleration(end_spline, 123 | land_offset_duration); 124 | spline_deriv_constraint_t all_but_end(waypoints.begin(), waypoints.end(), 125 | constraints); 126 | t_spline_t splines = all_but_end.subSplines_; 127 | splines.push_back(end_spline); 128 | return new exact_cubic_t(splines); 129 | } 130 | } // namespace helpers 131 | } // namespace spline 132 | #endif //_CLASS_EFFECTORSPLINE 133 | -------------------------------------------------------------------------------- /include/parametric-curves/helpers/effector_spline_rotation.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file exact_cubic.h 3 | * \brief class allowing to create an Exact cubic spline. 4 | * \author Steve T. 5 | * \version 0.1 6 | * \date 06/17/2013 7 | * 8 | * This file contains definitions for the ExactCubic class. 9 | * Given a set of waypoints (x_i*) and timestep (t_i), it provides the unique 10 | * set of cubic splines fulfulling those 4 restrictions : 11 | * - x_i(t_i) = x_i* ; this means that the curve passes trough each waypoint 12 | * - x_i(t_i+1) = x_i+1* ; 13 | * - its derivative is continous at t_i+1 14 | * - its acceleration is continous at t_i+1 15 | * more details in paper "Task-Space Trajectories via Cubic Spline Optimization" 16 | * By J. Zico Kolter and Andrew Y.ng (ICRA 2009) 17 | */ 18 | 19 | #ifndef _CLASS_EFFECTOR_SPLINE_ROTATION 20 | #define _CLASS_EFFECTOR_SPLINE_ROTATION 21 | 22 | #include 23 | 24 | #include "spline/curve_abc.h" 25 | #include "spline/helpers/effector_spline.h" 26 | 27 | namespace spline { 28 | namespace helpers { 29 | 30 | typedef Eigen::Matrix quat_t; 31 | typedef Eigen::Ref quat_ref_t; 32 | typedef const Eigen::Ref quat_ref_const_t; 33 | typedef Eigen::Matrix config_t; 34 | typedef curve_abc curve_abc_quat_t; 35 | typedef std::pair waypoint_quat_t; 36 | typedef std::vector t_waypoint_quat_t; 37 | typedef Eigen::Matrix point_one_dim_t; 38 | typedef spline_deriv_constraint 39 | spline_deriv_constraint_one_dim; 40 | typedef std::pair waypoint_one_dim_t; 41 | typedef std::vector t_waypoint_one_dim_t; 42 | 43 | class rotation_spline : public curve_abc_quat_t { 44 | public: 45 | rotation_spline(quat_ref_const_t quat_from = quat_t(0, 0, 0, 1), 46 | quat_ref_const_t quat_to = quat_t(0, 0, 0, 1), 47 | const double min = 0., const double max = 1.) 48 | : curve_abc_quat_t(), 49 | quat_from_(quat_from.data()), 50 | quat_to_(quat_to.data()), 51 | min_(min), 52 | max_(max), 53 | time_reparam_(computeWayPoints()) {} 54 | 55 | ~rotation_spline() {} 56 | 57 | /* Copy Constructors / operator=*/ 58 | rotation_spline& operator=(const rotation_spline& from) { 59 | quat_from_ = from.quat_from_; 60 | quat_to_ = from.quat_to_; 61 | min_ = from.min_; 62 | max_ = from.max_; 63 | time_reparam_ = spline_deriv_constraint_one_dim(from.time_reparam_); 64 | } 65 | /* Copy Constructors / operator=*/ 66 | 67 | quat_t operator()(const Numeric t) const { 68 | if (t <= min()) return quat_from_.coeffs(); 69 | if (t >= max()) return quat_to_.coeffs(); 70 | // normalize u 71 | Numeric u = (t - min()) / (max() - min()); 72 | // reparametrize u 73 | return quat_from_.slerp(time_reparam_(u)[0], quat_to_).coeffs(); 74 | } 75 | 76 | virtual quat_t derivate(time_t /*t*/, std::size_t /*order*/) const { 77 | throw std::runtime_error( 78 | "TODO quaternion spline does not implement derivate"); 79 | } 80 | 81 | spline_deriv_constraint_one_dim computeWayPoints() const { 82 | // initializing time reparametrization for spline 83 | t_waypoint_one_dim_t waypoints; 84 | waypoints.push_back(std::make_pair(0, point_one_dim_t::Zero())); 85 | waypoints.push_back(std::make_pair(1, point_one_dim_t::Ones())); 86 | return spline_deriv_constraint_one_dim(waypoints.begin(), waypoints.end()); 87 | } 88 | 89 | virtual time_t min() const { return min_; } 90 | virtual time_t max() const { return max_; } 91 | 92 | public: 93 | Eigen::Quaterniond quat_from_; // const 94 | Eigen::Quaterniond quat_to_; // const 95 | double min_; // const 96 | double max_; // const 97 | spline_deriv_constraint_one_dim time_reparam_; // const 98 | }; 99 | 100 | typedef exact_cubic >, 102 | rotation_spline> 103 | exact_cubic_quat_t; 104 | 105 | /// \class effector_spline_rotation 106 | /// \brief Represents a trajectory for and end effector 107 | /// uses the method effector_spline to create a spline trajectory. 108 | /// Additionally, handles the rotation of the effector as follows: 109 | /// does not rotate during the take off and landing phase, 110 | /// then uses a SLERP algorithm to interpolate the rotation in the quaternion 111 | /// space. 112 | class effector_spline_rotation { 113 | /* Constructors - destructors */ 114 | public: 115 | /// \brief Constructor. 116 | /// Given a set of waypoints, and the normal vector of the start and 117 | /// ending positions, automatically create the spline such that: 118 | /// + init and end velocities / accelerations are 0. 119 | /// + the effector lifts and lands exactly in the direction of the specified 120 | /// normals \param wayPointsBegin : an iterator pointing to the first 121 | /// element of a waypoint container \param wayPointsEnd : an iterator 122 | /// pointing to the end of a waypoint container \param to_quat : 4D 123 | /// vector, quaternion indicating rotation at take off(x, y, z, w) \param 124 | /// land_quat : 4D vector, quaternion indicating rotation at landing 125 | /// (x, y, z, w) \param lift_normal : normal to be followed by end 126 | /// effector at take-off \param land_normal : normal to be followed by 127 | /// end effector at landing \param lift_offset : length of the straight 128 | /// line along normal at take-off \param land_offset : length of the 129 | /// straight line along normal at landing \param lift_offset_duration : time 130 | /// travelled along straight line at take-off \param land_offset_duration : 131 | /// time travelled along straight line at landing 132 | /// 133 | template 134 | effector_spline_rotation(In wayPointsBegin, In wayPointsEnd, 135 | quat_ref_const_t& to_quat = quat_t(0, 0, 0, 1), 136 | quat_ref_const_t& land_quat = quat_t(0, 0, 0, 1), 137 | const Point& lift_normal = Eigen::Vector3d::UnitZ(), 138 | const Point& land_normal = Eigen::Vector3d::UnitZ(), 139 | const Numeric lift_offset = 0.02, 140 | const Numeric land_offset = 0.02, 141 | const Time lift_offset_duration = 0.02, 142 | const Time land_offset_duration = 0.02) 143 | : spline_(effector_spline(wayPointsBegin, wayPointsEnd, lift_normal, 144 | land_normal, lift_offset, land_offset, 145 | lift_offset_duration, land_offset_duration)), 146 | to_quat_(to_quat.data()), 147 | land_quat_(land_quat.data()), 148 | time_lift_offset_(spline_->min() + lift_offset_duration), 149 | time_land_offset_(spline_->max() - land_offset_duration), 150 | quat_spline_(simple_quat_spline()) { 151 | // NOTHING 152 | } 153 | 154 | /// \brief Constructor. 155 | /// Given a set of waypoints, and the normal vector of the start and 156 | /// ending positions, automatically create the spline such that: 157 | /// + init and end velocities / accelerations are 0. 158 | /// + the effector lifts and lands exactly in the direction of the specified 159 | /// normals \param wayPointsBegin : an iterator pointing to the first 160 | /// element of a waypoint container \param wayPointsEnd : an iterator 161 | /// pointing to the end of a waypoint container \param 162 | /// quatWayPointsBegin : en iterator pointing to the first element of a 4D 163 | /// vector (x, y, z, w) container of 164 | /// quaternions indicating rotation at specific time steps. 165 | /// \param quatWayPointsEnd : en iterator pointing to the end of 166 | /// a 4D vector (x, y, z, w) container of 167 | /// quaternions indicating rotation at specific time steps. 168 | /// \param lift_normal : normal to be followed by end effector at 169 | /// take-off \param land_normal : normal to be followed by end 170 | /// effector at landing \param lift_offset : length of the straight 171 | /// line along normal at take-off \param land_offset : length of the 172 | /// straight line along normal at landing \param lift_offset_duration : time 173 | /// travelled along straight line at take-off \param land_offset_duration : 174 | /// time travelled along straight line at landing 175 | /// 176 | template 177 | effector_spline_rotation(In wayPointsBegin, In wayPointsEnd, 178 | InQuat quatWayPointsBegin, InQuat quatWayPointsEnd, 179 | const Point& lift_normal = Eigen::Vector3d::UnitZ(), 180 | const Point& land_normal = Eigen::Vector3d::UnitZ(), 181 | const Numeric lift_offset = 0.02, 182 | const Numeric land_offset = 0.02, 183 | const Time lift_offset_duration = 0.02, 184 | const Time land_offset_duration = 0.02) 185 | : spline_(effector_spline(wayPointsBegin, wayPointsEnd, lift_normal, 186 | land_normal, lift_offset, land_offset, 187 | lift_offset_duration, land_offset_duration)), 188 | to_quat_((quatWayPointsBegin->second).data()), 189 | land_quat_(((quatWayPointsEnd - 1)->second).data()), 190 | time_lift_offset_(spline_->min() + lift_offset_duration), 191 | time_land_offset_(spline_->max() - land_offset_duration), 192 | quat_spline_(quat_spline(quatWayPointsBegin, quatWayPointsEnd)) { 193 | // NOTHING 194 | } 195 | 196 | ~effector_spline_rotation() { delete spline_; } 197 | /* Constructors - destructors */ 198 | 199 | /*Helpers*/ 200 | public: 201 | Numeric min() const { return spline_->min(); } 202 | Numeric max() const { return spline_->max(); } 203 | /*Helpers*/ 204 | 205 | /*Operations*/ 206 | public: 207 | /// \brief Evaluation of the effector position and rotation at time t. 208 | /// \param t : the time when to evaluate the spline 209 | /// \param return : a 7D vector; The 3 first values are the 3D position, the 210 | /// 4 last the quaternion describing the rotation 211 | /// 212 | config_t operator()(const Numeric t) const { 213 | config_t res; 214 | res.head<3>() = (*spline_)(t); 215 | res.tail<4>() = interpolate_quat(t); 216 | return res; 217 | } 218 | 219 | public: 220 | quat_t interpolate_quat(const Numeric t) const { 221 | if (t <= time_lift_offset_) return to_quat_.coeffs(); 222 | if (t >= time_land_offset_) return land_quat_.coeffs(); 223 | return quat_spline_(t); 224 | } 225 | 226 | private: 227 | exact_cubic_quat_t simple_quat_spline() const { 228 | std::vector splines; 229 | splines.push_back(rotation_spline(to_quat_.coeffs(), land_quat_.coeffs(), 230 | time_lift_offset_, time_land_offset_)); 231 | return exact_cubic_quat_t(splines); 232 | } 233 | 234 | template 235 | exact_cubic_quat_t quat_spline(InQuat quatWayPointsBegin, 236 | InQuat quatWayPointsEnd) const { 237 | if (std::distance(quatWayPointsBegin, quatWayPointsEnd) < 1) 238 | return simple_quat_spline(); 239 | exact_cubic_quat_t::t_spline_t splines; 240 | InQuat it(quatWayPointsBegin); 241 | Time init = time_lift_offset_; 242 | Eigen::Quaterniond current_quat = to_quat_; 243 | for (; it != quatWayPointsEnd; ++it) { 244 | splines.push_back( 245 | rotation_spline(current_quat.coeffs(), it->second, init, it->first)); 246 | current_quat = it->second; 247 | init = it->first; 248 | } 249 | splines.push_back(rotation_spline( 250 | current_quat.coeffs(), land_quat_.coeffs(), init, time_land_offset_)); 251 | return exact_cubic_quat_t(splines); 252 | } 253 | 254 | /*Operations*/ 255 | 256 | /*Attributes*/ 257 | public: 258 | const exact_cubic_t* spline_; 259 | const Eigen::Quaterniond to_quat_; 260 | const Eigen::Quaterniond land_quat_; 261 | const double time_lift_offset_; 262 | const double time_land_offset_; 263 | const exact_cubic_quat_t quat_spline_; 264 | /*Attributes*/ 265 | }; 266 | 267 | } // namespace helpers 268 | } // namespace spline 269 | #endif //_CLASS_EFFECTOR_SPLINE_ROTATION 270 | -------------------------------------------------------------------------------- /include/parametric-curves/infinite-const-acc.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file constant-acceleration.hpp 3 | * \brief Generates InfiniteConstAcc trajectory 4 | * \author Rohan Budhiraja 5 | * \date 2017 6 | * 7 | */ 8 | 9 | #ifndef _parameteric_curves_infinite_constant_acceleration_hpp 10 | #define _parameteric_curves_infinite_constant_acceleration_hpp 11 | 12 | #include 13 | #include 14 | 15 | namespace parametriccurves { 16 | 17 | /// \class InfiniteConstAcc 18 | /// \brief Creates InfiniteConstAcc curve 19 | /// s = s_0 + u_0*t+0.5*a_0*t^2 20 | 21 | template > 23 | struct InfiniteConstAcc : public AbstractCurve { 24 | typedef Point point_t; 25 | typedef Numeric time_t; 26 | typedef Numeric num_t; 27 | 28 | typedef AbstractCurve curve_abc_t; 29 | 30 | public: 31 | ///\brief Constructor 32 | 33 | InfiniteConstAcc( 34 | const time_t& traj_time_, 35 | const point_t& x_init_ = Eigen::Matrix::Zero(), 36 | const point_t& x_final_ = Eigen::Matrix::Zero()) 37 | : curve_abc_t(-1, -1), 38 | traj_time(traj_time_), 39 | x_init(x_init_), 40 | x_final(x_final_) {} 41 | 42 | ///\brief Destructor 43 | ~InfiniteConstAcc() {} 44 | 45 | public: 46 | virtual const point_t operator()(const time_t& t_) const { 47 | const time_t t = std::fmod(t_, 2 * traj_time); 48 | const point_t& m_ddx0 = 4.0 * (x_final - x_init) / (traj_time * traj_time); 49 | if (t <= 0.5 * traj_time) 50 | return x_init + 0.5 * m_ddx0 * t * t; 51 | else if (t > 0.5 * traj_time && t <= 1.5 * traj_time) 52 | return (x_init + x_final) / 2 + 53 | 0.5 * m_ddx0 * traj_time * (t - 0.5 * traj_time) - 54 | 0.5 * m_ddx0 * (t - 0.5 * traj_time) * (t - 0.5 * traj_time); 55 | else //(t>1.5*traj_time && t<=2*traj_time) 56 | return (x_init + x_final) / 2 - 57 | 0.5 * m_ddx0 * traj_time * (t - 1.5 * traj_time) + 58 | 0.5 * m_ddx0 * (t - 1.5 * traj_time) * (t - 1.5 * traj_time); 59 | } 60 | 61 | virtual const point_t derivate(const time_t& t_, 62 | const std::size_t& order) const { 63 | const time_t t = std::fmod(t_, 2 * traj_time); 64 | const point_t& m_ddx0 = 4.0 * (x_final - x_init) / (traj_time * traj_time); 65 | if (order == 1) { 66 | if (t <= 0.5 * traj_time) 67 | return m_ddx0 * t; 68 | else if (t > 0.5 * traj_time && t <= 1.5 * traj_time) 69 | return 0.5 * m_ddx0 * traj_time - m_ddx0 * (t - 0.5 * traj_time); 70 | else //(t>1.5*traj_time && t<=2*traj_time) 71 | return -0.5 * m_ddx0 * traj_time + m_ddx0 * (t - 1.5 * traj_time); 72 | } else if (order == 2) { 73 | if (t <= 0.5 * traj_time) 74 | return m_ddx0; 75 | else if (t > 0.5 * traj_time && t <= 1.5 * traj_time) 76 | return -m_ddx0; 77 | else //(t>1.5*traj_time && t<=2*traj_time) 78 | return m_ddx0; 79 | } else { 80 | std::cerr << "Higher order derivatives not supported" << std::endl; 81 | return point_t::Zero(Dim); 82 | } 83 | } 84 | 85 | public: 86 | virtual bool setInitialPoint(const point_t& x_init_) { 87 | if (x_init_.size() != x_init.size()) return false; 88 | x_init = x_init_; 89 | } 90 | 91 | virtual bool setInitialPoint(const double& x_init_) { 92 | if (Dim != 1) return false; 93 | x_init[0] = x_init_; 94 | return true; 95 | } 96 | 97 | virtual bool setFinalPoint(const Eigen::VectorXd& x_final_) { 98 | if (x_final.size() != x_final_.size()) return false; 99 | x_final = x_final_; 100 | return true; 101 | } 102 | 103 | virtual bool setFinalPoint(const double& x_final_) { 104 | if (Dim != 1) return false; 105 | x_final[0] = x_final_; 106 | return true; 107 | } 108 | 109 | virtual bool setTrajectoryTime(double traj_time_) { 110 | if (traj_time_ <= 0.0) return false; 111 | traj_time = traj_time_; 112 | return true; 113 | } 114 | 115 | protected: 116 | /*Attributes*/ 117 | point_t x_init; 118 | point_t x_final; 119 | time_t traj_time; 120 | }; 121 | } // namespace parametriccurves 122 | #endif //_CLASS_EXACTCUBIC 123 | -------------------------------------------------------------------------------- /include/parametric-curves/infinite-sinusoid.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file sinusoid.hpp 3 | * \brief Generates InfiniteSinusoidal trajectory 4 | * \author Rohan Budhiraja 5 | * \date 2017 6 | * 7 | */ 8 | 9 | #ifndef _parameteric_curves_sinusoid_hpp 10 | #define _parameteric_curves_sinusoid_hpp 11 | 12 | #include 13 | 14 | namespace parametriccurves { 15 | 16 | /// \class InfiniteSinusoid 17 | /// \brief Creates InfiniteSinusoid curve 18 | /// The sinusoid is actually a cosine so that it starts with zero velocity. 19 | /// Returns x = x_init + A*cos(2*pi*f*t) where f is give by 1/(2*traj_time) 20 | template > 22 | struct InfiniteSinusoid : public AbstractCurve { 23 | typedef Point point_t; 24 | typedef Numeric time_t; 25 | typedef Numeric num_t; 26 | 27 | typedef AbstractCurve curve_abc_t; 28 | 29 | public: 30 | ///\brief Constructor 31 | 32 | InfiniteSinusoid( 33 | const time_t& traj_time_, 34 | const point_t& x_init_ = Eigen::Matrix::Zero(), 35 | const point_t& x_final_ = Eigen::Matrix::Zero()) 36 | : curve_abc_t(-1, -1), 37 | traj_time(traj_time_), 38 | x_init(x_init_), 39 | x_final(x_final_) {} 40 | 41 | ///\brief Destructor 42 | ~InfiniteSinusoid() {} 43 | 44 | public: 45 | virtual const point_t operator()(const time_t& t) const { 46 | return x_init + 0.5 * (x_final - x_init) * (1.0 - cos(two_pi_f(t))); 47 | } 48 | 49 | virtual const point_t derivate(const time_t& t, 50 | const std::size_t& order) const { 51 | if (order == 1) 52 | return (x_final - x_init) * 0.5 * two_pi_f(1) * sin(two_pi_f(t)); 53 | else if (order == 2) 54 | return (x_final - x_init) * 0.5 * two_pi_f(1) * two_pi_f(1) * 55 | cos(two_pi_f(t)); 56 | else { 57 | std::cerr << "Higher order derivatives not supported" << std::endl; 58 | return point_t::Zero(Dim); 59 | } 60 | } 61 | 62 | public: 63 | virtual bool setInitialPoint(const point_t& x_init_) { 64 | if (x_init_.size() != x_init.size()) return false; 65 | x_init = x_init_; 66 | } 67 | 68 | virtual bool setInitialPoint(const double& x_init_) { 69 | if (Dim != 1) return false; 70 | x_init[0] = x_init_; 71 | return true; 72 | } 73 | 74 | virtual bool setFinalPoint(const Eigen::VectorXd& x_final_) { 75 | if (x_final.size() != x_final_.size()) return false; 76 | x_final = x_final_; 77 | return true; 78 | } 79 | 80 | virtual bool setFinalPoint(const double& x_final_) { 81 | if (Dim != 1) return false; 82 | x_final[0] = x_final_; 83 | return true; 84 | } 85 | virtual bool setTrajectoryTime(double traj_time_) { 86 | if (traj_time_ <= 0.0) return false; 87 | traj_time = traj_time_; 88 | return true; 89 | } 90 | 91 | protected: 92 | /*Attributes*/ 93 | point_t x_init; 94 | point_t x_final; 95 | time_t traj_time; 96 | 97 | private: 98 | inline const num_t two_pi_f(const time_t& t) const { 99 | return (M_PI / traj_time) * t; 100 | } 101 | }; 102 | } // namespace parametriccurves 103 | #endif //_CLASS_EXACTCUBIC 104 | -------------------------------------------------------------------------------- /include/parametric-curves/linear-chirp.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file linear-chirp.hpp 3 | * \brief Generates LinearChirp trajectory 4 | * \author Rohan Budhiraja 5 | * \date 2017 6 | * 7 | */ 8 | 9 | #ifndef _parameteric_curves_linear_chirp_hpp 10 | #define _parameteric_curves_linear_chirp_hpp 11 | 12 | #include 13 | 14 | namespace parametriccurves { 15 | 16 | /** \class LinearChirp 17 | * \brief Creates LinearChirp curve 18 | * Linear chirp trajectory generator. 19 | * A linear chirp is a sinusoid whose frequency is a linear function of time. 20 | * In particular the frequency starts from a value f0 and it increases linearly 21 | * up to a value f1. Then it goes back to f0 and the trajectory is ended. 22 | */ 23 | template > 25 | struct LinearChirp : public AbstractCurve { 26 | typedef Point point_t; 27 | typedef Point freq_t; 28 | typedef Point phase_t; 29 | typedef Numeric time_t; 30 | typedef Numeric num_t; 31 | typedef AbstractCurve curve_abc_t; 32 | 33 | public: 34 | ///\brief Constructor 35 | 36 | LinearChirp(const time_t& traj_time_, 37 | const point_t& x_init_ = Eigen::Matrix::Zero(), 38 | const point_t& x_final_ = Eigen::Matrix::Zero()) 39 | : curve_abc_t(0, traj_time_), x_init(x_init_), x_final(x_final_) { 40 | f0.setZero(Dim); 41 | f1.setZero(Dim); 42 | } 43 | 44 | LinearChirp(const time_t& traj_time_, const freq_t& f0_, const freq_t& f1_, 45 | const point_t& x_init_ = Eigen::Matrix::Zero(), 46 | const point_t& x_final_ = Eigen::Matrix::Zero()) 47 | : curve_abc_t(0, traj_time_), 48 | f0(f0_), 49 | f1(f1_), 50 | x_init(x_init_), 51 | x_final(x_final_) {} 52 | 53 | ///\brief Destructor 54 | ~LinearChirp() {} 55 | 56 | public: 57 | virtual const point_t operator()(const time_t& t) const { 58 | const point_t& m_p = 0.5 * (1.0 - phase(t).array().cos()); 59 | return x_init.array() + (x_final.array() - x_init.array()) * m_p.array(); 60 | } 61 | 62 | virtual const point_t derivate(const time_t& t, 63 | const std::size_t& order) const { 64 | if (order == 1) { 65 | const point_t& m_dp = M_PI * freq(t).array() * phase(t).array().sin(); 66 | return (x_final - x_init).array() * m_dp.array(); 67 | } else if (order == 2) { 68 | const point_t& m_ddp = 2.0 * M_PI * M_PI * freq(t).array() * 69 | freq(t).array() * phase(t).array().cos(); 70 | return (x_final - x_init).array() * m_ddp.array(); 71 | } else { 72 | std::cerr << "Higher order derivatives not supported" << std::endl; 73 | return point_t::Zero(Dim); 74 | } 75 | } 76 | 77 | public: 78 | /*Setters*/ 79 | virtual bool setInitialFrequency(const Eigen::VectorXd& f0_) { 80 | if (f0.size() != f0_.size()) return false; 81 | f0 = f0_; 82 | return true; 83 | } 84 | 85 | virtual bool setInitialFrequency(const double& f0_) { 86 | if (Dim != 1) return false; 87 | f0[0] = f0_; 88 | return true; 89 | } 90 | 91 | virtual bool setFinalFrequency(const Eigen::VectorXd& f1_) { 92 | if (f1.size() != f1_.size()) return false; 93 | f1 = f1_; 94 | return true; 95 | } 96 | 97 | virtual bool setFinalFrequency(const double& f1_) { 98 | if (Dim != 1) return false; 99 | f1[0] = f1_; 100 | return true; 101 | } 102 | 103 | virtual bool setInitialPoint(const point_t& x_init_) { 104 | if (x_init_.size() != x_init.size()) return false; 105 | x_init = x_init_; 106 | } 107 | 108 | virtual bool setInitialPoint(const double& x_init_) { 109 | if (Dim != 1) return false; 110 | x_init[0] = x_init_; 111 | return true; 112 | } 113 | 114 | virtual bool setFinalPoint(const Eigen::VectorXd& x_final_) { 115 | if (x_final.size() != x_final_.size()) return false; 116 | x_final = x_final_; 117 | return true; 118 | } 119 | 120 | virtual bool setFinalPoint(const double& x_final_) { 121 | if (Dim != 1) return false; 122 | x_final[0] = x_final_; 123 | return true; 124 | } 125 | 126 | protected: 127 | /*Attributes*/ 128 | freq_t f0; /// initial frequency 129 | freq_t f1; /// final frequency 130 | point_t x_init; 131 | point_t x_final; 132 | 133 | private: 134 | inline const freq_t freq(const time_t& t) const { 135 | const freq_t& m_k = 2.0 * (f1 - f0) / this->t_max; 136 | if (t < 0.5 * this->t_max) 137 | return f0 + m_k * t; 138 | else 139 | return f1 + m_k * (0.5 * this->t_max - t); 140 | } 141 | 142 | inline const phase_t phase(const time_t& t) const { 143 | const freq_t& m_k = 2.0 * (f1 - f0) / this->t_max; 144 | const phase_t& m_phi_0 = M_PI * this->t_max * (f0 - f1); 145 | if (t < 0.5 * this->t_max) 146 | return 2 * M_PI * t * (f0 + 0.5 * m_k * t); 147 | else 148 | return m_phi_0 + 2 * M_PI * t * (f1 + 0.5 * m_k * (this->t_max - t)); 149 | } 150 | }; 151 | } // namespace parametriccurves 152 | #endif //_CLASS_EXACTCUBIC 153 | -------------------------------------------------------------------------------- /include/parametric-curves/minimum-jerk.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file minimum-jerk.hpp 3 | * \brief Generates MinimumJerk trajectory 4 | * \author Rohan Budhiraja 5 | * \date 2017 6 | * 7 | */ 8 | 9 | #ifndef _parameteric_curves_minimum_jerk_hpp 10 | #define _parameteric_curves_minimum_jerk_hpp 11 | 12 | #include 13 | 14 | namespace parametriccurves { 15 | 16 | /** \class MinimumJerk 17 | * \brief Creates MinimumJerk curve 18 | */ 19 | template > 21 | struct MinimumJerk : public AbstractCurve { 22 | typedef Point point_t; 23 | typedef Numeric time_t; 24 | typedef Numeric num_t; 25 | typedef AbstractCurve curve_abc_t; 26 | 27 | public: 28 | ///\brief Constructor 29 | 30 | MinimumJerk(const time_t& traj_time_, 31 | const point_t& x_init_ = Eigen::Matrix::Zero(), 32 | const point_t& x_final_ = Eigen::Matrix::Zero()) 33 | : curve_abc_t(0, traj_time_), x_init(x_init_), x_final(x_final_) {} 34 | 35 | ///\brief Destructor 36 | ~MinimumJerk() {} 37 | 38 | public: 39 | virtual const point_t operator()(const time_t& t) const { 40 | time_t td = t / this->t_max; 41 | time_t td2 = td * td; 42 | time_t td3 = td2 * td; 43 | time_t td4 = td3 * td; 44 | time_t td5 = td4 * td; 45 | time_t p = 10 * td3 - 15 * td4 + 6 * td5; 46 | return x_init + (x_final - x_init) * p; 47 | } 48 | 49 | virtual const point_t derivate(const time_t& t, 50 | const std::size_t& order) const { 51 | time_t td = t / this->t_max; 52 | time_t td2 = td * td; 53 | time_t td3 = td2 * td; 54 | time_t td4 = td3 * td; 55 | time_t td5 = td4 * td; 56 | if (order == 1) { 57 | time_t dp = (30 * td2 - 60 * td3 + 30 * td4) / this->t_max; 58 | return (x_final - x_init) * dp; 59 | } else if (order == 2) { 60 | time_t ddp = 61 | (60 * td - 180 * td2 + 120 * td3) / (this->t_max * this->t_max); 62 | return (x_final - x_init) * ddp; 63 | } else { 64 | std::cerr << "Higher order derivatives not supported" << std::endl; 65 | return point_t::Zero(Dim); 66 | } 67 | } 68 | 69 | public: 70 | /*Setters*/ 71 | virtual bool setInitialPoint(const point_t& x_init_) { 72 | if (x_init_.size() != x_init.size()) return false; 73 | x_init = x_init_; 74 | } 75 | 76 | virtual bool setInitialPoint(const num_t& x_init_) { 77 | if (Dim != 1) return false; 78 | x_init[0] = x_init_; 79 | return true; 80 | } 81 | 82 | virtual bool setFinalPoint(const point_t& x_final_) { 83 | if (x_final.size() != x_final_.size()) return false; 84 | x_final = x_final_; 85 | return true; 86 | } 87 | 88 | virtual bool setFinalPoint(const double& x_final_) { 89 | if (Dim != 1) return false; 90 | x_final[0] = x_final_; 91 | return true; 92 | } 93 | 94 | protected: 95 | /*Attributes*/ 96 | point_t x_init; 97 | point_t x_final; 98 | 99 | private: 100 | }; 101 | } // namespace parametriccurves 102 | #endif //_CLASS_EXACTCUBIC 103 | -------------------------------------------------------------------------------- /include/parametric-curves/optimization/OptimizeSpline.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file OptimizeSpline.h 3 | * \brief Optimization loop for cubic spline generations 4 | * \author Steve T. 5 | * \version 0.1 6 | * \date 06/17/2013 7 | * 8 | * This file uses the mosek library to optimize the waypoints location 9 | * to generate exactCubic spline 10 | */ 11 | 12 | #ifndef _CLASS_SPLINEOPTIMIZER 13 | #define _CLASS_SPLINEOPTIMIZER 14 | 15 | #include 16 | #include 17 | 18 | #include "mosek/mosek.h" 19 | #include "spline/MathDefs.h" 20 | #include "spline/exact_cubic.h" 21 | 22 | namespace spline { 23 | /// \class SplineOptimizer 24 | /// \brief Mosek connection to produce optimized splines 25 | template > 27 | struct SplineOptimizer { 28 | typedef Eigen::Matrix MatrixX; 29 | typedef Point point_t; 30 | typedef Time time_t; 31 | typedef Numeric num_t; 32 | typedef exact_cubic exact_cubic_t; 33 | typedef SplineOptimizer splineOptimizer_t; 34 | 35 | /* Constructors - destructors */ 36 | public: 37 | ///\brief Initializes optimizer environment 38 | SplineOptimizer() { 39 | MSKrescodee r_ = MSK_makeenv(&env_, NULL); 40 | assert(r_ == MSK_RES_OK); 41 | } 42 | 43 | ///\brief Destructor 44 | ~SplineOptimizer() { MSK_deleteenv(&env_); } 45 | 46 | private: 47 | SplineOptimizer(const SplineOptimizer&); 48 | SplineOptimizer& operator=(const SplineOptimizer&); 49 | /* Constructors - destructors */ 50 | 51 | /*Operations*/ 52 | public: 53 | /// \brief Starts an optimization loop to create curve 54 | /// \param waypoints : a list comprising at least 2 waypoints in ascending 55 | /// time order 56 | /// \return An Optimised curve 57 | template 58 | exact_cubic_t* GenerateOptimizedCurve(In wayPointsBegin, 59 | In wayPointsEnd) const; 60 | /*Operations*/ 61 | 62 | private: 63 | template 64 | void ComputeHMatrices(In wayPointsBegin, In wayPointsEnd, MatrixX& h1, 65 | MatrixX& h2, MatrixX& h3, MatrixX& h4) const; 66 | 67 | /*Attributes*/ 68 | private: 69 | MSKenv_t env_; 70 | /*Attributes*/ 71 | 72 | private: 73 | typedef std::pair waypoint_t; 74 | typedef std::vector T_waypoints_t; 75 | }; 76 | 77 | template 79 | template 80 | inline void SplineOptimizer::ComputeHMatrices( 81 | In wayPointsBegin, In wayPointsEnd, MatrixX& h1, MatrixX& h2, MatrixX& h3, 82 | MatrixX& h4) const { 83 | std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd)); 84 | assert((!Safe) || (size > 1)); 85 | 86 | In it(wayPointsBegin), next(wayPointsBegin); 87 | ++next; 88 | Numeric t_previous((*it).first); 89 | 90 | for (std::size_t i(0); next != wayPointsEnd; ++next, ++it, ++i) { 91 | num_t const dTi((*next).first - (*it).first); 92 | num_t const dTi_sqr(dTi * dTi); 93 | // filling matrices values 94 | h3(i, i) = -3 / dTi_sqr; 95 | h3(i, i + 1) = 3 / dTi_sqr; 96 | h4(i, i) = -2 / dTi; 97 | h4(i, i + 1) = -1 / dTi; 98 | if (i + 2 < size) { 99 | In it2(next); 100 | ++it2; 101 | num_t const dTi_1((*it2).first - (*next).first); 102 | num_t const dTi_1sqr(dTi_1 * dTi_1); 103 | // this can be optimized but let's focus on clarity as long as not needed 104 | h1(i + 1, i) = 2 / dTi; 105 | h1(i + 1, i + 1) = 4 / dTi + 4 / dTi_1; 106 | h1(i + 1, i + 2) = 2 / dTi_1; 107 | h2(i + 1, i) = -6 / dTi_sqr; 108 | h2(i + 1, i + 1) = (6 / dTi_1sqr) - (6 / dTi_sqr); 109 | h2(i + 1, i + 2) = 6 / dTi_1sqr; 110 | } 111 | } 112 | } 113 | 114 | template 116 | template 117 | inline exact_cubic* 118 | SplineOptimizer::GenerateOptimizedCurve( 119 | In wayPointsBegin, In wayPointsEnd) const { 120 | exact_cubic_t* res = 0; 121 | int const size((int)std::distance(wayPointsBegin, wayPointsEnd)); 122 | if (Safe && size < 1) { 123 | throw; // TODO 124 | } 125 | // refer to the paper to understand all this. 126 | MatrixX h1 = MatrixX::Zero(size, size); 127 | MatrixX h2 = MatrixX::Zero(size, size); 128 | MatrixX h3 = MatrixX::Zero(size, size); 129 | MatrixX h4 = MatrixX::Zero(size, size); 130 | 131 | // remove this for the time being 132 | /*MatrixX g1 = MatrixX::Zero(size, size); 133 | MatrixX g2 = MatrixX::Zero(size, size); 134 | MatrixX g3 = MatrixX::Zero(size, size); 135 | MatrixX g4 = MatrixX::Zero(size, size);*/ 136 | 137 | ComputeHMatrices(wayPointsBegin, wayPointsEnd, h1, h2, h3, h4); 138 | 139 | // number of Waypoints : T + 1 => T mid points. Dim variables per points, + 140 | // acceleration + derivations (T * t+ 1 ) * Dim * 3 = nb var 141 | 142 | // NOG const MSKint32t numvar = ( size + size - 1) * 3 * 3; 143 | const MSKint32t numvar = (size)*Dim * 3; 144 | /* 145 | We store the variables in that order to simplifly matrix computation ( see 146 | later ) 147 | // NOG [ x0 x1 --- xn y0 --- y z0 --- zn x0. --- zn. x0..--- zn.. x0' --- zn..' 148 | ] T [ x0 x1 --- xn y0 --- y z0 --- zn x0. --- zn. x0..--- zn..] T 149 | */ 150 | 151 | /*the first constraint is H1x. = H2x => H2x - H1x. = 0 152 | this will give us size * 3 inequalities constraints 153 | So this line of A will be writen 154 | H2 -H1 0 0 0 0 155 | */ 156 | 157 | int ptOff = (int)Dim * size; // . offest 158 | int ptptOff = (int)Dim * 2 * size; // .. offest 159 | int prOff = 160 | (int)3 * 161 | ptOff; // ' offest 162 | // NOG int prptOff = (int) prOff + ptOff; // '. offest 163 | // NOG int prptptOff = (int) prptOff + ptOff; // '.. offest 164 | 165 | MatrixX h2x_h1x = MatrixX::Zero(size * Dim, numvar); 166 | /**A looks something like that : (n = size) 167 | [H2(0) 0 0 -H1(0) 0-------------------0] 168 | [ 0 0 H2(0) 0 0 -H1(0)---------------0] 169 | [ 0 0 0 H2(0) 0 0 H1(0)--------0] 170 | ... 171 | [ 0 0 0 0 H2(n) 0 0 0 -H1(n)-0] // row n 172 | 173 | Overall it's fairly easy to fill 174 | */ 175 | for (int i = 0; i < size * Dim; i = i + 3) { 176 | for (int j = 0; j < Dim; ++j) { 177 | int id = i + j; 178 | h2x_h1x.block(id, j * size, 1, size) = h2.row(i % 3); 179 | h2x_h1x.block(id, j * size + ptOff, 1, size) -= h1.row(i % 3); 180 | } 181 | } 182 | 183 | /*the second constraint is x' = G1x + G2x. => G1x + G2x. - x' = 0 184 | this will give us size * 3 inequalities constraints 185 | So this line of A will be writen 186 | H2 -H1 0 0 0 0 187 | */ 188 | MatrixX g1x_g2x = MatrixX::Zero(size * Dim, numvar); 189 | /**A looks something like that : (n = size) 190 | [G1(0) 0 0 G2(0) 0-----------------------0 -1 0] 191 | [ 0 0 G1(0) 0 0 G2(0)-------------------0 -1 0] 192 | [ 0 0 0 G1(0) 0 0 G2(0)-----------0 -1 0] 193 | ... 194 | [ 0 0 0 0 G1(n) 0 0 0 G2(n)-0 -1 0] // row n 195 | 196 | Overall it's fairly easy to fill 197 | */ 198 | // NOG 199 | /*for(int j = 0; j<3; ++j) 200 | { 201 | for(int j =0; j<3; ++j) 202 | { 203 | int id = i + j; 204 | g1x_g2x.block(id, j*size , 1, size) = g1.row(i); 205 | g1x_g2x.block(id, j*size + ptOff, 1, size) = g2.row(i); 206 | g1x_g2x.block(id, j*size + prOff, 1, size) -= 207 | MatrixX::Ones(1, size); 208 | } 209 | }*/ 210 | 211 | /*the third constraint is x.' = G3x + G4x. => G3x + G4x. - x.' = 0 212 | this will give us size * 3 inequalities constraints 213 | So this line of A will be writen 214 | H2 -H1 0 0 0 0 215 | */ 216 | MatrixX g3x_g4x = MatrixX::Zero(size * Dim, numvar); 217 | /**A looks something like that : (n = size) 218 | [G3(0) 0 0 G4(0) 0-------------------0 -1 0] 219 | [ 0 0 G3(0) 0 0 G4(0)---------------0 -1 0] 220 | [ 0 0 0 G3(0) 0 0 G4(0)--------0 -1 0] 221 | ... 222 | [ 0 0 0 0 G3(n) 0 0 0 G4(n)-0 -1 0] // row n 223 | 224 | Overall it's fairly easy to fill 225 | */ 226 | // NOG 227 | /*for(int j = 0; j<3; ++j) 228 | { 229 | for(int j =0; j<3; ++j) 230 | { 231 | int id = i + j; 232 | g3x_g4x.block(id, j*size , 1, size) = 233 | g3.row(i); g3x_g4x.block(id, j*size + ptOff, 1, size) = g4.row(i); 234 | g3x_g4x.block(id, j*size + prptOff, 1, size) -= 235 | MatrixX::Ones(1, size); 236 | } 237 | } 238 | */ 239 | /*the fourth constraint is x.. = 1/2(H3x + H4x.) => 1/2(H3x + H4x.) - x.. = 0 240 | => H3x + H4x. - 2x.. = 0 241 | this will give us size * 3 inequalities constraints 242 | So this line of A will be writen 243 | H2 -H1 0 0 0 0 244 | */ 245 | MatrixX h3x_h4x = MatrixX::Zero(size * Dim, numvar); 246 | /**A looks something like that : (n = size) 247 | [H3(0) 0 0 H4(0) 0-------------------0 -2 0] 248 | [ 0 0 H3(0) 0 0 H4(0)---------------0 -2 0] 249 | [ 0 0 0 H3(0) 0 0 H4(0)-------0 -2 0] 250 | ... 251 | [ 0 0 0 0 H3(n) 0 0 0 H4(n)-0 -2 0] // row n 252 | 253 | Overall it's fairly easy to fill 254 | */ 255 | for (int i = 0; i < size * Dim; i = i + Dim) { 256 | for (int j = 0; j < Dim; ++j) { 257 | int id = i + j; 258 | h3x_h4x.block(id, j * size, 1, size) = h3.row(i % 3); 259 | h3x_h4x.block(id, j * size + ptOff, 1, size) = h4.row(i % 3); 260 | h3x_h4x.block(id, j * size + ptptOff, 1, size) = 261 | MatrixX::Ones(1, size) * -2; 262 | } 263 | } 264 | 265 | /*the following constraints are easy to understand*/ 266 | 267 | /*x0,: = x^0*/ 268 | MatrixX x0_x0 = MatrixX::Zero(Dim, numvar); 269 | for (int j = 0; j < Dim; ++j) { 270 | x0_x0(0, 0) = 1; 271 | x0_x0(1, size) = 1; 272 | x0_x0(2, size * 2) = 1; 273 | } 274 | 275 | /*x0.,: = 0*/ 276 | MatrixX x0p_0 = MatrixX::Zero(Dim, numvar); 277 | for (int j = 0; j < Dim; ++j) { 278 | x0p_0(0, ptOff) = 1; 279 | x0p_0(1, ptOff + size) = 1; 280 | x0p_0(2, ptOff + size * 2) = 1; 281 | } 282 | 283 | /*xt,: = x^t*/ 284 | MatrixX xt_xt = MatrixX::Zero(Dim, numvar); 285 | for (int j = 0; j < Dim; ++j) { 286 | xt_xt(0, size - 1) = 1; 287 | xt_xt(1, 2 * size - 1) = 1; 288 | xt_xt(2, 3 * size - 1) = 1; 289 | } 290 | 291 | /*xT.,: = 0*/ 292 | MatrixX xtp_0 = MatrixX::Zero(Dim, numvar); 293 | for (int j = 0; j < Dim; ++j) { 294 | xtp_0(0, ptOff + size - 1) = 1; 295 | xtp_0(1, ptOff + size + size - 1) = 1; 296 | xtp_0(2, ptOff + size * 2 + size - 1) = 1; 297 | } 298 | 299 | // skipping constraints on x and y accelerations for the time being 300 | // to compute A i'll create an eigen matrix, then i'll convert it to a sparse 301 | // one and fill those tables 302 | 303 | // total number of constraints 304 | // NOG h2x_h1x (size * Dim) + h3x_h4x (size * Dim ) + g1x_g2x (size * Dim ) + 305 | // g3x_g4x (size*Dim) NOG + x0_x0 (Dim ) + x0p_0 (Dim) + xt_xt (Dim) + xtp_0 306 | // (Dim) = 4 * Dim * size + 4 * Dim h2x_h1x (size * Dim) + h3x_h4x (size * Dim 307 | // ) 308 | // + x0_x0 (Dim ) + x0p_0 (Dim) + xt_xt (Dim) + xtp_0 (Dim) = 2 * Dim * size 309 | // + 4 * Dim NOG const MSKint32t numcon = 12 * size + 12; 310 | const MSKint32t numcon = Dim * 2 * size + 4 * Dim; // TODO 311 | 312 | // this gives us the matrix A of size numcon * numvaar 313 | MatrixX a = MatrixX::Zero(numcon, numvar); 314 | a.block(0, 0, size * Dim, numvar) = h2x_h1x; 315 | a.block(size * Dim, 0, size * Dim, numvar) = h3x_h4x; 316 | a.block(size * Dim * 2, 0, Dim, numvar) = x0p_0; 317 | a.block(size * Dim * 2 + Dim, 0, Dim, numvar) = xtp_0; 318 | a.block(size * Dim * 2 + Dim * 2, 0, Dim, numvar) = x0_x0; 319 | a.block(size * Dim * 2 + Dim * 3, 0, Dim, numvar) = xt_xt; 320 | 321 | // convert to sparse representation 322 | Eigen::SparseMatrix spA; 323 | spA = a.sparseView(); 324 | 325 | // convert to sparse representation using column 326 | // http://docs.mosek.com/7.0/capi/Conventions_employed_in_the_API.html#sec-intro-subsubsec-cmo-rmo-matrix 327 | 328 | int nonZeros = spA.nonZeros(); 329 | 330 | /* Below is the sparse representation of the A 331 | matrix stored by column. */ 332 | double* aval = new double[nonZeros]; 333 | MSKint32t* asub = new MSKint32t[nonZeros]; 334 | MSKint32t* aptrb = new MSKint32t[numvar]; 335 | MSKint32t* aptre = new MSKint32t[numvar]; 336 | 337 | int currentIndex = 0; 338 | for (int j = 0; j < numvar; ++j) { 339 | bool nonZeroAtThisCol = false; 340 | for (int i = 0; i < numcon; ++i) { 341 | if (a(i, j) != 0) { 342 | if (!nonZeroAtThisCol) { 343 | aptrb[j] = currentIndex; 344 | nonZeroAtThisCol = true; 345 | } 346 | aval[currentIndex] = a(i, j); 347 | asub[currentIndex] = i; 348 | aptre[j] = currentIndex + 1; // overriding previous value 349 | ++currentIndex; 350 | } 351 | } 352 | } 353 | 354 | /*Q looks like this 355 | 0 0 0 0 0 0 | -> size * 3 356 | 0 0 2 0 0 0 | -> size *3 357 | 0 0 0 0 0 0 358 | 0 0 0 0 2 0 359 | 0 0 0 0 0 0 360 | */ 361 | /* Number of non-zeros in Q.*/ 362 | const MSKint32t numqz = size * Dim * 2; 363 | MSKint32t* qsubi = new MSKint32t[numqz]; 364 | MSKint32t* qsubj = new MSKint32t[numqz]; 365 | double* qval = new double[numqz]; 366 | for (int id = 0; id < numqz; ++id) { 367 | qsubi[id] = id + ptOff; // we want the x. 368 | qsubj[id] = id + ptOff; 369 | qval[id] = 2; 370 | } 371 | 372 | /* Bounds on constraints. */ 373 | MSKboundkeye* bkc = new MSKboundkeye[numcon]; 374 | 375 | double* blc = new double[numcon]; 376 | double* buc = new double[numcon]; 377 | 378 | for (int i = 0; i < numcon - Dim * 2; ++i) { 379 | bkc[i] = MSK_BK_FX; 380 | blc[i] = 0; 381 | buc[i] = 0; 382 | } 383 | for (int i = numcon - Dim * 2; i < numcon - Dim; ++i) // x0 = x^0 384 | { 385 | bkc[i] = MSK_BK_FX; 386 | blc[i] = wayPointsBegin->second(i - (numcon - Dim * 2)); 387 | buc[i] = wayPointsBegin->second(i - (numcon - Dim * 2)); 388 | } 389 | In last(wayPointsEnd); 390 | --last; 391 | for (int i = numcon - 3; i < numcon; ++i) // xT = x^T 392 | { 393 | bkc[i] = MSK_BK_FX; 394 | blc[i] = last->second(i - (numcon - Dim)); 395 | buc[i] = last->second(i - (numcon - Dim)); 396 | } 397 | 398 | ///*No Bounds on variables. */ 399 | MSKboundkeye* bkx = new MSKboundkeye[numvar]; 400 | 401 | double* blx = new double[numvar]; 402 | double* bux = new double[numvar]; 403 | 404 | for (int i = 0; i < numvar; ++i) { 405 | bkx[i] = MSK_BK_FR; 406 | blx[i] = -MSK_INFINITY; 407 | bux[i] = +MSK_INFINITY; 408 | } 409 | 410 | MSKrescodee r; 411 | MSKtask_t task = NULL; 412 | /* Create the optimization task. */ 413 | r = MSK_maketask(env_, numcon, numvar, &task); 414 | 415 | /* Directs the log task stream to the 'printstr' function. */ 416 | /*if ( r==MSK_RES_OK ) 417 | r = MSK_linkfunctotaskstream(task,MSK_STREAM_LOG,NULL,printstr);*/ 418 | 419 | /* Append 'numcon' empty constraints. 420 | The constraints will initially have no bounds. */ 421 | if (r == MSK_RES_OK) r = MSK_appendcons(task, numcon); 422 | 423 | /* Append 'numvar' variables. 424 | The variables will initially be fixed at zero (x=0). */ 425 | if (r == MSK_RES_OK) r = MSK_appendvars(task, numvar); 426 | 427 | for (int j = 0; j < numvar && r == MSK_RES_OK; ++j) { 428 | /* Set the linear term c_j in the objective.*/ 429 | if (r == MSK_RES_OK) r = MSK_putcj(task, j, 0); 430 | 431 | /* Set the bounds on variable j. 432 | blx[j] <= x_j <= bux[j] */ 433 | if (r == MSK_RES_OK) 434 | r = MSK_putvarbound(task, j, /* Index of variable.*/ 435 | bkx[j], /* Bound key.*/ 436 | blx[j], /* Numerical value of lower bound.*/ 437 | bux[j]); /* Numerical value of upper bound.*/ 438 | 439 | /* Input column j of A */ 440 | if (r == MSK_RES_OK) 441 | r = MSK_putacol(task, j, /* Variable (column) index.*/ 442 | aptre[j] - aptrb[j], /* Number of non-zeros in column j.*/ 443 | asub + aptrb[j], /* Pointer to row indexes of column j.*/ 444 | aval + aptrb[j]); /* Pointer to Values of column j.*/ 445 | } 446 | 447 | /* Set the bounds on constraints. 448 | for i=1, ...,numcon : blc[i] <= constraint i <= buc[i] */ 449 | for (int i = 0; i < numcon && r == MSK_RES_OK; ++i) 450 | r = MSK_putconbound(task, i, /* Index of constraint.*/ 451 | bkc[i], /* Bound key.*/ 452 | blc[i], /* Numerical value of lower bound.*/ 453 | buc[i]); /* Numerical value of upper bound.*/ 454 | 455 | /* Maximize objective function. */ 456 | if (r == MSK_RES_OK) r = MSK_putobjsense(task, MSK_OBJECTIVE_SENSE_MINIMIZE); 457 | 458 | if (r == MSK_RES_OK) { 459 | /* Input the Q for the objective. */ 460 | 461 | r = MSK_putqobj(task, numqz, qsubi, qsubj, qval); 462 | } 463 | 464 | if (r == MSK_RES_OK) { 465 | MSKrescodee trmcode; 466 | 467 | /* Run optimizer */ 468 | r = MSK_optimizetrm(task, &trmcode); 469 | if (r == MSK_RES_OK) { 470 | double* xx = (double*)calloc(numvar, sizeof(double)); 471 | if (xx) { 472 | /* Request the interior point solution. */ 473 | MSK_getxx(task, MSK_SOL_ITR, xx); 474 | T_waypoints_t nwaypoints; 475 | In begin(wayPointsBegin); 476 | for (int i = 0; i < size; i = ++i, ++begin) { 477 | point_t target; 478 | for (int j = 0; j < Dim; ++j) { 479 | target(j) = xx[i + j * size]; 480 | } 481 | nwaypoints.push_back(std::make_pair(begin->first, target)); 482 | } 483 | res = new exact_cubic_t(nwaypoints.begin(), nwaypoints.end()); 484 | free(xx); 485 | } 486 | } 487 | } 488 | /* Delete the task and the associated data. */ 489 | MSK_deletetask(&task); 490 | return res; 491 | } 492 | 493 | } // namespace spline 494 | #endif //_CLASS_SPLINEOPTIMIZER 495 | -------------------------------------------------------------------------------- /include/parametric-curves/polynomial.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file polynomial.hpp 3 | * \brief Definition of a cubic spline. 4 | * \author Steve T. 5 | * \version 0.1 6 | * \date 06/17/2013 7 | * 8 | * This file contains definitions for the Polynomial struct. 9 | * It allows the creation and evaluation of natural 10 | * smooth splines of arbitrary dimension and order 11 | */ 12 | 13 | #ifndef _parameteric_curves_polynomial_hpp 14 | #define _parameteric_curves_polynomial_hpp 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | namespace parametriccurves { 25 | /// \class Polynomial 26 | /// \brief Represents a Polynomialf arbitrary order defined on the interval 27 | /// [tBegin, tEnd]. It follows the equation 28 | /// x(t) = a + b(t - t_min_) + ... + d(t - t_min_)^N, where N is the order 29 | /// 30 | template > 32 | struct Polynomial : public parametriccurves::AbstractCurve { 33 | typedef Point point_t; 34 | typedef Numeric time_t; 35 | typedef Numeric num_t; 36 | typedef std::vector > t_point_t; 37 | typedef AbstractCurve curve_abc_t; 38 | typedef Eigen::Matrix coeff_t; 39 | typedef Eigen::Ref coeff_t_ref; 40 | 41 | public: 42 | ///\brief Constructor 43 | ///\param coefficients : a reference to an Eigen matrix where each column is a 44 | /// coefficient, 45 | /// from the zero order coefficient, up to the highest order. Spline order is 46 | /// given by the number of the columns -1. 47 | ///\param min: LOWER bound on interval definition of the spline 48 | ///\param max: UPPER bound on interval definition of the spline 49 | Polynomial(const coeff_t& coefficients, const time_t tmin, const time_t tmax) 50 | : curve_abc_t(tmin, tmax), 51 | coefficients_(coefficients), 52 | dim_(Dim), 53 | order_(coefficients_.cols() - 1) { 54 | safe_check(); 55 | } 56 | 57 | ///\brief Constructor 58 | ///\param coefficients : a container containing all coefficients of the 59 | /// spline, starting 60 | /// with the zero order coefficient, up to the highest order. Spline order is 61 | /// given by the size of the coefficients 62 | ///\param min: LOWER bound on interval definition of the spline 63 | ///\param max: UPPER bound on interval definition of the spline 64 | Polynomial(const t_point_t& coefficients, const time_t tmin, 65 | const time_t tmax) 66 | : curve_abc_t(tmin, tmax), 67 | coefficients_(init_coeffs(coefficients.begin(), coefficients.end())), 68 | dim_(Dim), 69 | order_(coefficients_.cols() - 1) { 70 | safe_check(); 71 | } 72 | 73 | Polynomial() {} 74 | 75 | ///\brief Constructor 76 | ///\param zeroOrderCoefficient : an iterator pointing to the first element of 77 | /// a structure containing the coefficients 78 | /// it corresponds to the zero degree coefficient 79 | ///\param out : an iterator pointing to the last element of a structure 80 | /// ofcoefficients \param min: LOWER bound on interval definition of the 81 | /// spline \param max: UPPER bound on interval definition of the spline 82 | template 83 | Polynomial(In zeroOrderCoefficient, In out, const time_t tmin, 84 | const time_t tmax) 85 | : curve_abc_t(tmin, tmax), 86 | coefficients_(init_coeffs(zeroOrderCoefficient, out)), 87 | dim_(Dim), 88 | order_(coefficients_.cols() - 1) { 89 | safe_check(); 90 | } 91 | 92 | ///\brief Destructor 93 | ~Polynomial() { 94 | // NOTHING 95 | } 96 | 97 | Polynomial(const Polynomial& other) 98 | : curve_abc_t(other.t_min, other.t_max), 99 | coefficients_(other.coefficients_), 100 | dim_(other.dim_), 101 | order_(other.order_) {} 102 | 103 | // Polynomial& operator=(const Polynomial& other); 104 | 105 | private: 106 | void safe_check() { 107 | if (this->t_min > this->t_max) std::out_of_range("TODO"); 108 | if (static_cast(coefficients_.size()) != order_ + 1) 109 | std::runtime_error("Spline order and coefficients do not match"); 110 | } 111 | 112 | /* Constructors - destructors */ 113 | 114 | /*Operations*/ 115 | public: 116 | /*/// \brief Evaluation of the cubic spline at time t. 117 | /// \param t : the time when to evaluate the spine 118 | /// \param return : the value x(t) 119 | virtual point_t operator()(const time_t t) const 120 | { 121 | if((t < t_min_ || t > t_max_) && Safe){ throw 122 | std::out_of_range("TODO");} time_t const dt (t-t_min_); time_t cdt(1); 123 | point_t currentPoint_ = point_t::Zero(); 124 | for(int i = 0; i < order_+1; ++i, cdt*=dt) 125 | currentPoint_ += cdt *coefficients_.col(i); 126 | return currentPoint_; 127 | }*/ 128 | 129 | /// \brief Evaluation of the cubic spline at time t using horner's scheme. 130 | /// \param t : the time when to evaluate the spine 131 | /// \param return : the value x(t) 132 | virtual const point_t operator()(const time_t& t) const { 133 | if ((t < this->t_min || t > this->t_max)) { 134 | throw std::out_of_range("TODO"); 135 | } 136 | const time_t& dt(t); 137 | point_t h = coefficients_.col(order_); 138 | std::size_t i = order_ - 1; 139 | bool ok = true; 140 | if (order_ != 0) { 141 | while (ok && order_ != 0) { 142 | h = dt * h + coefficients_.col(i); 143 | if (i == 0) 144 | ok = false; 145 | else 146 | i--; 147 | } 148 | return h; 149 | } else 150 | return h; 151 | } 152 | 153 | /// \brief Evaluation of the derivative spline at time t. 154 | /// \param t : the time when to evaluate the spline 155 | /// \param order : order of the derivative 156 | /// \param return : the value x(t) 157 | virtual const point_t derivate(const time_t& t, 158 | const std::size_t& order) const { 159 | if ((t < this->t_min || t > this->t_max)) { 160 | throw std::out_of_range("TODO"); 161 | } 162 | const time_t& dt(t); 163 | time_t cdt(1); 164 | point_t currentPoint_ = point_t::Zero(dim_); 165 | for (std::size_t i = order; i < order_ + 1; ++i, cdt *= dt) 166 | currentPoint_ += cdt * coefficients_.col(i) * fact(i, order); 167 | return currentPoint_; 168 | } 169 | 170 | virtual const std::size_t& size() const { return dim_; } 171 | 172 | virtual bool setInitialPoint(const point_t& /*x_init*/) { return false; } 173 | virtual bool setInitialPoint(const num_t& /*x_init*/) { return false; } 174 | 175 | protected: 176 | coeff_t coefficients_; // const 177 | std::size_t dim_; // const 178 | std::size_t order_; // const 179 | 180 | private: 181 | // Serialization of the class 182 | friend class boost::serialization::access; 183 | template 184 | void save(Archive& ar, const unsigned int /*version*/) const { 185 | ar& boost::serialization::make_nvp("dim", dim_); 186 | ar& boost::serialization::make_nvp("order", order_); 187 | ar& boost::serialization::make_nvp("coefficients", coefficients_); 188 | ar& boost::serialization::make_nvp("t_min", this->t_min); 189 | ar& boost::serialization::make_nvp("t_max", this->t_max); 190 | } 191 | 192 | template 193 | void load(Archive& ar, const unsigned int /*version*/) { 194 | ar& boost::serialization::make_nvp("dim", dim_); 195 | ar& boost::serialization::make_nvp("order", order_); 196 | ar& boost::serialization::make_nvp("coefficients", coefficients_); 197 | ar& boost::serialization::make_nvp("t_min", this->t_min); 198 | ar& boost::serialization::make_nvp("t_max", this->t_max); 199 | } 200 | 201 | BOOST_SERIALIZATION_SPLIT_MEMBER() 202 | 203 | num_t fact(const std::size_t n, const std::size_t order) const { 204 | std::size_t res(1); 205 | for (std::size_t i = 0; i < order; ++i) res *= n - i; 206 | return static_cast(res); 207 | } 208 | 209 | /*Attributes*/ 210 | template 211 | coeff_t init_coeffs(In zeroOrderCoefficient, In highestOrderCoefficient) { 212 | std::size_t size = 213 | std::distance(zeroOrderCoefficient, highestOrderCoefficient); 214 | coeff_t res = coeff_t(Dim, size); 215 | int i = 0; 216 | for (In cit = zeroOrderCoefficient; cit != highestOrderCoefficient; 217 | ++cit, ++i) 218 | res.col(i) = *cit; 219 | return res; 220 | } 221 | }; // class Polynomial 222 | } // namespace parametriccurves 223 | #endif //_STRUCT_POLYNOMIAL 224 | -------------------------------------------------------------------------------- /include/parametric-curves/serialization/archive.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __parametric_curves_serialization_archive_hpp__ 2 | #define __parametric_curves_serialization_archive_hpp__ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | namespace serialization { 15 | template 16 | struct Serializable { 17 | private: 18 | Derived& derived() { return *static_cast(this); } 19 | const Derived& derived() const { return *static_cast(this); } 20 | 21 | public: 22 | /// \brief Loads a Derived object from a text file. 23 | void loadFromText(const std::string& filename) { 24 | std::ifstream ifs(filename.c_str()); 25 | if (ifs) { 26 | boost::archive::text_iarchive ia(ifs); 27 | ia >> derived(); 28 | } else { 29 | const std::string exception_message(filename + 30 | " does not seem to be a valid file."); 31 | throw std::invalid_argument(exception_message); 32 | } 33 | } 34 | 35 | /// \brief Saved a Derived object as a text file. 36 | void saveAsText(const std::string& filename) const { 37 | std::ofstream ofs(filename.c_str()); 38 | if (ofs) { 39 | boost::archive::text_oarchive oa(ofs); 40 | oa << derived(); 41 | } else { 42 | const std::string exception_message(filename + 43 | " does not seem to be a valid file."); 44 | throw std::invalid_argument(exception_message); 45 | } 46 | } 47 | 48 | /// \brief Loads a Derived object from an XML file. 49 | void loadFromXML(const std::string& filename, const std::string& tag_name) { 50 | assert(!tag_name.empty()); 51 | std::ifstream ifs(filename.c_str()); 52 | if (ifs) { 53 | boost::archive::xml_iarchive ia(ifs); 54 | ia >> boost::serialization::make_nvp(tag_name.c_str(), derived()); 55 | } else { 56 | const std::string exception_message(filename + 57 | " does not seem to be a valid file."); 58 | throw std::invalid_argument(exception_message); 59 | } 60 | } 61 | 62 | /// \brief Saved a Derived object as an XML file. 63 | void saveAsXML(const std::string& filename, 64 | const std::string& tag_name) const { 65 | assert(!tag_name.empty()); 66 | std::ofstream ofs(filename.c_str()); 67 | if (ofs) { 68 | boost::archive::xml_oarchive oa(ofs); 69 | oa << boost::serialization::make_nvp(tag_name.c_str(), derived()); 70 | } else { 71 | const std::string exception_message(filename + 72 | " does not seem to be a valid file."); 73 | throw std::invalid_argument(exception_message); 74 | } 75 | } 76 | 77 | /// \brief Loads a Derived object from an binary file. 78 | void loadFromBinary(const std::string& filename) { 79 | std::ifstream ifs(filename.c_str()); 80 | if (ifs) { 81 | boost::archive::binary_iarchive ia(ifs); 82 | ia >> derived(); 83 | } else { 84 | const std::string exception_message(filename + 85 | " does not seem to be a valid file."); 86 | throw std::invalid_argument(exception_message); 87 | } 88 | } 89 | 90 | /// \brief Saved a Derived object as an binary file. 91 | void saveAsBinary(const std::string& filename) const { 92 | std::ofstream ofs(filename.c_str()); 93 | if (ofs) { 94 | boost::archive::binary_oarchive oa(ofs); 95 | oa << derived(); 96 | } else { 97 | const std::string exception_message(filename + 98 | " does not seem to be a valid file."); 99 | throw std::invalid_argument(exception_message); 100 | } 101 | } 102 | }; 103 | 104 | } // namespace serialization 105 | 106 | #endif // ifndef __parametric_curves_serialization_archive_hpp__ 107 | -------------------------------------------------------------------------------- /include/parametric-curves/serialization/eigen-matrix.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Code adapted from: 3 | https://gist.githubusercontent.com/mtao/5798888/raw/5be9fa9b66336c166dba3a92c0e5b69ffdb81501/eigen_boost_serialization.hpp 4 | Copyright (c) 2015 Michael Tao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in 14 | all copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 22 | THE SOFTWARE. 23 | */ 24 | 25 | #ifndef __parameteric_curves_serialization_eigen_matrix_hpp__ 26 | #define __parameteric_curves_serialization_eigen_matrix_hpp__ 27 | 28 | #include 29 | #include 30 | #include 31 | 32 | namespace boost { 33 | 34 | namespace serialization { 35 | 36 | template 38 | void save( 39 | Archive& ar, 40 | const Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& m, 41 | const unsigned int /*version*/) { 42 | Eigen::DenseIndex rows(m.rows()), cols(m.cols()); 43 | ar& BOOST_SERIALIZATION_NVP(rows); 44 | ar& BOOST_SERIALIZATION_NVP(cols); 45 | ar& make_nvp("data", make_array(m.data(), (size_t)m.size())); 46 | } 47 | 48 | template 50 | void load(Archive& ar, 51 | Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& m, 52 | const unsigned int /*version*/) { 53 | Eigen::DenseIndex rows, cols; 54 | ar >> BOOST_SERIALIZATION_NVP(rows); 55 | ar >> BOOST_SERIALIZATION_NVP(cols); 56 | m.resize(rows, cols); 57 | ar >> make_nvp("data", make_array(m.data(), (size_t)m.size())); 58 | } 59 | 60 | template 62 | void serialize( 63 | Archive& ar, 64 | Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& m, 65 | const unsigned int version) { 66 | split_free(ar, m, version); 67 | } 68 | 69 | } // namespace serialization 70 | } // namespace boost 71 | 72 | #endif // ifndef __parametric_curves_serialization_eigen_matrix_hpp__ 73 | -------------------------------------------------------------------------------- /include/parametric-curves/spatial/force-curve.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file exact_cubic.h 3 | * \brief class allowing to create an Exact cubic spline. 4 | * \author Rohan Budhiraja 5 | * \version 0.1 6 | * \date 09/11/2017 7 | * 8 | * This file contains representation of spatial vectors as splines 9 | */ 10 | 11 | #ifndef _parameteric_curves_force_curve_hpp 12 | #define _parameteric_curves_force_curve_hpp 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | namespace parametriccurves { 23 | namespace spatial { 24 | 25 | using parametriccurves::Spline; 26 | /// \class ForceCurve 27 | /// \brief Representation of a spatial vector curve in the form of splines 28 | /// Returns Plucker coordinates in the form of (Linear(3), Angular(3)) 29 | /// 30 | template 31 | struct ForceCurve 32 | : public AbstractCurve > { 33 | static const std::size_t Dim = 3; 34 | typedef Eigen::Matrix force_t; 35 | typedef Eigen::Matrix motion_t; 36 | typedef Spline > spline_lin_t; 37 | typedef Spline > spline_ang_t; 38 | typedef Numeric time_t; 39 | typedef Numeric num_t; 40 | typedef AbstractCurve curve_abc_t; 41 | 42 | public: 43 | ///\brief Constructor 44 | 45 | ForceCurve() : curve_abc_t() {} 46 | 47 | ///\brief Constructor 48 | ///\param subSplines: vector of subsplines 49 | ForceCurve(const spline_lin_t& linPart_, const spline_ang_t& angPart_) 50 | : curve_abc_t(linPart_.tmin(), linPart_.tmax()), 51 | linPart(linPart_), 52 | angPart(angPart_), 53 | motionVector(motion_t::Zero()) {} 54 | 55 | ///\brief Copy Constructor 56 | ForceCurve(const ForceCurve& other) 57 | : curve_abc_t(other.linPart.tmin(), other.linPart.tmax()), 58 | linPart(other.linPart), 59 | angPart(other.angPart), 60 | motionVector(motion_t::Zero()) {} 61 | 62 | ///\brief Destructor 63 | ~ForceCurve() {} 64 | 65 | public: 66 | virtual const force_t operator()(const time_t& t) const { 67 | force_t s = force_t::Zero(); 68 | s << linPart(t), angPart(t); 69 | return s; 70 | } 71 | 72 | virtual const force_t derivate(const time_t&, const std::size_t&) const { 73 | // TODO: Implement derivative 74 | return force_t::Zero(Dim); 75 | } 76 | 77 | virtual const std::size_t& size() const { return linPart.size(); } 78 | 79 | void setMotionVector(const motion_t& motionVector_) { 80 | motionVector = motionVector_; 81 | return; 82 | } 83 | 84 | virtual bool setInitialPoint(const force_t& /*x_init*/) { return false; } 85 | virtual bool setInitialPoint(const num_t& /*x_init*/) { return false; } 86 | 87 | protected: 88 | /*Attributes*/ 89 | spline_lin_t linPart; // const 90 | spline_ang_t angPart; // const 91 | motion_t motionVector; // const 92 | 93 | private: 94 | // Serialization of the class 95 | friend class boost::serialization::access; 96 | template 97 | void save(Archive& ar, const unsigned int /*version*/) const { 98 | ar& linPart; 99 | ar& angPart; 100 | 101 | return; 102 | } 103 | 104 | template 105 | void load(Archive& ar, const unsigned int /*version*/) { 106 | ar& linPart; 107 | ar& angPart; 108 | 109 | motionVector = motion_t::Zero(); 110 | this->t_min = linPart.tmin(); 111 | this->t_max = linPart.tmax(); 112 | 113 | assert(this->t_min == angPart.tmin()); 114 | assert(this->t_max == angPart.tmax()); 115 | return; 116 | } 117 | 118 | BOOST_SERIALIZATION_SPLIT_MEMBER() 119 | 120 | public: 121 | bool loadFromFile(const std::string& filename) { 122 | std::ifstream ifs(filename.c_str()); 123 | if (ifs) { 124 | boost::archive::text_iarchive ia(ifs); 125 | ForceCurve& force_curve = *static_cast(this); 126 | ia >> force_curve; 127 | } else { 128 | const std::string exception_message(filename + 129 | " does not seem to be a valid file."); 130 | throw std::invalid_argument(exception_message); 131 | return false; 132 | } 133 | return true; 134 | } 135 | 136 | /// \brief Saved a Derived object as a text file. 137 | bool saveToFile(const std::string& filename) const { 138 | std::ofstream ofs(filename.c_str()); 139 | if (ofs) { 140 | boost::archive::text_oarchive oa(ofs); 141 | oa << *static_cast(this); 142 | } else { 143 | const std::string exception_message(filename + 144 | " does not seem to be a valid file."); 145 | throw std::invalid_argument(exception_message); 146 | return false; 147 | } 148 | return true; 149 | } 150 | 151 | // BOOST_SERIALIZATION_SPLIT_MEMBER() 152 | }; 153 | } // namespace spatial 154 | } // namespace parametriccurves 155 | #endif //_CLASS_EXACTCUBIC 156 | -------------------------------------------------------------------------------- /include/parametric-curves/spline.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file exact_cubic.h 3 | * \brief class allowing to create an Exact cubic spline. 4 | * \author Steve T. 5 | * \version 0.1 6 | * \date 06/17/2013 7 | * 8 | * This file contains definitions for the Spline class. 9 | */ 10 | 11 | #ifndef _parameteric_curves_spline_hpp 12 | #define _parameteric_curves_spline_hpp 13 | 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | namespace parametriccurves { 26 | 27 | /// \brief Creates coefficient vector of a cubic spline defined on the interval 28 | /// [tBegin, tEnd]. It follows the equation 29 | /// x(t) = a + b(t - t_min_) + c(t - t_min_)^2 + d(t - t_min_)^3 30 | /// 31 | template 32 | T_Point make_cubic_vector(Point const& a, Point const& b, Point const& c, 33 | Point const& d) { 34 | T_Point res; 35 | res.push_back(a); 36 | res.push_back(b); 37 | res.push_back(c); 38 | res.push_back(d); 39 | return res; 40 | } 41 | 42 | template 43 | Polynomial create_cubic(Point const& a, Point const& b, 44 | Point const& c, Point const& d, 45 | const Numeric min, 46 | const Numeric max) { 47 | T_Point coeffs = make_cubic_vector(a, b, c, d); 48 | return Polynomial(coeffs.begin(), coeffs.end(), min, 49 | max); 50 | } 51 | /// \brief Creates coefficient vector of a quintic spline defined on the 52 | /// interval [tBegin, tEnd]. It follows the equation x(t) = a + b(t - t_min_) + 53 | /// c(t - t_min_)^2 + d(t - t_min_)^3 + e(t - t_min_)^4 + f(t - t_min_)^5 54 | /// 55 | template 56 | T_Point make_quintic_vector(Point const& a, Point const& b, Point const& c, 57 | Point const& d, Point const& e, Point const& f) { 58 | T_Point res; 59 | res.push_back(a); 60 | res.push_back(b); 61 | res.push_back(c); 62 | res.push_back(d); 63 | res.push_back(e); 64 | res.push_back(f); 65 | return res; 66 | } 67 | 68 | template 69 | Polynomial create_quintic(Point const& a, Point const& b, 70 | Point const& c, Point const& d, 71 | Point const& e, Point const& f, 72 | const Numeric min, 73 | const Numeric max) { 74 | T_Point coeffs = make_quintic_vector(a, b, c, d, e, f); 75 | return Polynomial(coeffs.begin(), coeffs.end(), min, 76 | max); 77 | } 78 | 79 | /// \class Spline 80 | /// \brief Represents a set of cubic splines defining a continuous function 81 | /// crossing each of the waypoint given in its initialization 82 | /// 83 | template , 85 | typename SplineBase = Polynomial > 86 | struct Spline : public AbstractCurve { 87 | typedef Point point_t; 88 | typedef std::vector > t_point_t; 89 | typedef Eigen::Matrix MatrixX; 90 | typedef Numeric time_t; 91 | typedef Numeric num_t; 92 | typedef SplineBase spline_t; 93 | typedef typename std::vector > 94 | t_spline_t; 95 | typedef typename t_spline_t::iterator it_spline_t; 96 | typedef typename t_spline_t::const_iterator cit_spline_t; 97 | typedef curve_constraints spline_constraints; 98 | typedef AbstractCurve curve_abc_t; 99 | 100 | public: 101 | ///\brief Constructor 102 | 103 | Spline() : curve_abc_t() {} 104 | 105 | ///\brief Constructor 106 | ///\param subSplines: vector of subsplines 107 | Spline(const t_spline_t& subSplines) 108 | : curve_abc_t(subSplines.front().tmin(), subSplines.back().tmax()), 109 | subSplines_(subSplines) {} 110 | 111 | ///\brief Copy Constructor 112 | Spline(const Spline& other) 113 | : curve_abc_t(other.subSplines_.front().tmin(), 114 | other.subSplines_.front().tmax()), 115 | subSplines_(other.subSplines_) {} 116 | 117 | ///\brief Destructor 118 | ~Spline() {} 119 | 120 | public: 121 | /* Given a set of waypoints (x_i*) and timestep (t_i), it provides the unique 122 | * set of cubic splines fulfulling those 4 restrictions : 123 | * - x_i(t_i) = x_i* ; this means that the curve passes through each waypoint 124 | * - x_i(t_i+1) = x_i+1* ; 125 | * - its derivative is continous at t_i+1 126 | * - its 2nd derivative is continous at t_i+1 127 | * more details in paper "Task-Space Trajectories via Cubic Spline 128 | * Optimization" By J. Zico Kolter and Andrew Y.ng (ICRA 2009) */ 129 | template 130 | void createSplineFromWayPoints(In wayPointsBegin, In wayPointsEnd) { 131 | std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd)); 132 | if (size < 1) { 133 | throw; // TODO 134 | } 135 | subSplines_.clear(); 136 | subSplines_.reserve(size); 137 | 138 | // refer to the paper to understand all this. 139 | MatrixX h1 = MatrixX::Zero(size, size); 140 | MatrixX h2 = MatrixX::Zero(size, size); 141 | MatrixX h3 = MatrixX::Zero(size, size); 142 | MatrixX h4 = MatrixX::Zero(size, size); 143 | MatrixX h5 = MatrixX::Zero(size, size); 144 | MatrixX h6 = MatrixX::Zero(size, size); 145 | 146 | MatrixX a = MatrixX::Zero(size, Dim); 147 | MatrixX b = MatrixX::Zero(size, Dim); 148 | MatrixX c = MatrixX::Zero(size, Dim); 149 | MatrixX d = MatrixX::Zero(size, Dim); 150 | MatrixX x = MatrixX::Zero(size, Dim); 151 | In it(wayPointsBegin), next(wayPointsBegin); 152 | ++next; 153 | 154 | for (std::size_t i(0); next != wayPointsEnd; ++next, ++it, ++i) { 155 | num_t const dTi((*next).first - (*it).first); 156 | num_t const dTi_sqr(dTi * dTi); 157 | num_t const dTi_cube(dTi_sqr * dTi); 158 | // filling matrices values 159 | h3(i, i) = -3 / dTi_sqr; 160 | h3(i, i + 1) = 3 / dTi_sqr; 161 | h4(i, i) = -2 / dTi; 162 | h4(i, i + 1) = -1 / dTi; 163 | h5(i, i) = 2 / dTi_cube; 164 | h5(i, i + 1) = -2 / dTi_cube; 165 | h6(i, i) = 1 / dTi_sqr; 166 | h6(i, i + 1) = 1 / dTi_sqr; 167 | if (i + 2 < size) { 168 | In it2(next); 169 | ++it2; 170 | num_t const dTi_1((*it2).first - (*next).first); 171 | num_t const dTi_1sqr(dTi_1 * dTi_1); 172 | // this can be optimized but let's focus on clarity as long as not 173 | // needed 174 | h1(i + 1, i) = 2 / dTi; 175 | h1(i + 1, i + 1) = 4 / dTi + 4 / dTi_1; 176 | h1(i + 1, i + 2) = 2 / dTi_1; 177 | h2(i + 1, i) = -6 / dTi_sqr; 178 | h2(i + 1, i + 1) = (6 / dTi_1sqr) - (6 / dTi_sqr); 179 | h2(i + 1, i + 2) = 6 / dTi_1sqr; 180 | } 181 | x.row(i) = (*it).second.transpose(); 182 | } 183 | // adding last x 184 | x.row(size - 1) = (*it).second.transpose(); 185 | a = x; 186 | parametriccurves::PseudoInverse(h1); 187 | b = h1 * h2 * x; // h1 * b = h2 * x => b = (h1)^-1 * h2 * x 188 | c = h3 * x + h4 * b; 189 | d = h5 * x + h6 * b; 190 | it = wayPointsBegin, next = wayPointsBegin; 191 | ++next; 192 | 193 | for (int i = 0; next != wayPointsEnd; ++i, ++it, ++next) { 194 | Numeric min = (*it).first; 195 | Numeric max = (*next).first; 196 | Point a_ = a.row(i) - b.row(i) * min + c.row(i) * min * min - 197 | d.row(i) * min * min * min; 198 | Point b_ = b.row(i) - 2 * c.row(i) * min + 3 * d.row(i) * min * min; 199 | Point c_ = c.row(i) - 3 * d.row(i) * min; 200 | Point d_ = d.row(i); 201 | subSplines_.push_back(create_cubic( 202 | a_, b_, c_, d_, min, max)); 203 | } 204 | Numeric min = (*it).first; 205 | Point a_ = a.row(size - 1) - b.row(size - 1) * min + 206 | c.row(size - 1) * min * min - d.row(size - 1) * min * min * min; 207 | Point b_ = b.row(size - 1) - 2 * c.row(size - 1) * min + 208 | 3 * d.row(size - 1) * min * min; 209 | Point c_ = c.row(size - 1) - 3 * d.row(size - 1) * min; 210 | Point d_ = d.row(size - 1); 211 | subSplines_.push_back( 212 | create_cubic(a_, b_, c_, d_, min, min)); 213 | 214 | this->t_min = subSplines_.front().tmin(); 215 | this->t_max = subSplines_.back().tmax(); 216 | return; 217 | } 218 | 219 | template 220 | void createSplineFromWayPointsConstr(In wayPointsBegin, In wayPointsEnd, 221 | const spline_constraints& constraints) { 222 | std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd)); 223 | if (size < 1) throw; // TODO 224 | subSplines_.clear(); 225 | subSplines_.reserve(size); 226 | spline_constraints cons = constraints; 227 | In it(wayPointsBegin), next(wayPointsBegin), end(wayPointsEnd - 1); 228 | ++next; 229 | for (std::size_t i(0); next != end; ++next, ++it, ++i) 230 | compute_one_spline(it, next, cons, subSplines_); 231 | compute_end_spline(it, next, cons, subSplines_); 232 | return; 233 | } 234 | 235 | public: 236 | virtual const point_t operator()(const time_t& t) const { 237 | if ((t < subSplines_.front().tmin() || t > subSplines_.back().tmax())) { 238 | throw std::out_of_range("t is out of range"); 239 | } 240 | for (cit_spline_t it = subSplines_.begin(); it != subSplines_.end(); ++it) { 241 | if (t >= (it->tmin()) && t <= (it->tmax())) { 242 | return it->operator()(t); 243 | } 244 | } 245 | const point_t dummy; 246 | return dummy; 247 | } 248 | 249 | virtual const point_t derivate(const time_t& t, 250 | const std::size_t& order) const { 251 | if ((t < subSplines_.front().tmin() || t > subSplines_.back().tmax())) { 252 | throw std::out_of_range("derivative call out of range"); 253 | } 254 | for (cit_spline_t it = subSplines_.begin(); it != subSplines_.end(); ++it) { 255 | if (t >= (it->tmin()) && t <= (it->tmax())) { 256 | return it->derivate(t, order); 257 | } 258 | } 259 | 260 | const point_t dummy; 261 | return dummy; 262 | } 263 | 264 | virtual const std::size_t& size() const { return subSplines_[0].size(); } 265 | const t_spline_t& getSubsplines() const { return subSplines_; } 266 | 267 | virtual bool setInitialPoint(const point_t& /*x_init*/) { return false; } 268 | virtual bool setInitialPoint(const num_t& /*x_init*/) { return false; } 269 | 270 | protected: 271 | /*Attributes*/ 272 | t_spline_t subSplines_; // const 273 | 274 | private: 275 | template 276 | void compute_one_spline(In wayPointsBegin, In wayPointsNext, 277 | spline_constraints& constraints, 278 | t_spline_t& subSplines) const { 279 | const point_t &a0 = wayPointsBegin->second, a1 = wayPointsNext->second; 280 | const point_t &b0 = constraints.init_vel, c0 = constraints.init_acc / 2.; 281 | const num_t &init_t = wayPointsBegin->first, end_t = wayPointsNext->first; 282 | const num_t dt = end_t - init_t, dt_2 = dt * dt, dt_3 = dt_2 * dt; 283 | const point_t d0 = (a1 - a0 - b0 * dt - c0 * dt_2) / dt_3; 284 | 285 | Point a_ = 286 | a0 - b0 * init_t + c0 * init_t * init_t - d0 * init_t * init_t * init_t; 287 | Point b_ = b0 - 2 * c0 * init_t + 3 * d0 * init_t * init_t; 288 | Point c_ = c0 - 3 * d0 * init_t; 289 | Point d_ = d0; 290 | subSplines.push_back(create_cubic( 291 | a_, b_, c_, d_, init_t, end_t)); 292 | 293 | constraints.init_vel = subSplines.back().derivate(end_t, 1); 294 | constraints.init_acc = subSplines.back().derivate(end_t, 2); 295 | } 296 | 297 | template 298 | void compute_end_spline(In wayPointsBegin, In wayPointsNext, 299 | spline_constraints& constraints, 300 | t_spline_t& subSplines) const { 301 | const point_t &a0 = wayPointsBegin->second, a1 = wayPointsNext->second; 302 | const point_t &b0 = constraints.init_vel, b1 = constraints.end_vel, 303 | c0 = constraints.init_acc / 2., c1 = constraints.end_acc; 304 | const num_t &init_t = wayPointsBegin->first, end_t = wayPointsNext->first; 305 | const num_t dt = end_t - init_t, dt_2 = dt * dt, dt_3 = dt_2 * dt, 306 | dt_4 = dt_3 * dt, dt_5 = dt_4 * dt; 307 | // solving a system of four linear eq with 4 unknows: d0, e0 308 | const point_t alpha_0 = a1 - a0 - b0 * dt - c0 * dt_2; 309 | const point_t alpha_1 = b1 - b0 - 2 * c0 * dt; 310 | const point_t alpha_2 = c1 - 2 * c0; 311 | const num_t x_d_0 = dt_3, x_d_1 = 3 * dt_2, x_d_2 = 6 * dt; 312 | const num_t x_e_0 = dt_4, x_e_1 = 4 * dt_3, x_e_2 = 12 * dt_2; 313 | const num_t x_f_0 = dt_5, x_f_1 = 5 * dt_4, x_f_2 = 20 * dt_3; 314 | 315 | point_t d, e, f; 316 | Eigen::MatrixXd rhs = Eigen::MatrixXd::Zero(3, Dim); 317 | rhs.row(0) = alpha_0; 318 | rhs.row(1) = alpha_1; 319 | rhs.row(2) = alpha_2; 320 | Eigen::Matrix3d eq = Eigen::Matrix3d::Zero(); 321 | eq(0, 0) = x_d_0; 322 | eq(0, 1) = x_e_0; 323 | eq(0, 2) = x_f_0; 324 | eq(1, 0) = x_d_1; 325 | eq(1, 1) = x_e_1; 326 | eq(1, 2) = x_f_1; 327 | eq(2, 0) = x_d_2; 328 | eq(2, 1) = x_e_2; 329 | eq(2, 2) = x_f_2; 330 | rhs = eq.inverse().eval() * rhs; 331 | d = rhs.row(0); 332 | e = rhs.row(1); 333 | f = rhs.row(2); 334 | num_t min = init_t; 335 | Numeric min2 = min * min; 336 | Numeric min3 = min2 * min; 337 | Numeric min4 = min3 * min; 338 | Numeric min5 = min4 * min; 339 | Point a_ = a0 - b0 * min + c0 * min2 - d * min3 + e * min4 - f * min5; 340 | Point b_ = b0 - 2 * c0 * min + 3 * d * min2 - 4 * e * min3 + 5 * f * min4; 341 | Point c_ = c0 - 3 * d * min + 6 * e * min2 - 10 * f * min3; 342 | Point d_ = d - 4 * e * min + 10 * f * min2; 343 | Point e_ = e - 5 * f * min; 344 | Point f_ = f; 345 | 346 | subSplines.push_back(create_quintic( 347 | a_, b_, c_, d_, e_, f_, init_t, end_t)); 348 | } 349 | 350 | // Serialization of the class 351 | friend class boost::serialization::access; 352 | template 353 | void save(Archive& ar, const unsigned int /*version*/) const { 354 | ar& subSplines_; 355 | 356 | return; 357 | } 358 | 359 | template 360 | void load(Archive& ar, const unsigned int /*version*/) { 361 | ar& subSplines_; 362 | 363 | this->t_min = subSplines_.front().tmin(); 364 | this->t_max = subSplines_.back().tmax(); 365 | return; 366 | } 367 | 368 | BOOST_SERIALIZATION_SPLIT_MEMBER() 369 | 370 | public: 371 | bool loadFromFile(const std::string& filename) { 372 | std::ifstream ifs(filename.c_str()); 373 | if (ifs) { 374 | boost::archive::text_iarchive ia(ifs); 375 | Spline& cubic_spline = *static_cast(this); 376 | ia >> cubic_spline; 377 | } else { 378 | const std::string exception_message(filename + 379 | " does not seem to be a valid file."); 380 | throw std::invalid_argument(exception_message); 381 | return false; 382 | } 383 | return true; 384 | } 385 | 386 | /// \brief Saved a Derived object as a text file. 387 | bool saveToFile(const std::string& filename) const { 388 | std::ofstream ofs(filename.c_str()); 389 | if (ofs) { 390 | boost::archive::text_oarchive oa(ofs); 391 | oa << *static_cast(this); 392 | } else { 393 | const std::string exception_message(filename + 394 | " does not seem to be a valid file."); 395 | throw std::invalid_argument(exception_message); 396 | return false; 397 | } 398 | return true; 399 | } 400 | 401 | // BOOST_SERIALIZATION_SPLIT_MEMBER() 402 | }; 403 | } // namespace parametriccurves 404 | #endif //_CLASS_EXACTCUBIC 405 | -------------------------------------------------------------------------------- /include/parametric-curves/text-file.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file text-file.hpp 3 | * \brief Reads from text file 4 | * 5 | */ 6 | 7 | #ifndef _parameteric_curves_text_file_hpp 8 | #define _parameteric_curves_text_file_hpp 9 | 10 | #include 11 | #include 12 | 13 | namespace parametriccurves { 14 | 15 | /// \class TextFile. 16 | /// \brief Loads curve from file 17 | template > 19 | struct TextFile : public AbstractCurve { 20 | typedef Point point_t; 21 | typedef Numeric time_t; 22 | typedef Numeric num_t; 23 | typedef Eigen::Matrix pos_t; 24 | typedef Eigen::Matrix vel_t; 25 | typedef Eigen::Matrix acc_t; 26 | 27 | typedef AbstractCurve curve_abc_t; 28 | 29 | public: 30 | ///\brief Constructor 31 | 32 | TextFile(const time_t& dt_, const std::size_t& size_) 33 | : curve_abc_t(-1, -1), timeStep(dt_), size(size_) {} 34 | 35 | ///\brief Destructor 36 | ~TextFile() {} 37 | 38 | public: 39 | virtual const point_t operator()(const time_t& t) const { 40 | Eigen::VectorXd::Index i = (Eigen::VectorXd::Index)std::floor(t / timeStep); 41 | return posValues.row(i); 42 | } 43 | 44 | virtual const point_t derivate(const time_t& t, 45 | const std::size_t& order) const { 46 | Eigen::VectorXd::Index i = (Eigen::VectorXd::Index)std::floor(t / timeStep); 47 | if (order == 1) 48 | return velValues.row(i); 49 | else if (order == 2) 50 | return accValues.row(i); 51 | else { 52 | std::cerr << "Higher order derivatives not supported" << std::endl; 53 | return point_t::Zero(size); 54 | } 55 | } 56 | 57 | public: 58 | virtual bool loadTextFile(const std::string& fileName) { 59 | Eigen::MatrixXd data = 60 | parametriccurves::utils::readMatrixFromFile(fileName); 61 | if (data.cols() == size) { 62 | std::cout << fileName << ": setting derivatives to zero" << std::endl; 63 | posValues = data; 64 | velValues.setZero(data.rows(), size); 65 | accValues.setZero(data.rows(), size); 66 | } else if (data.cols() == 2 * size) { 67 | std::cout << fileName << ": setting second derivative to zero" 68 | << std::endl; 69 | posValues = data.leftCols(size); 70 | velValues = data.rightCols(size); 71 | accValues = accValues.setZero(data.rows(), size); 72 | } else if (data.cols() == 3 * size) { 73 | posValues = data.leftCols(size); 74 | velValues = data.middleCols(size, size); 75 | accValues = data.rightCols(size); 76 | } else { 77 | std::cout << "Unexpected number of columns (expected " << size << " or " 78 | << 2 * size << " or " << 3 * size << ", found " << data.cols() 79 | << ")\n"; 80 | return false; 81 | } 82 | this->t_max = timeStep * (double)data.rows(); 83 | this->t_min = 0.0; 84 | x_init = posValues.row(0); 85 | return true; 86 | } 87 | 88 | virtual bool setInitialPoint(const point_t& /*x_init*/) { return false; } 89 | virtual bool setInitialPoint(const num_t& /*x_init*/) { return false; } 90 | 91 | const point_t& getInitialPoint(void) const { return x_init; } 92 | 93 | protected: 94 | /*Attributes*/ 95 | point_t x_init; 96 | pos_t posValues; 97 | vel_t velValues; 98 | acc_t accValues; 99 | time_t timeStep; 100 | std::size_t size; 101 | }; 102 | } // namespace parametriccurves 103 | #endif //_CLASS_EXACTCUBIC 104 | -------------------------------------------------------------------------------- /include/parametric-curves/utils/file-io.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _parameteric_curves_utils_file_io_hpp 2 | #define _parameteric_curves_utils_file_io_hpp 3 | 4 | #include 5 | #include /// to read text file 6 | 7 | #define MAXBUFSIZE ((int)1000000) 8 | 9 | namespace parametriccurves { 10 | namespace utils { 11 | 12 | const Eigen::MatrixXd readMatrixFromFile(const std::string& filename) { 13 | int cols = 0, rows = 0; 14 | double buff[MAXBUFSIZE]; 15 | 16 | // Read numbers from file into buffer. 17 | std::ifstream infile; 18 | infile.open(filename.c_str()); 19 | while (!infile.eof()) { 20 | std::string line; 21 | getline(infile, line); 22 | // std::cout<<"Read line "<> buff[cols * rows + temp_cols++]; 27 | 28 | if (temp_cols == 0) continue; 29 | 30 | if (cols == 0) 31 | cols = temp_cols; 32 | else if (temp_cols != cols && !infile.eof()) { 33 | std::cout << "Error while reading matrix from file, line " << rows 34 | << " has " << temp_cols 35 | << " columnds, while preceding lines had " << cols 36 | << " columnds\n"; 37 | std::cout << line << "\n"; 38 | break; 39 | } 40 | 41 | rows++; 42 | if ((rows + 1) * cols >= MAXBUFSIZE) { 43 | std::cout << "Max buffer size exceeded (" << rows << " rows, " << cols 44 | << " cols)\n"; 45 | break; 46 | } 47 | } 48 | infile.close(); 49 | rows--; 50 | 51 | // Populate matrix with numbers. 52 | Eigen::MatrixXd result(rows, cols); 53 | for (int i = 0; i < rows; i++) 54 | for (int j = 0; j < cols; j++) result(i, j) = buff[cols * i + j]; 55 | return result; 56 | } 57 | 58 | } // namespace utils 59 | } // namespace parametriccurves 60 | 61 | #endif //_CLASS_EXACTCUBIC 62 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [tool.black] 2 | exclude = "cmake" 3 | -------------------------------------------------------------------------------- /python/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(${PYWRAP} SHARED spline_python.cpp) 2 | target_link_libraries(${PYWRAP} eigenpy::eigenpy Boost::serialization) 3 | 4 | set_target_properties(${PYWRAP} PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${PY_NAME}) 5 | 6 | if(APPLE) 7 | # We need to change the extension for python bindings 8 | set_target_properties(${PYWRAP} PROPERTIES SUFFIX ".so") 9 | endif(APPLE) 10 | 11 | install(TARGETS ${PYWRAP} DESTINATION "${PYTHON_SITELIB}/${PY_NAME}") 12 | 13 | python_install_on_site(${PY_NAME} "__init__.py") 14 | -------------------------------------------------------------------------------- /python/parametric_curves/__init__.py: -------------------------------------------------------------------------------- 1 | from .libparametric_curves_pywrap import * # noqa 2 | -------------------------------------------------------------------------------- /python/spline_python.cpp: -------------------------------------------------------------------------------- 1 | // #include "parametriccurves/bezier_curve.h" 2 | #include 3 | #include 4 | #include 5 | 6 | // #include "parametriccurves/splines/spline_deriv_constraint.h" 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | /*** TEMPLATE SPECIALIZATION FOR PYTHON ****/ 14 | namespace bp = boost::python; 15 | 16 | typedef double real; 17 | typedef Eigen::Vector3d point_t; 18 | typedef Eigen::Matrix point6_t; 19 | typedef Eigen::Matrix ret_point_t; 20 | typedef Eigen::Matrix ret_point6_t; 21 | typedef Eigen::VectorXd time_waypoints_t; 22 | typedef Eigen::VectorXd time_vector_t; 23 | typedef Eigen::Matrix point_list_t; 24 | typedef Eigen::Matrix point_list6_t; 25 | typedef std::vector > t_point_t; 26 | typedef std::vector > t_point6_t; 27 | typedef std::pair Waypoint; 28 | typedef std::vector T_Waypoint; 29 | typedef std::pair Waypoint6; 30 | typedef std::vector T_Waypoint6; 31 | typedef std::vector > 32 | t3d_poly_coeffs_vector_t; 33 | typedef typename t3d_poly_coeffs_vector_t::iterator it3d_poly_coeffs_vector_t; 34 | typedef typename t3d_poly_coeffs_vector_t::const_iterator 35 | cit3d_poly_coeffs_vector_t; 36 | 37 | // typedef spline::bezier_curve bezier_t; 38 | // typedef spline::bezier_curve bezier6_t; 39 | typedef parametriccurves::Polynomial polynom_t; 40 | typedef typename std::vector > 41 | t_spline_t; 42 | typedef parametriccurves::Spline spline_t; 43 | typedef parametriccurves::spatial::ForceCurve force_t; 44 | typedef polynom_t::coeff_t coeff_t; 45 | typedef std::pair waypoint_t; 46 | typedef std::vector > 47 | t_waypoint_t; 48 | 49 | // typedef spline::spline_deriv_constraint spline_deriv_constraint_t; 51 | typedef parametriccurves::curve_constraints curve_constraints_t; 52 | typedef parametriccurves::curve_constraints curve_constraints6_t; 53 | /*** TEMPLATE SPECIALIZATION FOR PYTHON ****/ 54 | 55 | // EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_t) 56 | // EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier6_t) 57 | EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(polynom_t) 58 | EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(spline_t) 59 | 60 | EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curve_constraints_t) 61 | // EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(spline_deriv_constraint_t) 62 | 63 | namespace parametriccurves { 64 | using namespace boost::python; 65 | template 66 | T_Point vectorFromEigenArray(const PointList& array) { 67 | T_Point res; 68 | for (int i = 0; i < array.cols(); ++i) res.push_back(array.col(i)); 69 | return res; 70 | } 71 | /* 72 | template 73 | Bezier* wrapBezierConstructorTemplate(const PointList& array, const real lb = 74 | 0., const real ub =1.) 75 | { 76 | T_Point asVector = vectorFromEigenArray(array); 77 | return new Bezier(asVector.begin(), asVector.end(), lb, ub); 78 | } 79 | 80 | template Bezier* wrapBezierConstructorConstraintsTemplate(const 82 | PointList& array, const CurveConstraints& constraints, const real lb = 0., const 83 | real ub =1.) 84 | { 85 | T_Point asVector = vectorFromEigenArray(array); 86 | return new Bezier(asVector.begin(), asVector.end(), constraints, lb, ub); 87 | } 88 | */ 89 | /*3D constructors */ 90 | /* 91 | bezier_t* wrapBezierConstructor(const point_list_t& array) 92 | { 93 | return wrapBezierConstructorTemplate(array) ; 95 | } 96 | bezier_t* wrapBezierConstructorBounds(const point_list_t& array, const real lb, 97 | const real ub) 98 | { 99 | return wrapBezierConstructorTemplate(array, lb, ub) ; 101 | } 102 | bezier_t* wrapBezierConstructorConstraints(const point_list_t& array, const 103 | curve_constraints_t& constraints) 104 | { 105 | return wrapBezierConstructorConstraintsTemplate(array, constraints) ; 107 | } 108 | bezier_t* wrapBezierConstructorBoundsConstraints(const point_list_t& array, 109 | const curve_constraints_t& constraints, const real lb, const real ub) 110 | { 111 | return wrapBezierConstructorConstraintsTemplate(array, constraints, lb, ub) ; 113 | } 114 | */ 115 | /*END 3D constructors */ 116 | /*6D constructors */ 117 | /* 118 | bezier6_t* wrapBezierConstructor6(const point_list6_t& array) 119 | { 120 | return wrapBezierConstructorTemplate(array) ; 122 | } 123 | bezier6_t* wrapBezierConstructorBounds6(const point_list6_t& array, const real 124 | lb, const real ub) 125 | { 126 | return wrapBezierConstructorTemplate(array, lb, ub) ; 128 | } 129 | bezier6_t* wrapBezierConstructor6Constraints(const point_list6_t& array, const 130 | curve_constraints6_t& constraints) 131 | { 132 | return wrapBezierConstructorConstraintsTemplate(array, constraints) ; 134 | } 135 | bezier6_t* wrapBezierConstructorBounds6Constraints(const point_list6_t& array, 136 | const curve_constraints6_t& constraints, const real lb, const real ub) 137 | { 138 | return wrapBezierConstructorConstraintsTemplate(array, constraints, lb, ub) ; 140 | } 141 | */ 142 | /*END 6D constructors */ 143 | 144 | polynom_t* wrapSplineConstructor(const coeff_t& array) { 145 | return new polynom_t(array, 0., 1.); 146 | } 147 | 148 | t_waypoint_t getWayPoints(const coeff_t& array, 149 | const time_waypoints_t& time_wp) { 150 | t_waypoint_t res; 151 | for (int i = 0; i < array.cols(); ++i) 152 | res.push_back(std::make_pair(time_wp(i), array.col(i))); 153 | return res; 154 | } 155 | 156 | template 157 | Eigen::Matrix wayPointsToList( 158 | const BezierType& self) { 159 | typedef typename BezierType::t_point_t t_point; 160 | typedef typename BezierType::t_point_t::const_iterator cit_point; 161 | const t_point& wps = self.waypoints(); 162 | Eigen::Matrix res(dim, wps.size()); 163 | int col = 0; 164 | for (cit_point cit = wps.begin(); cit != wps.end(); ++cit, ++col) 165 | res.block(0, col) = *cit; 166 | return res; 167 | } 168 | void spline_from_waypoints(spline_t& self, const coeff_t& array, 169 | const time_waypoints_t& time_wp) { 170 | t_waypoint_t wps = getWayPoints(array, time_wp); 171 | self.createSplineFromWayPoints(wps.begin(), wps.end()); 172 | return; 173 | } 174 | 175 | spline_t* spline_by_concatenation_constructor(const bp::list& list_splines) { 176 | t_spline_t subSplines; 177 | subSplines.clear(); 178 | for (int i = 0; i < len(list_splines); ++i) { 179 | spline_t _sp = bp::extract(list_splines[i]); 180 | const t_spline_t& _vec_subspline = _sp.getSubsplines(); 181 | subSplines.insert(subSplines.end(), _vec_subspline.begin(), 182 | _vec_subspline.end()); 183 | } 184 | return new spline_t(subSplines); 185 | } 186 | 187 | spline_t* wrapExactCubicConstructorvoid() { return new spline_t(); } 188 | 189 | spline_t* wrapExactCubicConstructorPolySequence( 190 | const bp::list& list_polynomials, const time_vector_t& time_vector) { 191 | typedef std::vector > 192 | t3d_poly_coeffs_vector_t; 193 | t3d_poly_coeffs_vector_t poly_coeffs_vector; 194 | t_spline_t subSplines; 195 | subSplines.clear(); 196 | 197 | assert(time_vector.size() == len(list_polynomials) + 1); 198 | for (int i = 0; i < len(list_polynomials); ++i) { 199 | subSplines.push_back(polynom_t(bp::extract(list_polynomials[i]), 200 | time_vector[i], time_vector[i + 1])); 201 | // time_vector[i], time_vector[i+1])); 202 | } 203 | return new spline_t(subSplines); 204 | } 205 | 206 | force_t* wrapForceCurveConstructorvoid() { return new force_t(); } 207 | 208 | force_t* wrapForceCurveConstructorSplines(const spline_t& linear_part, 209 | const spline_t& ang_part) { 210 | return new force_t(linear_part, ang_part); 211 | } 212 | 213 | void spline_from_waypoints_constr(spline_t& self, const coeff_t& array, 214 | const time_waypoints_t& time_wp, 215 | const curve_constraints_t& constraints) { 216 | t_waypoint_t wps = getWayPoints(array, time_wp); 217 | self.createSplineFromWayPointsConstr(wps.begin(), wps.end(), constraints); 218 | return; 219 | } 220 | 221 | point_t get_init_vel(const curve_constraints_t& c) { return c.init_vel; } 222 | 223 | point_t get_init_acc(const curve_constraints_t& c) { return c.init_acc; } 224 | 225 | point_t get_end_vel(const curve_constraints_t& c) { return c.end_vel; } 226 | 227 | point_t get_end_acc(const curve_constraints_t& c) { return c.end_acc; } 228 | 229 | void set_init_vel(curve_constraints_t& c, const point_t& val) { 230 | c.init_vel = val; 231 | } 232 | 233 | void set_init_acc(curve_constraints_t& c, const point_t& val) { 234 | c.init_acc = val; 235 | } 236 | 237 | void set_end_vel(curve_constraints_t& c, const point_t& val) { 238 | c.end_vel = val; 239 | } 240 | 241 | void set_end_acc(curve_constraints_t& c, const point_t& val) { 242 | c.end_acc = val; 243 | } 244 | 245 | BOOST_PYTHON_MODULE(libparametric_curves_pywrap) { 246 | /** BEGIN eigenpy init**/ 247 | bp::import("eigenpy"); 248 | 249 | eigenpy::enableEigenPySpecific(); 250 | eigenpy::enableEigenPySpecific(); 251 | eigenpy::enableEigenPySpecific(); 252 | eigenpy::enableEigenPySpecific(); 253 | eigenpy::enableEigenPySpecific(); 254 | eigenpy::enableEigenPySpecific(); 255 | eigenpy::enableEigenPySpecific(); 256 | /*eigenpy::exposeAngleAxis(); 257 | eigenpy::exposeQuaternion();*/ 258 | /** END eigenpy init**/ 259 | 260 | /** BEGIN bezier curve 6**/ 261 | /* 262 | class_ 263 | ("bezier6", no_init) 264 | .def("__init__", make_constructor(&wrapBezierConstructor6)) 265 | .def("__init__", make_constructor(&wrapBezierConstructorBounds6)) 266 | //.def("__init__", 267 | make_constructor(&wrapBezierConstructor6Constraints)) 268 | //.def("__init__", 269 | make_constructor(&wrapBezierConstructorBounds6Constraints)) .def("min", 270 | &bezier6_t::min) .def("max", &bezier6_t::max) .def("__call__", 271 | &bezier6_t::operator()) .def("derivate", &bezier6_t::derivate) 272 | .def("compute_derivate", &bezier6_t::compute_derivate) 273 | .def("compute_primitive", &bezier6_t::compute_primitive) 274 | .def("waypoints", &wayPointsToList) 275 | .def_readonly("degree", &bezier6_t::degree_) 276 | .def_readonly("nbWaypoints", &bezier6_t::size_) 277 | ; 278 | */ 279 | /** END bezier curve**/ 280 | 281 | /** BEGIN bezier curve**/ 282 | /* 283 | class_ 284 | ("bezier", no_init) 285 | .def("__init__", make_constructor(&wrapBezierConstructor)) 286 | .def("__init__", make_constructor(&wrapBezierConstructorBounds)) 287 | .def("__init__", make_constructor(&wrapBezierConstructorConstraints)) 288 | .def("__init__", 289 | make_constructor(&wrapBezierConstructorBoundsConstraints)) .def("min", 290 | &bezier_t::min) .def("max", &bezier_t::max) .def("__call__", 291 | &bezier_t::operator()) .def("derivate", &bezier_t::derivate) 292 | .def("compute_derivate", &bezier_t::compute_derivate) 293 | .def("compute_primitive", &bezier_t::compute_primitive) 294 | .def("waypoints", &wayPointsToList) 295 | .def_readonly("degree", &bezier_t::degree_) 296 | .def_readonly("nbWaypoints", &bezier_t::size_) 297 | ; 298 | */ 299 | /** END bezier curve**/ 300 | 301 | /** BEGIN spline curve function**/ 302 | class_("polynomial", 303 | init()) 304 | .def("__init__", make_constructor(&wrapSplineConstructor)) 305 | .def("min", &polynom_t::tmin) 306 | .def("max", &polynom_t::tmax) 307 | .def("__call__", &polynom_t::operator()) 308 | .def("derivate", &polynom_t::derivate); 309 | /** END cubic function**/ 310 | 311 | /** BEGIN spline curve**/ 312 | class_("spline", no_init) 313 | .def("__init__", make_constructor(&wrapExactCubicConstructorvoid)) 314 | .def("__init__", make_constructor(&wrapExactCubicConstructorPolySequence)) 315 | .def("__init__", make_constructor(&spline_by_concatenation_constructor)) 316 | .def("min", &spline_t::tmin, 317 | bp::return_value_policy()) 318 | .def("max", &spline_t::tmax, 319 | bp::return_value_policy()) 320 | .def("__call__", &spline_t::operator(), 321 | bp::return_value_policy()) 322 | .def("derivate", &spline_t::derivate, 323 | bp::return_value_policy()) 324 | .def("create_spline_from_waypoints", &spline_from_waypoints, 325 | boost::python::args("waypoints", "time vector"), 326 | "Creates a cubic spline from a set of way points") 327 | .def("create_spline_from_waypoints_constr", &spline_from_waypoints_constr, 328 | boost::python::args("waypoints", "time vector", "constraints"), 329 | "Creates a spline from a set of way points and constraints") 330 | .def("load_from_file", &spline_t::loadFromFile, 331 | boost::python::args("filename"), "Loads *this") 332 | .def("save_to_file", &spline_t::saveToFile, 333 | boost::python::args("filename"), "Saves *this"); 334 | /** BEGIN force curve**/ 335 | 336 | class_("forcecurve", no_init) 337 | .def("__init__", make_constructor(&wrapForceCurveConstructorvoid)) 338 | .def("__init__", make_constructor(&wrapForceCurveConstructorSplines)) 339 | .def("min", &force_t::tmin, 340 | bp::return_value_policy()) 341 | .def("max", &force_t::tmax, 342 | bp::return_value_policy()) 343 | .def("__call__", &force_t::operator(), 344 | bp::return_value_policy()) 345 | .def("derivate", &force_t::derivate, 346 | bp::return_value_policy()) 347 | .def("set_motion_vector", &force_t::setMotionVector, 348 | boost::python::args("motionvector"), 349 | "sets motion vector for derivate") 350 | .def("load_from_file", &force_t::loadFromFile, 351 | boost::python::args("filename"), "Loads *this") 352 | .def("save_to_file", &force_t::saveToFile, 353 | boost::python::args("filename"), "Saves *this"); 354 | 355 | /** END force curve**/ 356 | 357 | /** END bezier curve**/ 358 | 359 | /** BEGIN curve constraints**/ 360 | class_("curve_constraints", init<>()) 361 | .add_property("init_vel", &get_init_vel, &set_init_vel) 362 | .add_property("init_acc", &get_init_acc, &set_init_acc) 363 | .add_property("end_vel", &get_end_vel, &set_end_vel) 364 | .add_property("end_acc", &get_end_acc, &set_end_acc); 365 | /** END curve constraints**/ 366 | } 367 | } // namespace parametriccurves 368 | -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | [flake8] 2 | exclude = cmake 3 | max-line-length = 88 4 | ignore = E226, E704, E24, E121, W504, E126, E123, W503, E203 5 | -------------------------------------------------------------------------------- /tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_unit_test(spline_tests Main.cpp) 2 | target_link_libraries(spline_tests ${PROJECT_NAME}) 3 | 4 | if(BUILD_PYTHON_INTERFACE) 5 | add_subdirectory(python) 6 | endif(BUILD_PYTHON_INTERFACE) 7 | -------------------------------------------------------------------------------- /tests/Main.cpp: -------------------------------------------------------------------------------- 1 | #include "parametric-curves/polynomial.hpp" 2 | #include "parametric-curves/spline.hpp" 3 | #ifdef EXTENDED 4 | #include "parametric-curves/bezier_curve.h" 5 | #include "parametric-curves/helpers/effector_spline.h" 6 | #include "parametric-curves/helpers/effector_spline_rotation.h" 7 | #include "parametric-curves/spline_deriv_constraint.h" 8 | #endif 9 | #include 10 | #include 11 | #include 12 | 13 | using namespace std; 14 | 15 | namespace parametriccurves { 16 | typedef Eigen::Vector3d point_t; 17 | typedef std::vector > t_point_t; 18 | typedef Polynomial polynom_t; 19 | typedef Spline Spline_t; 20 | #ifdef TEST_EXTENDED 21 | typedef spline_deriv_constraint 22 | spline_deriv_constraint_t; 23 | typedef bezier_curve bezier_curve_t; 24 | #endif 25 | typedef Spline_t::spline_constraints spline_constraints_t; 26 | typedef std::pair Waypoint; 27 | typedef std::vector T_Waypoint; 28 | 29 | typedef Eigen::Matrix point_one; 30 | typedef Polynomial polynom_one; 31 | typedef Spline Spline_one; 32 | typedef std::pair WaypointOne; 33 | typedef std::vector T_WaypointOne; 34 | 35 | bool QuasiEqual(const double a, const double b, const float margin) { 36 | if ((a <= 0 && b <= 0) || (a >= 0 && b >= 0)) { 37 | return (abs(a - b)) <= margin; 38 | } else { 39 | return abs(a) + abs(b) <= margin; 40 | } 41 | } 42 | 43 | const double margin = 0.001; 44 | 45 | } // namespace parametriccurves 46 | using namespace parametriccurves; 47 | ostream& operator<<(ostream& os, const point_t& pt) { 48 | os << "(" << pt.x() << ", " << pt.y() << ", " << pt.z() << ")"; 49 | return os; 50 | } 51 | 52 | void ComparePoints(const Eigen::VectorXd& pt1, const Eigen::VectorXd& pt2, 53 | const std::string& errmsg, bool& error, 54 | bool notequal = false) { 55 | if ((pt1 - pt2).norm() > margin && !notequal) { 56 | error = true; 57 | std::cout << errmsg << pt1.transpose() << " ; " << pt2.transpose() 58 | << std::endl; 59 | } 60 | } 61 | 62 | /*Cubic Function tests*/ 63 | 64 | void CubicFunctionTest(bool& error) { 65 | Spline test; 66 | std::string errMsg("In test CubicFunctionTest ; unexpected result for x "); 67 | point_t a(1, 2, 3); 68 | point_t b(2, 3, 4); 69 | point_t c(3, 4, 5); 70 | point_t d(3, 6, 7); 71 | t_point_t vec; 72 | vec.push_back(a); 73 | vec.push_back(b); 74 | vec.push_back(c); 75 | vec.push_back(d); 76 | polynom_t cf(vec.begin(), vec.end(), 0, 1); 77 | point_t res1; 78 | res1 = cf(0); 79 | point_t x0(1, 2, 3); 80 | ComparePoints(x0, res1, errMsg + "(0) ", error); 81 | 82 | point_t x1(9, 15, 19); 83 | res1 = cf(1); 84 | ComparePoints(x1, res1, errMsg + "(1) ", error); 85 | 86 | point_t x2(3.125, 5.25, 7.125); 87 | res1 = cf(0.5); 88 | ComparePoints(x2, res1, errMsg + "(0.5) ", error); 89 | 90 | vec.clear(); 91 | vec.push_back(a); 92 | vec.push_back(b); 93 | vec.push_back(c); 94 | vec.push_back(d); 95 | polynom_t cf2(vec, 0.5, 1); 96 | res1 = cf2(0.5); 97 | point_t x4(3.125, 5.25, 7.125); 98 | ComparePoints(x4, res1, errMsg + "x3 ", error); 99 | error = true; 100 | try { 101 | cf2(0.4); 102 | } catch (...) { 103 | error = false; 104 | } 105 | if (error) { 106 | std::cout << "Evaluation of cubic cf2 error, 0.4 should be an out of range " 107 | "value\n"; 108 | } 109 | error = true; 110 | try { 111 | cf2(1.1); 112 | } catch (...) { 113 | error = false; 114 | } 115 | if (error) { 116 | std::cout << "Evaluation of cubic cf2 error, 1.1 should be an out of range " 117 | "value\n"; 118 | } 119 | if (cf.tmax() != 1) { 120 | error = true; 121 | std::cout 122 | << "Evaluation of exactCubic error, MaxBound should be equal to 1\n"; 123 | } 124 | if (cf.tmin() != 0) { 125 | error = true; 126 | std::cout 127 | << "Evaluation of exactCubic error, MinBound should be equal to 1\n"; 128 | } 129 | } 130 | 131 | #ifdef TEST_EXTENDED 132 | /*bezier_curve Function tests*/ 133 | void BezierCurveTest(bool& error) { 134 | std::string errMsg("In test BezierCurveTest ; unexpected result for x "); 135 | point_t a(1, 2, 3); 136 | point_t b(2, 3, 4); 137 | point_t c(3, 4, 5); 138 | point_t d(3, 6, 7); 139 | 140 | std::vector params; 141 | params.push_back(a); 142 | params.push_back(b); 143 | 144 | // 2d curve 145 | bezier_curve_t cf(params.begin(), params.end()); 146 | point_t res1; 147 | res1 = cf(0); 148 | point_t x20 = a; 149 | ComparePoints(x20, res1, errMsg + "2(0) ", error); 150 | 151 | point_t x21 = b; 152 | res1 = cf(1); 153 | ComparePoints(x21, res1, errMsg + "2(1) ", error); 154 | 155 | // 3d curve 156 | params.push_back(c); 157 | bezier_curve_t cf3(params.begin(), params.end()); 158 | res1 = cf3(0); 159 | ComparePoints(a, res1, errMsg + "3(0) ", error); 160 | 161 | res1 = cf3(1); 162 | ComparePoints(c, res1, errMsg + "3(1) ", error); 163 | 164 | // 4d curve 165 | params.push_back(d); 166 | bezier_curve_t cf4(params.begin(), params.end(), 0.4, 2); 167 | res1 = cf4(0.4); 168 | ComparePoints(a, res1, errMsg + "3(0) ", error); 169 | 170 | res1 = cf4(2); 171 | ComparePoints(d, res1, errMsg + "3(1) ", error); 172 | 173 | // testing bernstein polynomes 174 | std::string errMsg2( 175 | "In test BezierCurveTest ; Bernstein polynoms do not evaluate as " 176 | "analytical evaluation"); 177 | for (double d = 0.; d < 1.; d += 0.1) { 178 | ComparePoints(cf3.evalBernstein(d), cf3(d), errMsg2, error); 179 | ComparePoints(cf3.evalHorner(d), cf3(d), errMsg2, error); 180 | } 181 | 182 | bool error_in(true); 183 | try { 184 | cf(-0.4); 185 | } catch (...) { 186 | error_in = false; 187 | } 188 | if (error_in) { 189 | std::cout << "Evaluation of bezier cf error, -0.4 should be an out of " 190 | "range value\n"; 191 | error = true; 192 | } 193 | error_in = true; 194 | try { 195 | cf(1.1); 196 | } catch (...) { 197 | error_in = false; 198 | } 199 | if (error_in) { 200 | std::cout << "Evaluation of bezier cf error, 1.1 should be an out of range " 201 | "value\n"; 202 | error = true; 203 | } 204 | if (cf.max() != 1) { 205 | error = true; 206 | std::cout 207 | << "Evaluation of exactCubic error, MaxBound should be equal to 1\n"; 208 | } 209 | if (cf.min() != 0) { 210 | error = true; 211 | std::cout 212 | << "Evaluation of exactCubic error, MinBound should be equal to 1\n"; 213 | } 214 | } 215 | 216 | #include 217 | void BezierCurveTestCompareHornerAndBernstein(bool& error) { 218 | using namespace std; 219 | std::vector values; 220 | for (int i = 0; i < 100000; ++i) values.push_back(rand() / RAND_MAX); 221 | 222 | // first compare regular evaluation (low dim pol) 223 | point_t a(1, 2, 3); 224 | point_t b(2, 3, 4); 225 | point_t c(3, 4, 5); 226 | point_t d(3, 6, 7); 227 | point_t e(3, 61, 7); 228 | point_t f(3, 56, 7); 229 | point_t g(3, 36, 7); 230 | point_t h(43, 6, 7); 231 | point_t i(3, 6, 77); 232 | 233 | std::vector params; 234 | params.push_back(a); 235 | params.push_back(b); 236 | params.push_back(c); 237 | 238 | // 3d curve 239 | bezier_curve_t cf(params.begin(), params.end()); 240 | 241 | clock_t s0, e0, s1, e1, s2, e2; 242 | s0 = clock(); 243 | for (std::vector::const_iterator cit = values.begin(); 244 | cit != values.end(); ++cit) { 245 | cf(*cit); 246 | } 247 | e0 = clock(); 248 | 249 | s1 = clock(); 250 | for (std::vector::const_iterator cit = values.begin(); 251 | cit != values.end(); ++cit) { 252 | cf.evalBernstein(*cit); 253 | } 254 | e1 = clock(); 255 | 256 | s2 = clock(); 257 | for (std::vector::const_iterator cit = values.begin(); 258 | cit != values.end(); ++cit) { 259 | cf.evalHorner(*cit); 260 | } 261 | e2 = clock(); 262 | 263 | std::cout << "time for analytical eval " << double(e0 - s0) / CLOCKS_PER_SEC 264 | << std::endl; 265 | std::cout << "time for bernstein eval " << double(e1 - s1) / CLOCKS_PER_SEC 266 | << std::endl; 267 | std::cout << "time for horner eval " << double(e2 - s2) / CLOCKS_PER_SEC 268 | << std::endl; 269 | 270 | std::cout << "now with high order polynom " << std::endl; 271 | 272 | params.push_back(d); 273 | params.push_back(e); 274 | params.push_back(f); 275 | params.push_back(g); 276 | params.push_back(h); 277 | params.push_back(i); 278 | 279 | bezier_curve_t cf2(params.begin(), params.end()); 280 | 281 | s1 = clock(); 282 | for (std::vector::const_iterator cit = values.begin(); 283 | cit != values.end(); ++cit) { 284 | cf2.evalBernstein(*cit); 285 | } 286 | e1 = clock(); 287 | 288 | s2 = clock(); 289 | for (std::vector::const_iterator cit = values.begin(); 290 | cit != values.end(); ++cit) { 291 | cf2.evalHorner(*cit); 292 | } 293 | e2 = clock(); 294 | 295 | s0 = clock(); 296 | for (std::vector::const_iterator cit = values.begin(); 297 | cit != values.end(); ++cit) { 298 | cf2(*cit); 299 | } 300 | e0 = clock(); 301 | 302 | std::cout << "time for analytical eval " << double(e0 - s0) / CLOCKS_PER_SEC 303 | << std::endl; 304 | std::cout << "time for bernstein eval " << double(e1 - s1) / CLOCKS_PER_SEC 305 | << std::endl; 306 | std::cout << "time for horner eval " << double(e2 - s2) / CLOCKS_PER_SEC 307 | << std::endl; 308 | } 309 | 310 | void BezierDerivativeCurveTest(bool& error) { 311 | std::string errMsg( 312 | "In test BezierDerivativeCurveTest ; unexpected result for x "); 313 | point_t a(1, 2, 3); 314 | point_t b(2, 3, 4); 315 | point_t c(3, 4, 5); 316 | 317 | std::vector params; 318 | params.push_back(a); 319 | params.push_back(b); 320 | 321 | params.push_back(c); 322 | bezier_curve_t cf3(params.begin(), params.end()); 323 | 324 | ComparePoints(cf3(0), cf3.derivate(0., 0), errMsg, error); 325 | ComparePoints(cf3(0), cf3.derivate(0., 1), errMsg, error, true); 326 | ComparePoints(point_t::Zero(), cf3.derivate(0., 100), errMsg, error); 327 | } 328 | 329 | void BezierDerivativeCurveConstraintTest(bool& error) { 330 | std::string errMsg( 331 | "In test BezierDerivativeCurveConstraintTest ; unexpected result for x "); 332 | point_t a(1, 2, 3); 333 | point_t b(2, 3, 4); 334 | point_t c(3, 4, 5); 335 | 336 | bezier_curve_t::curve_constraints_t constraints; 337 | constraints.init_vel = point_t(-1, -1, -1); 338 | constraints.init_acc = point_t(-2, -2, -2); 339 | constraints.end_vel = point_t(-10, -10, -10); 340 | constraints.end_acc = point_t(-20, -20, -20); 341 | 342 | std::vector params; 343 | params.push_back(a); 344 | params.push_back(b); 345 | 346 | params.push_back(c); 347 | bezier_curve_t cf3(params.begin(), params.end(), constraints); 348 | 349 | assert(cf3.degree_ == params.size() + 3); 350 | assert(cf3.size_ == params.size() + 4); 351 | 352 | ComparePoints(a, cf3(0), errMsg, error); 353 | ComparePoints(c, cf3(1), errMsg, error); 354 | ComparePoints(constraints.init_vel, cf3.derivate(0., 1), errMsg, error); 355 | ComparePoints(constraints.end_vel, cf3.derivate(1., 1), errMsg, error); 356 | ComparePoints(constraints.init_acc, cf3.derivate(0., 2), errMsg, error); 357 | ComparePoints(constraints.end_vel, cf3.derivate(1., 1), errMsg, error); 358 | ComparePoints(constraints.end_acc, cf3.derivate(1., 2), errMsg, error); 359 | } 360 | 361 | #endif 362 | /*Exact Cubic Function tests*/ 363 | void ExactCubicNoErrorTest(bool& error) { 364 | T_Waypoint waypoints; 365 | for (double i = 0; i <= 1; i = i + 0.2) { 366 | waypoints.push_back(std::make_pair(i, point_t(i, i, i))); 367 | } 368 | Spline_t exactCubic; 369 | exactCubic.createSplineFromWayPoints(waypoints.begin(), waypoints.end()); 370 | point_t res1; 371 | try { 372 | exactCubic(0); 373 | exactCubic(1); 374 | } catch (...) { 375 | error = true; 376 | std::cout << "Evaluation of ExactCubicNoErrorTest error\n"; 377 | } 378 | error = true; 379 | try { 380 | exactCubic(1.2); 381 | } catch (...) { 382 | error = false; 383 | } 384 | if (error) { 385 | std::cout << "Evaluation of exactCubic cf error, 1.2 should be an out of " 386 | "range value\n"; 387 | } 388 | if (exactCubic.tmax() != 1) { 389 | error = true; 390 | std::cout 391 | << "Evaluation of exactCubic error, MaxBound should be equal to 1\n"; 392 | } 393 | if (exactCubic.tmin() != 0) { 394 | error = true; 395 | std::cout 396 | << "Evaluation of exactCubic error, MinBound should be equal to 1\n"; 397 | } 398 | } 399 | 400 | /*Exact Cubic Function tests*/ 401 | void ExactCubicTwoPointsTest(bool& error) { 402 | T_Waypoint waypoints; 403 | for (double i = 0; i < 2; ++i) { 404 | waypoints.push_back(std::make_pair(i, point_t(i, i, i))); 405 | } 406 | Spline_t exactCubic; 407 | exactCubic.createSplineFromWayPoints(waypoints.begin(), waypoints.end()); 408 | 409 | point_t res1 = exactCubic(0); 410 | std::string errmsg( 411 | "in ExactCubic 2 points Error While checking that given wayPoints are " 412 | "crossed (expected / obtained)"); 413 | ComparePoints(point_t(0, 0, 0), res1, errmsg, error); 414 | 415 | res1 = exactCubic(1); 416 | ComparePoints(point_t(1, 1, 1), res1, errmsg, error); 417 | } 418 | 419 | void ExactCubicOneDimTest(bool& error) { 420 | T_WaypointOne waypoints; 421 | point_one zero; 422 | zero(0, 0) = 9; 423 | point_one one; 424 | one(0, 0) = 14; 425 | point_one two; 426 | two(0, 0) = 25; 427 | waypoints.push_back(std::make_pair(0., zero)); 428 | waypoints.push_back(std::make_pair(1., one)); 429 | waypoints.push_back(std::make_pair(2., two)); 430 | Spline_one exactCubic; 431 | exactCubic.createSplineFromWayPoints(waypoints.begin(), waypoints.end()); 432 | 433 | point_one res1 = exactCubic(0); 434 | std::string errmsg( 435 | "in ExactCubicOneDim Error While checking that given wayPoints are " 436 | "crossed (expected / obtained)"); 437 | ComparePoints(zero, res1, errmsg, error); 438 | 439 | res1 = exactCubic(1); 440 | ComparePoints(one, res1, errmsg, error); 441 | } 442 | 443 | void CheckWayPointConstraint(const std::string& errmsg, const double step, 444 | const T_Waypoint&, const Spline_t* curve, 445 | bool& error) { 446 | point_t res1; 447 | for (double i = 0; i <= 1; i = i + step) { 448 | res1 = (*curve)(i); 449 | ComparePoints(point_t(i, i, i), res1, errmsg, error); 450 | } 451 | } 452 | 453 | void CheckDerivative(const std::string& errmsg, const double eval_point, 454 | const std::size_t order, const point_t& target, 455 | const Spline_t* curve, bool& error) { 456 | point_t res1 = curve->derivate(eval_point, order); 457 | ComparePoints(target, res1, errmsg, error); 458 | } 459 | 460 | void ExactCubicPointsCrossedTest(bool& error) { 461 | T_Waypoint waypoints; 462 | for (double i = 0; i <= 1; i = i + 0.2) { 463 | waypoints.push_back(std::make_pair(i, point_t(i, i, i))); 464 | } 465 | Spline_t exactCubic; 466 | exactCubic.createSplineFromWayPoints(waypoints.begin(), waypoints.end()); 467 | std::string errmsg( 468 | "Error While checking that given wayPoints are crossed (expected / " 469 | "obtained)"); 470 | CheckWayPointConstraint(errmsg, 0.2, waypoints, &exactCubic, error); 471 | } 472 | void ExactCubicVelocityConstraintsTest(bool& error) { 473 | T_Waypoint waypoints; 474 | for (double i = 0; i <= 1; i = i + 0.2) { 475 | waypoints.push_back(std::make_pair(i, point_t(i, i, i))); 476 | } 477 | std::string errmsg( 478 | "Error in ExactCubicVelocityConstraintsTest (1); while checking that " 479 | "given wayPoints are crossed (expected / " 480 | "obtained)"); 481 | spline_constraints_t constraints; 482 | constraints.end_vel = point_t(0, 0, 0); 483 | constraints.init_vel = point_t(0, 0, 0); 484 | constraints.end_acc = point_t(0, 0, 0); 485 | constraints.init_acc = point_t(0, 0, 0); 486 | 487 | Spline_t exactCubic; 488 | 489 | exactCubic.createSplineFromWayPointsConstr(waypoints.begin(), waypoints.end(), 490 | constraints); 491 | // now check that init and end velocity are 0 492 | CheckWayPointConstraint(errmsg, 0.2, waypoints, &exactCubic, error); 493 | std::string errmsg3( 494 | "Error in ExactCubicVelocityConstraintsTest (2); while checking " 495 | "derivative (expected / obtained)"); 496 | // now check derivatives 497 | CheckDerivative(errmsg3, 0, 1, constraints.init_vel, &exactCubic, error); 498 | CheckDerivative(errmsg3, 1, 1, constraints.end_vel, &exactCubic, error); 499 | CheckDerivative(errmsg3, 0, 2, constraints.init_acc, &exactCubic, error); 500 | CheckDerivative(errmsg3, 1, 2, constraints.end_acc, &exactCubic, error); 501 | 502 | constraints.end_vel = point_t(1, 2, 3); 503 | constraints.init_vel = point_t(-1, -2, -3); 504 | constraints.end_acc = point_t(4, 5, 6); 505 | constraints.init_acc = point_t(-4, -4, -6); 506 | std::string errmsg2( 507 | "Error in ExactCubicVelocityConstraintsTest (3); while checking that " 508 | "given wayPoints are crossed (expected / " 509 | "obtained)"); 510 | Spline_t exactCubic2; 511 | exactCubic2.createSplineFromWayPointsConstr(waypoints.begin(), 512 | waypoints.end(), constraints); 513 | CheckWayPointConstraint(errmsg2, 0.2, waypoints, &exactCubic2, error); 514 | 515 | std::string errmsg4( 516 | "Error in ExactCubicVelocityConstraintsTest (4); while checking " 517 | "derivative (expected / obtained)"); 518 | // now check derivatives 519 | CheckDerivative(errmsg4, 0, 1, constraints.init_vel, &exactCubic2, error); 520 | CheckDerivative(errmsg4, 1, 1, constraints.end_vel, &exactCubic2, error); 521 | CheckDerivative(errmsg4, 0, 2, constraints.init_acc, &exactCubic2, error); 522 | CheckDerivative(errmsg4, 1, 2, constraints.end_acc, &exactCubic2, error); 523 | } 524 | void CheckPointOnline(const std::string& errmsg, const point_t& A, 525 | const point_t& B, const double target, 526 | const Spline_t* curve, bool& error) { 527 | point_t res1 = curve->operator()(target); 528 | point_t ar = (res1 - A); 529 | ar.normalize(); 530 | point_t rb = (B - res1); 531 | rb.normalize(); 532 | if (ar.dot(rb) < 0.99999) { 533 | error = true; 534 | std::cout << errmsg << " ; " << A.transpose() << "\n ; " << B.transpose() 535 | << "\n ; " << target << " ; " << res1.transpose() << std::endl; 536 | } 537 | } 538 | /* 539 | void EffectorTrajectoryTest(bool& error) 540 | { 541 | // create arbitrary trajectory 542 | T_Waypoint waypoints; 543 | for(double i = 0; i <= 10; i = i + 2) 544 | { 545 | waypoints.push_back(std::make_pair(i,point_t(i,i,i))); 546 | } 547 | helpers::Spline_t* eff_traj = 548 | helpers::effector_spline(waypoints.begin(),waypoints.end(), 549 | Eigen::Vector3d::UnitZ(),Eigen::Vector3d(0,0,2), 550 | 1,0.02,1,0.5); 551 | point_t zero(0,0,0); 552 | point_t off1(0,0,1); 553 | point_t off2(10,10,10.02); 554 | point_t end(10,10,10); 555 | std::string errmsg("Error in EffectorTrajectoryTest; while checking waypoints 556 | (expected / obtained)"); std::string errmsg2("Error in EffectorTrajectoryTest; 557 | while checking derivative (expected / obtained)"); 558 | //first check start / goal positions 559 | ComparePoints(zero, (*eff_traj)(0), errmsg, error); 560 | ComparePoints(off1, (*eff_traj)(1), errmsg, error); 561 | ComparePoints(off2, (*eff_traj)(9.5), errmsg, error); 562 | ComparePoints(end , (*eff_traj)(10), errmsg, error); 563 | 564 | //then check offset at start / goal positions 565 | // now check derivatives 566 | CheckDerivative(errmsg2,0,1,zero,eff_traj, error); 567 | CheckDerivative(errmsg2,10,1,zero ,eff_traj, error); 568 | CheckDerivative(errmsg2,0,2,zero,eff_traj, error); 569 | CheckDerivative(errmsg2,10,2,zero ,eff_traj, error); 570 | 571 | //check that end and init splines are line 572 | std::string errmsg3("Error in EffectorTrajectoryTest; while checking that 573 | init/end splines are line (point A/ point B, time value / point obtained) 574 | \n"); for(double i = 0.1; i<1; i+=0.1) 575 | { 576 | CheckPointOnline(errmsg3,(*eff_traj)(0),(*eff_traj)(1),i,eff_traj,error); 577 | } 578 | 579 | for(double i = 9.981; i<10; i+=0.002) 580 | { 581 | CheckPointOnline(errmsg3,(*eff_traj)(9.5),(*eff_traj)(10),i,eff_traj,error); 582 | } 583 | delete eff_traj; 584 | } 585 | 586 | helpers::quat_t GetXRotQuat(const double theta) 587 | { 588 | Eigen::AngleAxisd m (theta, Eigen::Vector3d::UnitX()); 589 | return helpers::quat_t(Eigen::Quaterniond(m).coeffs().data()); 590 | } 591 | 592 | double GetXRotFromQuat(helpers::quat_ref_const_t q) 593 | { 594 | Eigen::Quaterniond quat (q.data()); 595 | Eigen::AngleAxisd m (quat); 596 | return m.angle() / M_PI * 180.; 597 | } 598 | 599 | void EffectorSplineRotationNoRotationTest(bool& error) 600 | { 601 | // create arbitrary trajectory 602 | T_Waypoint waypoints; 603 | for(double i = 0; i <= 10; i = i + 2) 604 | { 605 | waypoints.push_back(std::make_pair(i,point_t(i,i,i))); 606 | } 607 | helpers::effector_spline_rotation eff_traj(waypoints.begin(),waypoints.end()); 608 | helpers::config_t q_init; q_init << 0.,0.,0.,0.,0.,0.,1.; 609 | helpers::config_t q_end; q_end << 10.,10.,10.,0.,0.,0.,1.; 610 | helpers::config_t q_to; q_to << 0.,0,0.02,0.,0.,0.,1.; 611 | helpers::config_t q_land; q_land << 10,10, 10.02, 0, 0.,0.,1.; 612 | helpers::config_t q_mod; q_mod << 6.,6.,6.,0.,0.,0.,1.; 613 | std::string errmsg("Error in EffectorSplineRotationNoRotationTest; while 614 | checking waypoints (expected / obtained)"); ComparePoints(q_init , 615 | eff_traj(0), errmsg,error); ComparePoints(q_to , eff_traj(0.02), 616 | errmsg,error); ComparePoints(q_land , eff_traj(9.98), errmsg,error); 617 | ComparePoints(q_mod , eff_traj(6), errmsg,error); 618 | ComparePoints(q_end , eff_traj(10), errmsg,error); 619 | } 620 | 621 | void EffectorSplineRotationRotationTest(bool& error) 622 | { 623 | // create arbitrary trajectory 624 | T_Waypoint waypoints; 625 | for(double i = 0; i <= 10; i = i + 2) 626 | { 627 | waypoints.push_back(std::make_pair(i,point_t(i,i,i))); 628 | } 629 | helpers::quat_t init_quat = GetXRotQuat(M_PI); 630 | helpers::effector_spline_rotation eff_traj(waypoints.begin(),waypoints.end(), 631 | init_quat); helpers::config_t q_init = helpers::config_t::Zero(); 632 | q_init.tail<4>() = init_quat; helpers::config_t q_end; q_end 633 | << 10.,10.,10.,0.,0.,0.,1.; helpers::config_t q_to = q_init; q_to(2) +=0.02; 634 | helpers::config_t q_land = q_end ; q_land(2)+=0.02; 635 | helpers::quat_t q_mod = GetXRotQuat(M_PI_2);; 636 | std::string errmsg("Error in EffectorSplineRotationRotationTest; while 637 | checking waypoints (expected / obtained)"); ComparePoints(q_init, eff_traj(0), 638 | errmsg,error); ComparePoints(q_to , eff_traj(0.02), errmsg,error); 639 | ComparePoints(q_land, eff_traj(9.98), errmsg,error); 640 | ComparePoints(q_mod , eff_traj(5).tail<4>(), errmsg,error); 641 | ComparePoints(q_end , eff_traj(10), errmsg,error); 642 | } 643 | 644 | void EffectorSplineRotationWayPointRotationTest(bool& error) 645 | { 646 | // create arbitrary trajectory 647 | T_Waypoint waypoints; 648 | for(double i = 0; i <= 10; i = i + 2) 649 | { 650 | waypoints.push_back(std::make_pair(i,point_t(i,i,i))); 651 | } 652 | helpers::quat_t init_quat = GetXRotQuat(0); 653 | helpers::t_waypoint_quat_t quat_waypoints_; 654 | 655 | 656 | helpers::quat_t q_pi_0 = GetXRotQuat(0); 657 | helpers::quat_t q_pi_2 = GetXRotQuat(M_PI_2); 658 | helpers::quat_t q_pi = GetXRotQuat(M_PI); 659 | 660 | quat_waypoints_.push_back(std::make_pair(0.4,q_pi_0)); 661 | quat_waypoints_.push_back(std::make_pair(6,q_pi_2)); 662 | quat_waypoints_.push_back(std::make_pair(8,q_pi)); 663 | 664 | 665 | helpers::effector_spline_rotation eff_traj(waypoints.begin(),waypoints.end(), 666 | quat_waypoints_.begin(), quat_waypoints_.end()); 667 | helpers::config_t q_init = helpers::config_t::Zero(); q_init.tail<4>() = 668 | init_quat; helpers::config_t q_end; q_end << 10.,10.,10.,0.,0.,0.,1.; 669 | q_end.tail<4>() = q_pi; helpers::config_t q_mod; q_mod.head<3>() = 670 | point_t(6,6,6) ; q_mod.tail<4>() = q_pi_2; helpers::config_t q_to = q_init; 671 | q_to(2) +=0.02; helpers::config_t q_land = q_end ; q_land(2)+=0.02; 672 | std::string errmsg("Error in EffectorSplineRotationWayPointRotationTest; while 673 | checking waypoints (expected / obtained)"); ComparePoints(q_init, eff_traj(0), 674 | errmsg,error); ComparePoints(q_to , eff_traj(0.02), errmsg,error); 675 | ComparePoints(q_land, eff_traj(9.98), errmsg,error); 676 | ComparePoints(q_mod , eff_traj(6), errmsg,error); ComparePoints(q_end , 677 | eff_traj(10), errmsg,error); 678 | } 679 | 680 | void TestReparametrization(bool& error) 681 | { 682 | helpers::rotation_spline s; 683 | const helpers::spline_deriv_constraint_one_dim& sp = s.time_reparam_; 684 | if(sp.min() != 0) 685 | { 686 | std::cout << "in TestReparametrization; min value is not 0, got " << sp.min() 687 | << std::endl; error = true; 688 | } 689 | if(sp.max() != 1) 690 | { 691 | std::cout << "in TestReparametrization; max value is not 1, got " << sp.max() 692 | << std::endl; error = true; 693 | } 694 | if(sp(1)[0] != 1.) 695 | { 696 | std::cout << "in TestReparametrization; end value is not 1, got " << sp(1)[0] 697 | << std::endl; error = true; 698 | } 699 | if(sp(0)[0] != 0.) 700 | { 701 | std::cout << "in TestReparametrization; init value is not 0, got " << sp(0)[0] 702 | << std::endl; error = true; 703 | } 704 | for(double i =0; i<1; i+=0.002) 705 | { 706 | if(sp(i)[0]>sp(i+0.002)[0]) 707 | { 708 | std::cout << "in TestReparametrization; reparametrization not monotonous " << 709 | sp.max() << std::endl; error = true; 710 | } 711 | } 712 | } 713 | */ 714 | int main(int /*argc*/, char** /*argv[]*/) { 715 | std::cout << "performing tests... \n"; 716 | bool error = false; 717 | CubicFunctionTest(error); 718 | ExactCubicNoErrorTest(error); 719 | ExactCubicPointsCrossedTest( 720 | error); // checks that given wayPoints are crossed 721 | ExactCubicTwoPointsTest(error); 722 | ExactCubicOneDimTest(error); 723 | ExactCubicVelocityConstraintsTest(error); 724 | // EffectorTrajectoryTest(error); 725 | // EffectorSplineRotationNoRotationTest(error); 726 | // EffectorSplineRotationRotationTest(error); 727 | // TestReparametrization(error); 728 | // EffectorSplineRotationWayPointRotationTest(error); 729 | // BezierCurveTest(error); 730 | // BezierDerivativeCurveTest(error); 731 | // BezierDerivativeCurveConstraintTest(error); 732 | // BezierCurveTestCompareHornerAndBernstein(error); 733 | if (error) { 734 | std::cout << "There were some errors\n"; 735 | return -1; 736 | } else { 737 | std::cout << "no errors found \n"; 738 | return 0; 739 | } 740 | } 741 | -------------------------------------------------------------------------------- /tests/python/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # ADD_PYTHON_UNIT_TEST("py-spline" "tests/python/test.py" 2 | # "python/parametric_curves") 3 | add_python_unit_test("py-quick" "tests/python/quick.py" "python") 4 | -------------------------------------------------------------------------------- /tests/python/quick.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | from numpy import array 4 | from parametric_curves import curve_constraints, forcecurve, polynomial, spline 5 | 6 | 7 | class ParametricCurvesQuickTests(unittest.TestCase): 8 | def test_quick(self): 9 | cc = curve_constraints() 10 | cc.init_vel = array([0, 1, 2]) 11 | 12 | forcecurve() 13 | spline() 14 | pn = polynomial(array([[1, 2, 3], [4, 5, 6], [7, 8, 9]])) 15 | 16 | self.assertEqual(pn.min(), 0.0) 17 | self.assertEqual(pn.max(), 1.0) 18 | 19 | 20 | if __name__ == "__main__": 21 | unittest.main() 22 | -------------------------------------------------------------------------------- /tests/python/test.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | from numpy import matrix 4 | from numpy.linalg import norm 5 | 6 | from spline import ( 7 | bezier, 8 | bezier6, 9 | curve_constraints, 10 | exact_cubic, 11 | polynom, 12 | spline_deriv_constraint, 13 | ) 14 | 15 | 16 | class ParametricCurvesTests(unittest.TestCase): 17 | def test_all(self): 18 | waypoints = matrix([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0]]).transpose() 19 | waypoints6 = matrix( 20 | [[1.0, 2.0, 3.0, 7.0, 5.0, 5.0], [4.0, 5.0, 6.0, 4.0, 5.0, 6.0]] 21 | ).transpose() 22 | time_waypoints = matrix([0.0, 1.0]) 23 | 24 | # testing bezier curve 25 | a = bezier6(waypoints6) 26 | a = bezier(waypoints, -1.0, 3.0) 27 | 28 | self.assertEqual(a.degree, a.nbWaypoints - 1) 29 | a.min() 30 | a.max() 31 | a(0.4) 32 | self.assertTrue((a.derivate(0.4, 0) == a(0.4)).all()) 33 | a.derivate(0.4, 2) 34 | a = a.compute_derivate(100) 35 | 36 | prim = a.compute_primitive(1) 37 | 38 | for i in range(10): 39 | t = float(i) / 10.0 40 | self.assertTrue((a(t) == prim.derivate(t, 1)).all()) 41 | self.assertTrue((prim(0) == matrix([0.0, 0.0, 0.0])).all()) 42 | 43 | prim = a.compute_primitive(2) 44 | for i in range(10): 45 | t = float(i) / 10.0 46 | self.assertTrue((a(t) == prim.derivate(t, 2)).all()) 47 | self.assertTrue((prim(0) == matrix([0.0, 0.0, 0.0])).all()) 48 | 49 | # testing bezier with constraints 50 | c = curve_constraints() 51 | c.init_vel = matrix([0.0, 1.0, 1.0]) 52 | c.end_vel = matrix([0.0, 1.0, 1.0]) 53 | c.init_acc = matrix([0.0, 1.0, -1.0]) 54 | c.end_acc = matrix([0.0, 100.0, 1.0]) 55 | 56 | a = bezier(waypoints, c) 57 | self.assertLess(norm(a.derivate(0, 1) - c.init_vel), 1e-10) 58 | self.assertLess(norm(a.derivate(1, 2) - c.end_acc), 1e-10) 59 | 60 | # testing polynom function 61 | a = polynom(waypoints) 62 | a = polynom(waypoints, -1.0, 3.0) 63 | a.min() 64 | a.max() 65 | a(0.4) 66 | self.assertTrue(((a.derivate(0.4, 0) == a(0.4)).all())) 67 | a.derivate(0.4, 2) 68 | 69 | # testing exact_cubic function 70 | a = exact_cubic(waypoints, time_waypoints) 71 | a.min() 72 | a.max() 73 | a(0.4) 74 | self.assertTrue(((a.derivate(0.4, 0) == a(0.4)).all())) 75 | a.derivate(0.4, 2) 76 | 77 | # testing spline_deriv_constraints 78 | c = curve_constraints() 79 | c.init_vel 80 | c.end_vel 81 | c.init_acc 82 | c.end_acc 83 | 84 | c.init_vel = matrix([0.0, 1.0, 1.0]) 85 | c.end_vel = matrix([0.0, 1.0, 1.0]) 86 | c.init_acc = matrix([0.0, 1.0, 1.0]) 87 | c.end_acc = matrix([0.0, 1.0, 1.0]) 88 | 89 | a = spline_deriv_constraint(waypoints, time_waypoints) 90 | a = spline_deriv_constraint(waypoints, time_waypoints, c) 91 | 92 | 93 | if __name__ == "__main__": 94 | unittest.main() 95 | --------------------------------------------------------------------------------