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
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)
[![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)
[![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/)
[![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://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.
......@@ -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
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.
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:
......@@ -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.
Please refer to the Main.cpp files to see all the unit tests and possibilities offered by the library
Installation
-------------
## Dependencies
## Installation
### Dependencies
* [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)
* [eigenpy](https://github.com/stack-of-tasks/eigenpy)
......@@ -87,7 +87,7 @@ If everything went fine you should obtain the following output:
performing tests...
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:
```
OPTION (BUILD_PYTHON_INTERFACE "Build the python binding" ON)
......
Subproject commit b0a77e2ead3f8dbb201b05e42048bb1cfc6a518e
Subproject commit 3d6176d439963702d97b82555e3007de05a4e6a4
......@@ -17,21 +17,23 @@
#include <Eigen/Dense>
#include <Eigen/SVD>
#include <vector>
#include <utility>
#include <vector>
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_>
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();
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) {
if (m_sigma(i) > pinvtoler) m_sigma_inv(i, i) = 1.0 / m_sigma(i);
}
......
......@@ -39,13 +39,16 @@ struct AbstractCurve {
/// \param t : the time when to evaluate the spline
/// \param order : order of the derivative
/// \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:
/*Getters*/
virtual const time_t tmin() const { return t_min; }
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 */
virtual bool setInitialPoint(const point_t& /*x_init*/) = 0;
......
......@@ -9,13 +9,13 @@
#ifndef _CLASS_BERNSTEIN
#define _CLASS_BERNSTEIN
#include "curve_abc.h"
#include "MathDefs.h"
#include <math.h>
#include <vector>
#include <stdexcept>
#include <vector>
#include "MathDefs.h"
#include "curve_abc.h"
namespace spline {
///
......@@ -31,14 +31,17 @@ unsigned int fact(const unsigned int n) {
///
/// \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
/// \brief Computes a Bernstein polynome
///
template <typename Numeric = double>
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() {}
......
......@@ -9,16 +9,14 @@
#ifndef _CLASS_BEZIERCURVE
#define _CLASS_BEZIERCURVE
#include "curve_abc.h"
#include "bernstein.h"
#include "curve_constraint.h"
#include "MathDefs.h"
#include <vector>
#include <iostream>
#include <stdexcept>
#include <vector>
#include <iostream>
#include "MathDefs.h"
#include "bernstein.h"
#include "curve_abc.h"
#include "curve_constraint.h"
namespace spline {
/// \class BezierCurve
......@@ -26,8 +24,8 @@ namespace spline {
/// For degree lesser than 4, the evaluation is analitycal.Otherwise
/// 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,
typename Point = Eigen::Matrix<Numeric, Dim, 1> >
template <typename Time = double, typename Numeric = Time, Eigen::Index Dim = 3,
bool Safe = false, typename Point = Eigen::Matrix<Numeric, Dim, 1> >
struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> {
typedef Point point_t;
typedef Time time_t;
......@@ -43,7 +41,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> {
///\param PointsBegin, PointsEnd : the points parametering the Bezier curve
///
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),
maxBound_(maxBound),
size_(std::distance(PointsBegin, PointsEnd)),
......@@ -52,28 +51,35 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> {
assert(bernstein_.size() == size_);
In it(PointsBegin);
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);
}
///\brief Constructor
/// This constructor will add 4 points (2 after the first one, 2 before the last one)
/// to ensure that velocity and acceleration constraints are respected
/// This constructor will add 4 points (2 after the first one, 2 before the
/// last one) to ensure that velocity and acceleration constraints are
/// respected
///\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>
bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints, const time_t minBound = 0,
const time_t maxBound = 1)
bezier_curve(In PointsBegin, In PointsEnd,
const curve_constraints_t& constraints,
const time_t minBound = 0, const time_t maxBound = 1)
: minBound_(minBound),
maxBound_(maxBound),
size_(std::distance(PointsBegin, PointsEnd) + 4),
degree_(size_ - 1),
bernstein_(spline::makeBernstein<num_t>(degree_)) {
if (Safe && (size_ < 1 || minBound >= maxBound))
throw std::out_of_range("can't create bezier min bound is higher than max bound");
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);
throw std::out_of_range(
"can't create bezier min bound is higher than max bound");
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
......@@ -94,7 +100,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> {
virtual point_t operator()(const time_t t) const {
num_t nT = (t - minBound_) / (maxBound_ - minBound_);
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 {
num_t dt = (1 - nT);
switch (size_) {
......@@ -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;
break;
case 4:
return pts_[0] * dt * dt * dt + 3 * pts_[1] * nT * dt * dt + 3 * pts_[2] * nT * nT * dt +
pts_[3] * nT * nT * nT;
return pts_[0] * dt * dt * dt + 3 * pts_[1] * nT * dt * dt +
3 * pts_[2] * nT * nT * dt + pts_[3] * nT * nT * nT;
default:
return evalHorner(nT);
break;
......@@ -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 {
if (order == 0) return *this;
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)));
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);
}
......@@ -137,10 +146,11 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> {
num_t new_degree = (num_t)(degree_ + 1);
t_point_t n_wp;
point_t current_sum = point_t::Zero();
// recomputing waypoints q_i from derivative waypoints p_i. q_0 is the given constant.
// then q_i = (sum( j = 0 -> j = i-1) p_j) /n+1
// recomputing waypoints q_i from derivative waypoints p_i. q_0 is the given
// constant. then q_i = (sum( j = 0 -> j = i-1) p_j) /n+1
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;
n_wp.push_back(current_sum / new_degree);
}
......@@ -165,14 +175,16 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> {
point_t evalBernstein(const Numeric u) const {
point_t res = point_t::Zero();
typename t_point_t::const_iterator pts_it = pts_.begin();
for (typename std::vector<Bern<Numeric> >::const_iterator cit = bernstein_.begin(); cit != bernstein_.end();
++cit, ++pts_it)
for (typename std::vector<Bern<Numeric> >::const_iterator cit =
bernstein_.begin();
cit != bernstein_.end(); ++cit, ++pts_it)
res += cit->operator()(u) * (*pts_it);
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 {
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> {
private:
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;
point_t P0, P1, P2, P_n_2, P_n_1, PN;
P0 = *PointsBegin;
......
......@@ -17,7 +17,8 @@ namespace parametriccurves {
/// \brief Creates InfiniteSinusoid curve
/// 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)
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> {
typedef Point point_t;
typedef Numeric time_t;
......@@ -27,7 +28,8 @@ struct Constant : public AbstractCurve<Numeric, Point> {
public:
///\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_) {}
///\brief Destructor
......@@ -36,7 +38,10 @@ struct Constant : public AbstractCurve<Numeric, Point> {
public:
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_) {
if (x_init.size() != x_init_.size()) return false;
......
/**
* \file curve_constraint.h
* \brief struct to define constraints on start / end velocities and acceleration
* on a curve
* \author Steve T.
* \version 0.1
* \date 04/05/2017
* \brief struct to define constraints on start / end velocities and
* acceleration on a curve \author Steve T. \version 0.1 \date 04/05/2017
*
*/
......@@ -20,7 +17,11 @@ namespace parametriccurves {
template <typename Point>
struct curve_constraints {
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() {}
point_t init_vel;
......
......@@ -6,8 +6,8 @@
* \date 06/17/2013
*
* 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
* cubic splines fulfulling those 4 restrictions :
* Given a set of waypoints (x_i*) and timestep (t_i), it provides the unique
* 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+1) = x_i+1* ;
* - its derivative is continous at t_i+1
......@@ -29,20 +29,25 @@ typedef Eigen::Matrix<Numeric, 3, 1> Point;
typedef std::vector<Point, Eigen::aligned_allocator<Point> > T_Point;
typedef std::pair<double, Point> 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::exact_cubic_t exact_cubic_t;
typedef spline_deriv_constraint_t::t_spline_t t_spline_t;
typedef spline_deriv_constraint_t::spline_t spline_t;
Waypoint compute_offset(const Waypoint& source, const Point& normal, const Numeric offset, const Time time_offset) {
// compute time such that the equation from source to offsetpoint is necessarily a line
Waypoint compute_offset(const Waypoint& source, const Point& normal,
const Numeric offset, const Time time_offset) {
// compute time such that the equation from source to offsetpoint is
// necessarily a line
Numeric norm = normal.norm();
assert(norm > 0.);
return std::make_pair(source.first + time_offset, (source.second + normal / norm * offset));
return std::make_pair(source.first + time_offset,
(source.second + normal / norm * offset));
}
spline_t make_end_spline(const Point& normal, const Point& from, const Numeric offset, const Time init_time,
spline_t make_end_spline(const Point& normal, const Point& from,
const Numeric offset, const Time init_time,
const Time time_offset) {
// compute spline from land way point to end point
// constraints are null velocity and acceleration
......@@ -59,11 +64,12 @@ spline_t make_end_spline(const Point& normal, const Point& from, const Numeric o
points.push_back(c);
points.push_back(d);
return spline_t(points.begin(), points.end(), init_time, init_time + time_offset);
return spline_t(points.begin(), points.end(), init_time,
init_time + time_offset);
}
spline_constraints_t compute_required_offset_velocity_acceleration(const spline_t& end_spline,
const Time time_offset) {
spline_constraints_t compute_required_offset_velocity_acceleration(
const spline_t& end_spline, const Time time_offset) {
// computing end velocity: along landing normal and respecting time
spline_constraints_t constraints;
constraints.end_acc = end_spline.derivate(end_spline.min(), 2);
......@@ -76,36 +82,47 @@ spline_constraints_t compute_required_offset_velocity_acceleration(const spline_
/// Given a set of waypoints, and the normal vector of the start and
/// ending positions, automatically create the spline such that:
/// + init and end velocities / accelerations are 0.
/// + the effector lifts and lands exactly in the direction of the specified normals
/// \param wayPointsBegin : an iterator pointing to the first element of a waypoint container
/// \param wayPointsEnd : an iterator pointing to the end of a waypoint container
/// \param lift_normal : normal to be followed by end effector at take-off
/// \param land_normal : normal to be followed by end effector at landing
/// \param lift_offset : length of the straight line along normal at take-off
/// \param land_offset : length of the straight line along normal at landing
/// \param lift_offset_duration : time travelled along straight line at take-off
/// \param land_offset_duration : time travelled along straight line at landing
/// + the effector lifts and lands exactly in the direction of the specified
/// normals \param wayPointsBegin : an iterator pointing to the first element
/// of a waypoint container \param wayPointsEnd : an iterator pointing to
/// the end of a waypoint container \param lift_normal : normal
/// to be followed by end effector at take-off \param land_normal : normal
/// to be followed by end effector at landing \param lift_offset : length
/// of the straight line along normal at take-off \param land_offset :
/// length of the straight line along normal at landing \param
/// lift_offset_duration : time travelled along straight line at take-off \param
/// land_offset_duration : time travelled along straight line at landing
///
template <typename In>
exact_cubic_t* effector_spline(In wayPointsBegin, In wayPointsEnd, const Point& lift_normal = Eigen::Vector3d::UnitZ(),
const Point& land_normal = Eigen::Vector3d::UnitZ(), const Numeric lift_offset = 0.02,
const Numeric land_offset = 0.02, const Time lift_offset_duration = 0.02,
exact_cubic_t* effector_spline(
In wayPointsBegin, In wayPointsEnd,
const Point& lift_normal = Eigen::Vector3d::UnitZ(),
const Point& land_normal = Eigen::Vector3d::UnitZ(),
const Numeric lift_offset = 0.02, const Numeric land_offset = 0.02,
const Time lift_offset_duration = 0.02,
const Time land_offset_duration = 0.02) {
T_Waypoint waypoints;
const Waypoint &inPoint = *wayPointsBegin, endPoint = *(wayPointsEnd - 1);
waypoints.push_back(inPoint);
// adding initial offset
waypoints.push_back(compute_offset(inPoint, lift_normal, lift_offset, lift_offset_duration));
waypoints.push_back(
compute_offset(inPoint, lift_normal, lift_offset, lift_offset_duration));
// inserting all waypoints but last
waypoints.insert(waypoints.end(), wayPointsBegin + 1, wayPointsEnd - 1);
// inserting waypoint to start landing
const Waypoint& landWaypoint = compute_offset(endPoint, land_normal, land_offset, -land_offset_duration);
const Waypoint& landWaypoint =
compute_offset(endPoint, land_normal, land_offset, -land_offset_duration);
waypoints.push_back(landWaypoint);
// specifying end velocity constraint such that landing will be in straight line
// specifying end velocity constraint such that landing will be in straight
// line
spline_t end_spline =
make_end_spline(land_normal, landWaypoint.second, land_offset, landWaypoint.first, land_offset_duration);
spline_constraints_t constraints = compute_required_offset_velocity_acceleration(end_spline, land_offset_duration);
spline_deriv_constraint_t all_but_end(waypoints.begin(), waypoints.end(), constraints);
make_end_spline(land_normal, landWaypoint.second, land_offset,
landWaypoint.first, land_offset_duration);
spline_constraints_t constraints =
compute_required_offset_velocity_acceleration(end_spline,
land_offset_duration);
spline_deriv_constraint_t all_but_end(waypoints.begin(), waypoints.end(),
constraints);
t_spline_t splines = all_but_end.subSplines_;
splines.push_back(end_spline);
return new exact_cubic_t(splines);
......
......@@ -6,8 +6,8 @@
* \date 06/17/2013
*
* 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
* cubic splines fulfulling those 4 restrictions :
* Given a set of waypoints (x_i*) and timestep (t_i), it provides the unique
* 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+1) = x_i+1* ;
* - its derivative is continous at t_i+1
......@@ -19,10 +19,11 @@
#ifndef _CLASS_EFFECTOR_SPLINE_ROTATION
#define _CLASS_EFFECTOR_SPLINE_ROTATION
#include "spline/helpers/effector_spline.h"
#include "spline/curve_abc.h"
#include <Eigen/Geometry>
#include "spline/curve_abc.h"
#include "spline/helpers/effector_spline.h"
namespace spline {
namespace helpers {
......@@ -34,13 +35,15 @@ typedef curve_abc<Time, Numeric, 4, false, quat_t> curve_abc_quat_t;
typedef std::pair<Numeric, quat_t> waypoint_quat_t;
typedef std::vector<waypoint_quat_t> t_waypoint_quat_t;
typedef Eigen::Matrix<Numeric, 1, 1> point_one_dim_t;
typedef spline_deriv_constraint<Numeric, Numeric, 1, false, point_one_dim_t> spline_deriv_constraint_one_dim;
typedef spline_deriv_constraint<Numeric, Numeric, 1, false, point_one_dim_t>
spline_deriv_constraint_one_dim;
typedef std::pair<Numeric, point_one_dim_t> waypoint_one_dim_t;
typedef std::vector<waypoint_one_dim_t> t_waypoint_one_dim_t;
class rotation_spline : public curve_abc_quat_t {
public:
rotation_spline(quat_ref_const_t quat_from = quat_t(0, 0, 0, 1), quat_ref_const_t quat_to = quat_t(0, 0, 0, 1),
rotation_spline(quat_ref_const_t quat_from = quat_t(0, 0, 0, 1),
quat_ref_const_t quat_to = quat_t(0, 0, 0, 1),
const double min = 0., const double max = 1.)
: curve_abc_quat_t(),
quat_from_(quat_from.data()),
......@@ -71,7 +74,8 @@ class rotation_spline : public curve_abc_quat_t {
}
virtual quat_t derivate(time_t /*t*/, std::size_t /*order*/) const {
throw std::runtime_error("TODO quaternion spline does not implement derivate");
throw std::runtime_error(
"TODO quaternion spline does not implement derivate");
}
spline_deriv_constraint_one_dim computeWayPoints() const {
......@@ -93,7 +97,8 @@ class rotation_spline : public curve_abc_quat_t {