Unverified Commit c35a2071 authored by Guilhem Saurel's avatar Guilhem Saurel Committed by GitHub
Browse files

Merge pull request #19 from nim65s/devel

format
parents b27f096f f0f9f7b3
Pipeline #18175 failed with stage
in 17 minutes and 7 seconds
# format (Guilhem Saurel, 2022-04-05)
7e4f60e57398fb63c6ee43d74cc4775e0c5bf435
# Format: yapf 0.32 (Guilhem Saurel, 2022-02-07)
f3e915be9dfc8132db9dc2dd65443bb80cef2e93
# [Python] format (Guilhem Saurel, 2019-10-09)
cba6436a362584053e9ebd4f10dcb641299d9017
...@@ -8,4 +8,4 @@ _build* ...@@ -8,4 +8,4 @@ _build*
.*~ .*~
*.user *.user
*~ *~
*.pyc *.pyc
\ No newline at end of file
repos:
- repo: https://github.com/pre-commit/mirrors-clang-format
rev: v13.0.1
hooks:
- id: clang-format
args: [-i, --style=Google]
- repo: https://github.com/pre-commit/pre-commit-hooks
rev: v4.1.0
hooks:
- id: check-added-large-files
- id: check-ast
- id: check-executables-have-shebangs
- id: check-json
- id: check-merge-conflict
- id: check-symlinks
- id: check-toml
- id: check-yaml
- id: debug-statements
- id: destroyed-symlinks
- id: detect-private-key
- id: end-of-file-fixer
- id: fix-byte-order-marker
- id: mixed-line-ending
- id: trailing-whitespace
- repo: https://github.com/psf/black
rev: 22.3.0
hooks:
- id: black
- repo: https://github.com/PyCQA/flake8
rev: 4.0.1
hooks:
- id: flake8
Subproject commit dc8b946d456d2c41ad12b819111b005148c68031 Subproject commit cbc92f81bdd0dcfd7fc1eae56801d8111267937d
Spline # Spline
===================
[![Building Status](https://travis-ci.org/stack-of-tasks/parametric-curves.svg?branch=master)](https://travis-ci.org/stack-of-tasks/parametric-curves) [![Building Status](https://travis-ci.org/stack-of-tasks/parametric-curves.svg?branch=master)](https://travis-ci.org/stack-of-tasks/parametric-curves)
[![Pipeline status](https://gepgitlab.laas.fr/stack-of-tasks/parametric-curves/badges/master/pipeline.svg)](https://gepgitlab.laas.fr/stack-of-tasks/parametric-curves/commits/master) [![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)
[![Coverage report](https://gepgitlab.laas.fr/stack-of-tasks/parametric-curves/badges/master/coverage.svg?job=doc-coverage)](http://projects.laas.fr/gepetto/doc/stack-of-tasks/parametric-curves/master/coverage/) [![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/)
[![Code style: black](https://img.shields.io/badge/code%20style-black-000000.svg)](https://github.com/psf/black)
[![pre-commit.ci status](https://results.pre-commit.ci/badge/github/stack-of-tasks/parametric-curves/master.svg)](https://results.pre-commit.ci/latest/github/stack-of-tasks/parametric-curves)
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. 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.
...@@ -18,9 +19,8 @@ The library is template-based, thus generic: the curves can be of any dimension ...@@ -18,9 +19,8 @@ The library is template-based, thus generic: the curves can be of any dimension
While a Bezier curve implementation is provided, the main interest While a Bezier curve implementation is provided, the main interest
of this library is to create spline curves of arbitrary order of this library is to create spline curves of arbitrary order
---------- ## Example of use for and end-effector trajectory
Example of use for and end-effector trajectory
-------------
The library comes with an helper class to automatically generate end-effector trajectories. The library comes with an helper class to automatically generate end-effector trajectories.
For instance, to create a 2 second long trajectory from the point (0,0,0) to (1,1,0), with a waypoint For instance, to create a 2 second long trajectory from the point (0,0,0) to (1,1,0), with a waypoint
at (0.5,0.5,0.5), one can use the following code: at (0.5,0.5,0.5), one can use the following code:
...@@ -60,12 +60,12 @@ Additional parameters for the same methods an be used to specify parameters for ...@@ -60,12 +60,12 @@ Additional parameters for the same methods an be used to specify parameters for
landing phases: height and duration of the phase, and along which normal. landing phases: height and duration of the phase, and along which normal.
Please refer to the Main.cpp files to see all the unit tests and possibilities offered by the library Please refer to the Main.cpp files to see all the unit tests and possibilities offered by the library
Installation ## Installation
-------------
## Dependencies ### Dependencies
* [Eigen (version >= 3.2.2)](http://eigen.tuxfamily.org/index.php?title=Main_Page) * [Eigen (version >= 3.2.2)](http://eigen.tuxfamily.org/index.php?title=Main_Page)
## Additional dependencies for python bindings ### Additional dependencies for python bindings
* [Boost.Python](http://www.boost.org/doc/libs/1_63_0/libs/python/doc/html/index.html) * [Boost.Python](http://www.boost.org/doc/libs/1_63_0/libs/python/doc/html/index.html)
* [eigenpy](https://github.com/stack-of-tasks/eigenpy) * [eigenpy](https://github.com/stack-of-tasks/eigenpy)
...@@ -87,7 +87,7 @@ If everything went fine you should obtain the following output: ...@@ -87,7 +87,7 @@ If everything went fine you should obtain the following output:
performing tests... performing tests...
no errors found no errors found
``` ```
### Optional: Python bindings installation #### Optional: Python bindings installation
To install the Python bindings, in the CMakeLists.txt file, first enable the BUILD_PYTHON_INTERFACE option: To install the Python bindings, in the CMakeLists.txt file, first enable the BUILD_PYTHON_INTERFACE option:
``` ```
OPTION (BUILD_PYTHON_INTERFACE "Build the python binding" ON) OPTION (BUILD_PYTHON_INTERFACE "Build the python binding" ON)
......
Subproject commit b0a77e2ead3f8dbb201b05e42048bb1cfc6a518e Subproject commit 3d6176d439963702d97b82555e3007de05a4e6a4
...@@ -17,21 +17,23 @@ ...@@ -17,21 +17,23 @@
#include <Eigen/Dense> #include <Eigen/Dense>
#include <Eigen/SVD> #include <Eigen/SVD>
#include <vector>
#include <utility> #include <utility>
#include <vector>
namespace parametriccurves { namespace parametriccurves {
// REF: boulic et al An inverse kinematics architecture enforcing an arbitrary number of strict priority levels // REF: boulic et al An inverse kinematics architecture enforcing an arbitrary
// number of strict priority levels
template <typename _Matrix_Type_> template <typename _Matrix_Type_>
void PseudoInverse(_Matrix_Type_& pinvmat) { void PseudoInverse(_Matrix_Type_& pinvmat) {
Eigen::JacobiSVD<_Matrix_Type_> svd(pinvmat, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::JacobiSVD<_Matrix_Type_> svd(
pinvmat, Eigen::ComputeFullU | Eigen::ComputeFullV);
_Matrix_Type_ m_sigma = svd.singularValues(); _Matrix_Type_ m_sigma = svd.singularValues();
double pinvtoler = 1.e-6; // choose your tolerance widely! double pinvtoler = 1.e-6; // choose your tolerance widely!
_Matrix_Type_ m_sigma_inv = _Matrix_Type_::Zero(pinvmat.cols(), pinvmat.rows()); _Matrix_Type_ m_sigma_inv =
_Matrix_Type_::Zero(pinvmat.cols(), pinvmat.rows());
for (long i = 0; i < m_sigma.rows(); ++i) { for (long i = 0; i < m_sigma.rows(); ++i) {
if (m_sigma(i) > pinvtoler) m_sigma_inv(i, i) = 1.0 / m_sigma(i); if (m_sigma(i) > pinvtoler) m_sigma_inv(i, i) = 1.0 / m_sigma(i);
} }
......
...@@ -39,13 +39,16 @@ struct AbstractCurve { ...@@ -39,13 +39,16 @@ struct AbstractCurve {
/// \param t : the time when to evaluate the spline /// \param t : the time when to evaluate the spline
/// \param order : order of the derivative /// \param order : order of the derivative
/// \param return : the value x(t) /// \param return : the value x(t)
virtual const point_t derivate(const time_t& t, const std::size_t& order) const = 0; virtual const point_t derivate(const time_t& t,
const std::size_t& order) const = 0;
public: public:
/*Getters*/ /*Getters*/
virtual const time_t tmin() const { return t_min; } virtual const time_t tmin() const { return t_min; }
virtual const time_t tmax() const { return t_max; } virtual const time_t tmax() const { return t_max; }
virtual bool checkRange(const time_t t) const { return (t >= t_min) && (t <= t_max); } virtual bool checkRange(const time_t t) const {
return (t >= t_min) && (t <= t_max);
}
/* Setters */ /* Setters */
virtual bool setInitialPoint(const point_t& /*x_init*/) = 0; virtual bool setInitialPoint(const point_t& /*x_init*/) = 0;
......
...@@ -9,13 +9,13 @@ ...@@ -9,13 +9,13 @@
#ifndef _CLASS_BERNSTEIN #ifndef _CLASS_BERNSTEIN
#define _CLASS_BERNSTEIN #define _CLASS_BERNSTEIN
#include "curve_abc.h"
#include "MathDefs.h"
#include <math.h> #include <math.h>
#include <vector>
#include <stdexcept> #include <stdexcept>
#include <vector>
#include "MathDefs.h"
#include "curve_abc.h"
namespace spline { namespace spline {
/// ///
...@@ -31,14 +31,17 @@ unsigned int fact(const unsigned int n) { ...@@ -31,14 +31,17 @@ unsigned int fact(const unsigned int n) {
/// ///
/// \brief Computes a binomal coefficient /// \brief Computes a binomal coefficient
/// ///
unsigned int bin(const unsigned int n, const unsigned int k) { return fact(n) / (fact(k) * fact(n - k)); } unsigned int bin(const unsigned int n, const unsigned int k) {
return fact(n) / (fact(k) * fact(n - k));
}
/// \class Bernstein /// \class Bernstein
/// \brief Computes a Bernstein polynome /// \brief Computes a Bernstein polynome
/// ///
template <typename Numeric = double> template <typename Numeric = double>
struct Bern { struct Bern {
Bern(const unsigned int m, const unsigned int i) : m_minus_i(m - i), i_(i), bin_m_i_(bin(m, i)) {} Bern(const unsigned int m, const unsigned int i)
: m_minus_i(m - i), i_(i), bin_m_i_(bin(m, i)) {}
~Bern() {} ~Bern() {}
......
...@@ -9,16 +9,14 @@ ...@@ -9,16 +9,14 @@
#ifndef _CLASS_BEZIERCURVE #ifndef _CLASS_BEZIERCURVE
#define _CLASS_BEZIERCURVE #define _CLASS_BEZIERCURVE
#include "curve_abc.h" #include <iostream>
#include "bernstein.h"
#include "curve_constraint.h"
#include "MathDefs.h"
#include <vector>
#include <stdexcept> #include <stdexcept>
#include <vector>
#include <iostream> #include "MathDefs.h"
#include "bernstein.h"
#include "curve_abc.h"
#include "curve_constraint.h"
namespace spline { namespace spline {
/// \class BezierCurve /// \class BezierCurve
...@@ -26,8 +24,8 @@ namespace spline { ...@@ -26,8 +24,8 @@ namespace spline {
/// For degree lesser than 4, the evaluation is analitycal.Otherwise /// For degree lesser than 4, the evaluation is analitycal.Otherwise
/// the bernstein polynoms are used to evaluate the spline at a given location. /// the bernstein polynoms are used to evaluate the spline at a given location.
/// ///
template <typename Time = double, typename Numeric = Time, Eigen::Index Dim = 3, bool Safe = false, template <typename Time = double, typename Numeric = Time, Eigen::Index Dim = 3,
typename Point = Eigen::Matrix<Numeric, Dim, 1> > bool Safe = false, typename Point = Eigen::Matrix<Numeric, Dim, 1> >
struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> { struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> {
typedef Point point_t; typedef Point point_t;
typedef Time time_t; typedef Time time_t;
...@@ -43,7 +41,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> { ...@@ -43,7 +41,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> {
///\param PointsBegin, PointsEnd : the points parametering the Bezier curve ///\param PointsBegin, PointsEnd : the points parametering the Bezier curve
/// ///
template <typename In> template <typename In>
bezier_curve(In PointsBegin, In PointsEnd, const time_t minBound = 0, const time_t maxBound = 1) bezier_curve(In PointsBegin, In PointsEnd, const time_t minBound = 0,
const time_t maxBound = 1)
: minBound_(minBound), : minBound_(minBound),
maxBound_(maxBound), maxBound_(maxBound),
size_(std::distance(PointsBegin, PointsEnd)), size_(std::distance(PointsBegin, PointsEnd)),
...@@ -52,28 +51,35 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> { ...@@ -52,28 +51,35 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> {
assert(bernstein_.size() == size_); assert(bernstein_.size() == size_);
In it(PointsBegin); In it(PointsBegin);
if (Safe && (size_ < 1 || minBound >= maxBound)) if (Safe && (size_ < 1 || minBound >= maxBound))
throw std::out_of_range("can't create bezier min bound is higher than max bound"); // TODO throw std::out_of_range(
"can't create bezier min bound is higher than max bound"); // TODO
for (; it != PointsEnd; ++it) pts_.push_back(*it); for (; it != PointsEnd; ++it) pts_.push_back(*it);
} }
///\brief Constructor ///\brief Constructor
/// This constructor will add 4 points (2 after the first one, 2 before the last one) /// This constructor will add 4 points (2 after the first one, 2 before the
/// to ensure that velocity and acceleration constraints are respected /// last one) to ensure that velocity and acceleration constraints are
/// respected
///\param PointsBegin, PointsEnd : the points parametering the Bezier curve ///\param PointsBegin, PointsEnd : the points parametering the Bezier curve
///\param constraints : constraints applying on start / end velocities and acceleration ///\param constraints : constraints applying on start / end velocities and
/// acceleration
/// ///
template <typename In> template <typename In>
bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints, const time_t minBound = 0, bezier_curve(In PointsBegin, In PointsEnd,
const time_t maxBound = 1) const curve_constraints_t& constraints,
const time_t minBound = 0, const time_t maxBound = 1)
: minBound_(minBound), : minBound_(minBound),
maxBound_(maxBound), maxBound_(maxBound),
size_(std::distance(PointsBegin, PointsEnd) + 4), size_(std::distance(PointsBegin, PointsEnd) + 4),
degree_(size_ - 1), degree_(size_ - 1),
bernstein_(spline::makeBernstein<num_t>(degree_)) { bernstein_(spline::makeBernstein<num_t>(degree_)) {
if (Safe && (size_ < 1 || minBound >= maxBound)) if (Safe && (size_ < 1 || minBound >= maxBound))
throw std::out_of_range("can't create bezier min bound is higher than max bound"); throw std::out_of_range(
t_point_t updatedList = add_constraints<In>(PointsBegin, PointsEnd, constraints); "can't create bezier min bound is higher than max bound");
for (cit_point_t cit = updatedList.begin(); cit != updatedList.end(); ++cit) pts_.push_back(*cit); t_point_t updatedList =
add_constraints<In>(PointsBegin, PointsEnd, constraints);
for (cit_point_t cit = updatedList.begin(); cit != updatedList.end(); ++cit)
pts_.push_back(*cit);
} }
///\brief Destructor ///\brief Destructor
...@@ -94,7 +100,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> { ...@@ -94,7 +100,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> {
virtual point_t operator()(const time_t t) const { virtual point_t operator()(const time_t t) const {
num_t nT = (t - minBound_) / (maxBound_ - minBound_); num_t nT = (t - minBound_) / (maxBound_ - minBound_);
if (Safe & !(0 <= nT && nT <= 1)) { if (Safe & !(0 <= nT && nT <= 1)) {
throw std::out_of_range("can't evaluate bezier curve, out of range"); // TODO throw std::out_of_range(
"can't evaluate bezier curve, out of range"); // TODO
} else { } else {
num_t dt = (1 - nT); num_t dt = (1 - nT);
switch (size_) { switch (size_) {
...@@ -107,8 +114,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> { ...@@ -107,8 +114,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> {
return pts_[0] * dt * dt + 2 * pts_[1] * nT * dt + pts_[2] * nT * nT; return pts_[0] * dt * dt + 2 * pts_[1] * nT * dt + pts_[2] * nT * nT;
break; break;
case 4: case 4:
return pts_[0] * dt * dt * dt + 3 * pts_[1] * nT * dt * dt + 3 * pts_[2] * nT * nT * dt + return pts_[0] * dt * dt * dt + 3 * pts_[1] * nT * dt * dt +
pts_[3] * nT * nT * nT; 3 * pts_[2] * nT * nT * dt + pts_[3] * nT * nT * nT;
default: default:
return evalHorner(nT); return evalHorner(nT);
break; break;
...@@ -122,10 +129,12 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> { ...@@ -122,10 +129,12 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> {
bezier_curve_t compute_derivate(const std::size_t order) const { bezier_curve_t compute_derivate(const std::size_t order) const {
if (order == 0) return *this; if (order == 0) return *this;
t_point_t derived_wp; t_point_t derived_wp;
for (typename t_point_t::const_iterator pit = pts_.begin(); pit != pts_.end() - 1; ++pit) for (typename t_point_t::const_iterator pit = pts_.begin();
pit != pts_.end() - 1; ++pit)
derived_wp.push_back(degree_ * (*(pit + 1) - (*pit))); derived_wp.push_back(degree_ * (*(pit + 1) - (*pit)));
if (derived_wp.empty()) derived_wp.push_back(point_t::Zero()); if (derived_wp.empty()) derived_wp.push_back(point_t::Zero());
bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(), minBound_, maxBound_); bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(), minBound_,
maxBound_);
return deriv.compute_derivate(order - 1); return deriv.compute_derivate(order - 1);
} }
...@@ -137,10 +146,11 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> { ...@@ -137,10 +146,11 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> {
num_t new_degree = (num_t)(degree_ + 1); num_t new_degree = (num_t)(degree_ + 1);
t_point_t n_wp; t_point_t n_wp;
point_t current_sum = point_t::Zero(); point_t current_sum = point_t::Zero();
// recomputing waypoints q_i from derivative waypoints p_i. q_0 is the given constant. // recomputing waypoints q_i from derivative waypoints p_i. q_0 is the given
// then q_i = (sum( j = 0 -> j = i-1) p_j) /n+1 // constant. then q_i = (sum( j = 0 -> j = i-1) p_j) /n+1
n_wp.push_back(current_sum); n_wp.push_back(current_sum);
for (typename t_point_t::const_iterator pit = pts_.begin(); pit != pts_.end(); ++pit) { for (typename t_point_t::const_iterator pit = pts_.begin();
pit != pts_.end(); ++pit) {
current_sum += *pit; current_sum += *pit;
n_wp.push_back(current_sum / new_degree); n_wp.push_back(current_sum / new_degree);
} }
...@@ -165,14 +175,16 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> { ...@@ -165,14 +175,16 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> {
point_t evalBernstein(const Numeric u) const { point_t evalBernstein(const Numeric u) const {
point_t res = point_t::Zero(); point_t res = point_t::Zero();
typename t_point_t::const_iterator pts_it = pts_.begin(); typename t_point_t::const_iterator pts_it = pts_.begin();
for (typename std::vector<Bern<Numeric> >::const_iterator cit = bernstein_.begin(); cit != bernstein_.end(); for (typename std::vector<Bern<Numeric> >::const_iterator cit =
++cit, ++pts_it) bernstein_.begin();
cit != bernstein_.end(); ++cit, ++pts_it)
res += cit->operator()(u) * (*pts_it); res += cit->operator()(u) * (*pts_it);
return res; return res;
} }
/// ///
/// \brief Evaluates all Bernstein polynomes for a certain degree using horner's scheme /// \brief Evaluates all Bernstein polynomes for a certain degree using
/// horner's scheme
/// ///
point_t evalHorner(const Numeric t) const { point_t evalHorner(const Numeric t) const {
typename t_point_t::const_iterator pts_it = pts_.begin(); typename t_point_t::const_iterator pts_it = pts_.begin();
...@@ -194,7 +206,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> { ...@@ -194,7 +206,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> {
private: private:
template <typename In> template <typename In>
t_point_t add_constraints(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints) { t_point_t add_constraints(In PointsBegin, In PointsEnd,
const curve_constraints_t& constraints) {
t_point_t res; t_point_t res;
point_t P0, P1, P2, P_n_2, P_n_1, PN; point_t P0, P1, P2, P_n_2, P_n_1, PN;
P0 = *PointsBegin; P0 = *PointsBegin;
......
...@@ -17,7 +17,8 @@ namespace parametriccurves { ...@@ -17,7 +17,8 @@ namespace parametriccurves {
/// \brief Creates InfiniteSinusoid curve /// \brief Creates InfiniteSinusoid curve
/// The sinusoid is actually a cosine so that it starts with zero velocity. /// The sinusoid is actually a cosine so that it starts with zero velocity.
/// Returns x = x_init + A*cos(2*pi*f*t) where f is give by 1/(2*traj_time) /// Returns x = x_init + A*cos(2*pi*f*t) where f is give by 1/(2*traj_time)
template <typename Numeric = double, Eigen::Index Dim = 1, typename Point = Eigen::Matrix<Numeric, Dim, 1> > template <typename Numeric = double, Eigen::Index Dim = 1,
typename Point = Eigen::Matrix<Numeric, Dim, 1> >
struct Constant : public AbstractCurve<Numeric, Point> { struct Constant : public AbstractCurve<Numeric, Point> {
typedef Point point_t; typedef Point point_t;
typedef Numeric time_t; typedef Numeric time_t;
...@@ -27,7 +28,8 @@ struct Constant : public AbstractCurve<Numeric, Point> { ...@@ -27,7 +28,8 @@ struct Constant : public AbstractCurve<Numeric, Point> {
public: public:
///\brief Constructor ///\brief Constructor
Constant(const time_t& traj_time, const point_t& x_init_ = Eigen::Matrix<Numeric, Dim, 1>::Zero()) Constant(const time_t& traj_time,
const point_t& x_init_ = Eigen::Matrix<Numeric, Dim, 1>::Zero())
: curve_abc_t(0, traj_time), x_init(x_init_) {} : curve_abc_t(0, traj_time), x_init(x_init_) {}
///\brief Destructor ///\brief Destructor
...@@ -36,7 +38,10 @@ struct Constant : public AbstractCurve<Numeric, Point> { ...@@ -36,7 +38,10 @@ struct Constant : public AbstractCurve<Numeric, Point> {
public: public:
virtual const point_t operator()(const time_t& t) const { return x_init; } virtual const point_t operator()(const time_t& t) const { return x_init; }
virtual const point_t derivate(const time_t& t, const std::size_t& order) const { return point_t::Zero(); } virtual const point_t derivate(const time_t& t,
const std::size_t& order) const {
return point_t::Zero();
}
virtual bool setInitialPoint(const point_t& x_init_) { virtual bool setInitialPoint(const point_t& x_init_) {
if (x_init.size() != x_init_.size()) return false; if (x_init.size() != x_init_.size()) return false;
......
/** /**
* \file curve_constraint.h * \file curve_constraint.h
* \brief struct to define constraints on start / end velocities and acceleration * \brief struct to define constraints on start / end velocities and
* on a curve * acceleration on a curve \author Steve T. \version 0.1 \date 04/05/2017
* \author Steve T.
* \version 0.1
* \date 04/05/2017
* *
*/ */
...@@ -20,7 +17,11 @@ namespace parametriccurves { ...@@ -20,7 +17,11 @@ namespace parametriccurves {
template <typename Point> template <typename Point>
struct curve_constraints { struct curve_constraints {
typedef Point point_t; typedef Point point_t;
curve_constraints() : init_vel(point_t::Zero()), init_acc(init_vel), end_vel(init_vel), end_acc(init_vel) {} curve_constraints()
: init_vel(point_t::Zero()),
init_acc(init_vel),
end_vel(init_vel),
end_acc(init_vel) {}
~curve_constraints() {} ~curve_constraints() {}
point_t init_vel; point_t init_vel;
......
...@@ -6,8 +6,8 @@ ...@@ -6,8 +6,8 @@
* \date 06/17/2013 * \date 06/17/2013
* *
* This file contains definitions for the ExactCubic class. * This file contains definitions for the ExactCubic class.
* Given a set of waypoints (x_i*) and timestep (t_i), it provides the unique set of * Given a set of waypoints (x_i*) and timestep (t_i), it provides the unique
* cubic splines fulfulling those 4 restrictions : * set of cubic splines fulfulling those 4 restrictions :
* - x_i(t_i) = x_i* ; this means that the curve passes trough each waypoint * - x_i(t_i) = x_i* ; this means that the curve passes trough each waypoint
* - x_i(t_i+1) = x_i+1* ; * - x_i(t_i+1) = x_i+1* ;
* - its derivative is continous at t_i+1 * - its derivative is continous at t_i+1
...@@ -29,20 +29,25 @@ typedef Eigen::Matrix<Numeric, 3, 1> Point; ...@@ -29,20 +29,25 @@ typedef Eigen::Matrix<Numeric, 3, 1> Point;
typedef std::vector<Point, Eigen::aligned_allocator<Point> > T_Point; typedef std::vector<Point, Eigen::aligned_allocator<Point> > T_Point;
typedef std::pair<double, Point> Waypoint; typedef std::pair<double, Point> Waypoint;
typedef std::vector<Waypoint> T_Waypoint; typedef std::vector<Waypoint> T_Waypoint;
typedef spline_deriv_constraint<Time, Numeric, 3, true, Point, T_Point> spline_deriv_constraint_t; typedef spline_deriv_constraint<Time, Numeric, 3, true, Point, T_Point>
spline_deriv_constraint_t;
typedef spline_deriv_constraint_t::spline_constraints spline_constraints_t; typedef spline_deriv_constraint_t::spline_constraints spline_constraints_t;
typedef spline_deriv_constraint_t::exact_cubic_t exact_cubic_t; typedef spline_deriv_constraint_t::exact_cubic_t exact_cubic_t;
typedef spline_deriv_constraint_t::t_spline_t t_spline_t; typedef spline_deriv_constraint_t::t_spline_t t_spline_t;
typedef spline_deriv_constraint_t