Commit c6ec4d76 authored by stevet's avatar stevet
Browse files

doc

parent 4b9a8aeb
File added
\documentclass{article}
\usepackage[utf8]{inputenc}
\title{DOCUMENTATION - Curves}
\author{Jason Chemin}
\date{April 2019}
\usepackage{makecell}
%\usepackage{amsmath}
\usepackage{mathtools}
\usepackage{listings}
\usepackage{xcolor}
\usepackage{graphicx}
\usepackage{float}
\usepackage{caption}
\lstset { %
language=C++,
backgroundcolor=\color{black!5}, % set backgroundcolor
basicstyle=\footnotesize,% basic font setting
}
\begin{document}
\newcommand{\commentt}[2]{\textcolor{#1}{\textbf{\textit{[#2]}}}} % make sure no comments left in the paper
%~ \newcommand{\commentt}[2]{} % using this line rather than the previous one you remove all comments
\newcommand{\nmnote}[1]{\commentt{red}{NM: #1}}
\newcommand{\stn}[1]{\commentt{blue}{ST: #1}}
\maketitle
\tableofcontents
\newpage
\section{Curves}
\subsection{Definition of different types of curves}
% Definitions here : https://math.stackexchange.com/questions/97233/what-is-the-difference-between-natural-cubic-spline-hermite-spline-b%C3%A9zier-spli
\noindent
\textbf{\textit{Piecewise curve}} : a curve that is formed from a collection of simple segments strung end-to-end so that their junctions are fairly smooth. There are many kinds of spline that use different mathematics functions, but most of splines consist of polynomial segments where we can add some constraints of continuity.\\
\noindent
\textbf{\textit{Bezier curve}} : Polynomial parametric curve defined by several control points. \textit{Properties} :
\begin{itemize}
\item Pass through the first and last control points.
\item Curve is contained in the convex hull of its defining control point and smoothly follows the control points.
\item The degree of polynomial defining the curve segment with $n$ control points is $n-1$.
\end{itemize}
\noindent
\textbf{\textit{Cubic spline}} : spline where each polynomial segments has degree 3.\\
\noindent
\textbf{\textit{Exact Cubic spline*}} : Cubic spline optimized as in the paper : "Task-Space Trajectories via Cubic Spline Optimization" By J. Zico Kolter and Andrew Y.ng (ICRA 2009). \textit{Properties} :
\begin{itemize}
\item Pass through each waypoint.
\item C2 continuous.
\end{itemize}
\noindent
\textbf{\textit{Cubic hermite spline}} : spline where each segment is a third degree polynomial and described by its extremum positions end derivatives.
\begin{itemize}
\item Pass through all positions given.
\item No convex hull guarantees.
\item C1 continuous.
\end{itemize}
\subsection{curve\_abc}
All curves in our implementation inherits from class curve\_abc and have to implement the following functions :\\
\begin{lstlisting}
// Evaluation of the cubic spline at time t.
virtual point_t operator()(const time_t t)
\end{lstlisting}
\begin{lstlisting}
// Evaluate the derivative of order N of curve at time t.
virtual point_t derivate(const time_t t, const std::size_t order)
\end{lstlisting}
\begin{lstlisting}
// Get dimension of curve.
virtual std::size_t dim()
\end{lstlisting}
\begin{lstlisting}
// Get the minimum time for which the curve is defined.
virtual time_t min()
\end{lstlisting}
\begin{lstlisting}
// Get the maximum time for which the curve is defined.
virtual time_t max()
\end{lstlisting}
To serialize a curve, curve\_abc inherits from boost::serialization::Serializable where serialization functions are defined in include/curves/serialization/archive.hpp, so it has to implement the function :
\begin{lstlisting}
template<class Archive>
void serialize(Archive& ar, const unsigned int version)
\end{lstlisting}
\subsection{Bezier curve}
A Bezier curve is a parametric curve defined by a set of control points ${p_0, p_1, ..., p_n}$ where $n$ is its order. The curve passes through its first and last control point.
All Bezier curves of order superior to 3 can be represented as Bezier curves of order 3 or less joined end-to-end, where the last control point of one curve coincides with the first control point of the next curve. \\\\
If you want to create a bezier using five or more control points, we recommend you instead to create a piecewise bezier curve containing bezier curves of four or less control points
(See section piecewise curve).
\subsubsection{Definition}
A bezier curve with $n+1$ is represented by :
\begin{equation}\label{eq:bezierBernstein}
x(t) = \sum_{i=0}^{N} B_i^N((t-t_{min})/T) \cdot p_i
\end{equation}
where
\begin{equation}\label{eq:bernstein}
B_i^n((t-t_{min})/T) = \binom{n}{i} \cdot ((t-t_{min})/T)^i \cdot (1-(t-t_{min})/T)^{n-i}
\end{equation}
are Bernstein polynomials, $p_i$ is the control point at index $i$, $t \in [t_{min},t_{max}]$ and $T=t_{max}-t_{min}$.\\
To add constraints on initial and final velocity on a Bezier curve x(t), we can set position of control points accordingly to its derivative $x'(t)$ and $x''(t)$. For more details, read subsection III.A of paper "C-CROC: Continuous and Convex Resolution of Centroidal dynamic trajectories for legged robots in multi-contact scenarios" by Pierre Fernbach et al. .
\begin{table}[H]
\begin{center}
\begin{tabular}{ |c|c|c|c| }
\hline
Order of Bezier curve $x(t)$ & Derivative $x'(t)$ & $x'(t=0)$ & $x'(t=1)$ \\
\hline
1 (Linear) & $(p_1-p_0)/T$ & $(p_1-p_0)/T$ & $(p_1-p_0)/T$ \\
\hline
2 (Quadratic) & \makecell{$(2(1-t)(p_1-p_0)+$\\$2t(p_2-p_1)))/T$} & $2(p_1-p_0)/T$ & $2(p_2-p_1)/T$ \\
\hline
3 (Cubic) & \makecell{$(3(1-t)^2(p_1-p_0)+$\\$6t(1-t)(p_2-p_1)+$\\$3t^2(p_3-p_2))/T$} & $3(p_1-p_0)/T$ & $3(p_3-p_2)/T$\\
\hline
\end{tabular}
\caption{Derivative order 1 of bezier curve defined between [0,1] at initial and final time.}
\label{table:bezier_derivative1}
\end{center}
\end{table}
\begin{table}[H]
\begin{center}
\begin{tabular}{ |c|c|c| }
\hline
Order of Bezier curve $x(t)$ & $x''(t=0)$ & $x''(t=1)$ \\
\hline
1 (Linear) & $(p_1-p_0)/T^2$ & $(p_1-p_0)/T^2$ \\
\hline
2 (Quadratic) & $2(p_1-p_0)/T^2$ & $2(p_2-p_1)/T^2$ \\
\hline
3 (Cubic) & $3(p_1-p_0)/T^2$ & $3(p_3-p_2)/T^2$ \\
\hline
\end{tabular}
\caption{Derivative order 2 of bezier curve defined between [0,1] at initial and final time.}
\label{table:bezier_derivative2}
\end{center}
\end{table}
Our method to ensure that velocity and acceleration constraints are respected on a bezier curve, consists in adding two control points $P_{c1}$, $P_{c2}$ after the first one $P_0$ and two, $Pc_{n-1}$ and $Pc_{n-2}$, before the last one $P_n$. The derivatives order 1 and 2 of a Bezier curve for $N$ control points are :\\\\
\begin{equation}
\begin{cases}
x'(t) = \frac{N}{T} \cdot \sum_{i=0}^{N-1} B_i^{N-1}(t) \cdot (P_{i+1}-P_i)\\
x''(t) = \frac{N\cdot (N-1)}{T^2} \cdot \sum_{i=0}^{N-2} B_i^{N-2}(t) \cdot (P_{i+2} - 2 P_{i+1} + P_i)
\end{cases}
\end{equation}
At initial and final time, $t=T_{min}$ and $t=T_{max}$ and $T=T_{max}-T_{min}$, the derivatives are equal to :\\
\begin{equation}
\begin{cases}
x'(0) = \frac{N}{T} \cdot (P_1-P_0)\\
x''(0) = \frac{N}{T^2} \cdot(N-1) \cdot (P_2 - 2 P_1 + P_0)\\
x'(1.0) = \frac{N}{T} \cdot (P_N-P_{N-1})\\
x''(1.0) = \frac{N}{T^2} \cdot (N-1) \cdot (P_N - 2 P_{N-1} + P_{N-2})
\end{cases}
\end{equation}
By adding $Pc_{1}$, $Pc_{2}$ right after the first control point $P_0$ and $Pc_{n-1}$, $Pc_{n-2}$ before the last control points $P_N$ to bezier curve. We can set them as followed to respect the constraints fixed on acceleration and velocities at initial and final time :\\
\begin{equation}
\begin{cases}
Pc_1 = P_0 + \dfrac{constraints.init_vel}{N-1} \cdot T\\
Pc_2 = 2 Pc_1 - P_0 + \dfrac{constraints.init_acc}{(N-1) \cdot (N-2)} \cdot T^2\\
Pc_{N-1} = P_N - \dfrac{constraints.end_vel}{N-1} \cdot T\\
Pc_{N-2} = 2 Pc_{N-1} - P_N + \dfrac{constraints.end_acc}{(N-1) \cdot (N-2)} \cdot T^2
\end{cases}
\end{equation}
Evaluation of the curve at time $t$ has been implemented in three different ways.
First, the evaluation of \textbf{Bernstein} which consists in solving the equation \eqref{eq:bezierBernstein}.
The second one is the evaluation \textbf{De Casteljau} which computes the $N-1$ centroids of parameters $(t,1-t)$ of consecutive $N$ control points of bezier curve, and perform it iteratively until getting one point in the list which will be the evaluation of bezier curve at time $t$.
\begin{center}
\includegraphics[width=\textwidth]{images/De_Casteljau_construction.png}
\captionof{figure}{Evaluation of Bezier curve using De Casteljau algorithm.}
\end{center}
Finally, the last one implemented is \textbf{Horner} evaluation which is the one used by default in our algorithm as it is faster than the two others. We apply the Horner's scheme in order to reduce complexity of calculation of this polynomial. Applying it transforms equation \eqref{eq:bezierBernstein} into $ x(t) = (1-t)^N(\sum_{i=0}^{N} \binom{N}{i} \frac{1-t}{t}^i P_i)$ reduces computation time.\\
In this implementation we use the Horner evaluation which is faster for low order bezier curve, but which is less stable with higher orders. Otherwise we should use De Casteljau which is more stable and better to use with high orders.\\
\subsubsection{Template and constructors}
The template for this class is composed of (See applications of bezier\_curve.h) :\\
- typename Time=double\\
- typename Numeric=Time\\
- bool Safe=false\\
- typename Point=Eigen::Matrix$<$Numeric, Eigen::Dynamic, $1>$\\
The type of point used by default is of dynamic-size. Size of control points given in the constructor will be of the same size as all points returned by bezier\_curve.
\noindent
The class bezier\_curve has the following constructors :
\begin{lstlisting}
bezier_curve() // default constructor
\end{lstlisting}
\begin{lstlisting}
bezier_curve(In PointsBegin, In PointsEnd,
const time_t T_min=0.0,
const time_t T_max=1.0,
const time_t mult_T=1.0)
\end{lstlisting}
\begin{lstlisting}
bezier_curve(In PointsBegin, In PointsEnd,
const curve_constraints_t& constraints
const time_t T_min=0.0,
const time_t T_max=1.0,
const time_t mult_T=1.0)
\end{lstlisting}
\begin{lstlisting}
bezier_curve(const bezier_curve& other)
\end{lstlisting}
\noindent
Where parameters of constructors are : \\\\
- \textbf{\textit{PointBegin}} and \textbf{\textit{PointsEnd}} : iterators pointing to the first and last element of a control point container.;\\ The actual implementation of bezier curve allows you to enter any number of control points as long as it is superior or equal to one. But in most of cases if the degree is superior to 3, it is prefered to create a composite Bezier curve with segments of degree inferior or equal to 3.\\\\
- \textbf{T} : Upper bound of time range which is between $[0;T]$ (default $[0;1]$). This value is then used in evaluation of the curve at time t. In these evaluation, we divide the time by $T$ to bring interval $[0;T]$ to $[0;1]$;\\\\
- \textbf{\textit{mult\_T}} : By default equals to $1.0$. Each time the curve the curve is derived, this value will be multiply by $1/(T_{max}-T_{min})$. The evaluation of curve at time t is multiplied by mult\_T. For more details, read subsection III.A of paper "C-CROC: Continuous and Convex Resolution of Centroidal dynamic trajectories for legged robots in multi-contact scenarios" by Pierre Fernbach and Steve Tonneau.\\\\
- \textbf{\textit{constraints}} : constraints applied on start and end point of the curve.\\
Structure curve\_constraint contains 4 instance variables to set : \textit{init\_vel},\textit{init\_acc}, \textit{end\_vel}, \textit{end\_acc} which correspond to initial and final velocities and accelerations.\\\\
- \textbf{\textit{other}} : the bezier curve to copy.
\subsubsection{Bezier in C++}
Here are some possible applications using the class bezier\_curve.h in C++ :\\
\begin{lstlisting}
#include "curves/bezier_curve.h"
using namespace curves;
\end{lstlisting}
\begin{lstlisting}
// Template for bezier curve
// Points in bezier can have any dimension
typedef Eigen::VectorXd pointX_t;
typedef double time_t;
typdef double num_t;
typedef bezier_curve <time_t, num_t,
true, pointX_t> bezier_curve_t;
// We will use points of dimension 3 and 4 for the example
typedef Eigen::Vector3d point3_t;
typedef Eigen::Vector4d point4_t;
// Creation of control points in 3D : P0, P1, P2, P3
point3_t P0(1,2,3);
point3_t P1(2,3,4);
point3_t P2(3,4,5);
point3_t P3(3,6,7);
// Creation of control points in 4D : P4_0, P4_1, P4_2
point4_t P4_0(1,2,3,4);
point4_t P4_1(2,3,4,5);
point4_t P4_2(3,4,5,6);
// Creation of the container of control points
std::vector<point3_t> cp;
std::vector<point4_t> cp4;
// Creation of bezier curve 4D defined between [0.,1.]
cp4.push_back(P4_0);
cp4.push_back(P4_1)
cp4.push_back(P4_2)
bezier_curve_t bc_4D(cp4.begin(),cp4.end());
// Create some 3D bezier curves defined between [0.,1.]
// Two control points = Degree 1 (Linear Bezier curve)
cp.push_back(P0);
cp.push_back(P1);
bezier_curve_t bc1(cp.begin(),cp.end());
// Three control points = Degree 2 (Quadratic Bezier curve)
cp.push_back(P2);
bezier_curve_t bc2(cp.begin(),cp.end());
// Four control points = Degree 3 (Cubic Bezier curve)
cp.push_back(P3);
bezier_curve_t bc3(cp.begin(),cp.end());
// Value of curve at t=0 and t=1
point3_t res = bc3(0.);
res = bc3(1.);
// Value of derivative curve at t=0.5
res = bc3.derivate(0.5,1); // derivative order 1
res = bc3.derivate(0.5,2); // derivative order 2
// Get the derivative or primitive curve at any time t
// (Recommended if derivative computed several times)
// derivative order 1
bezier_curve_t bc_der1 = bc3.compute_derivate(1);
res = bc_der1(0.5); // equivalent to bc3.derivate(0.5,1)
// primitive order 1
bezier_curve_t bc_prim1 = bc3.compute_primitive(1);
// Time reparameterization
time_t T_min = 1.;
time_t T_max = 3.; // Bezier curve defined between [1.0,3.0]
bezier_curve_t bc3_T_modif(cp.begin(),cp.end(), T_min, T_max);
res = bc3_T_modif(T_min); // value at t=T_min equals to bc3(0.)
res = bc3_T_modif(T_max); // value at t=T_max equals to bc3(1.)
// Add constraints on curve
bezier_curve_t::curve_constraints_t constraints;
constraints.init_vel = point3_t(-1.,-1.,-1.);
constraints.init_acc = point3_t(-2.,-3.,-2.);
constraints.end_vel = point3_t(1.,2.,1.);
constraints.end_acc = point3_t(3.,2.,3.);
bezier_curve_t bc4(beg,end,constraints,T_min,T_max);
bezier_curve_t bc4_der1 = bc4.compute_derivate(1);
bezier_curve_t bc4_der2 = bc4.compute_derivate(2);
res = bc4_der1(T_min); // equal to init_vel
res = bc4_der1(T_max); // equal to end_vel
res = bc4_der2(T_min); // equal to init_acc
res = bc4_der2(T_max); // equal to end_acc
// Upper and Lower bound of definition interval
T_min = bc3_T_modif.min(); // Equal to 1.0
T_max = bc3_T_modif.max(); // Equal to 3.0
// Get waypoints
bezier_curve_t::t_point_t waypoints = bc3.waypoints();
bezier_curve_t::point_t first_waypoint = bc3.waypointAtIndex(0);
\end{lstlisting}
\subsubsection{Bezier in Python}
We can use bezier\_curve.h through curves.so as followed :
\begin{lstlisting}
from curves import bezier, curve_constraints
\end{lstlisting}
\begin{lstlisting}
# Creation of bezier in 3D with two control points in [0,1]
P3_0 = [1., 2., 3.]
P3_1 = [4., 5., 6.]
waypoints = matrix([P3_0,P3_1]).transpose()
bc_3dim = bezier(waypoints)
res = bc_3dim(1.) # value at t=1., equal to P3_1
# Creation of bezier in 6D with two control points in [0,1]
P6_0 = [1., 2., 3., 7., 5., 5.]
P6_1 = [4., 5., 6., 4., 5., 6.]
waypoints6 = matrix([P6_0,P6_1]).transpose()
bc_6dof = bezier6(waypoints6)
res = bc_6dof(0.) # value at t=0., equal to P6_0
# Time reparameterization
T_min = 0.0
T_max = 2.0 # Time range will be [0.,2.]
bc_3dim_T_modified = bezier3(waypoints3, T_min, T_max)
res = bc_3dof_T_modified(T_min) # at t=T_min equals to P3_0
res = bc_3dof_T_modified(T_max) # at t=T_max equals to P3_1
# Add constraints on curve
c = curve_constraints()
c.init_vel = matrix([0., 1., 1.]).transpose()
c.end_vel = matrix([0., 1., 1.]).transpose()
c.init_acc = matrix([0., 1., -1.]).transpose()
c.end_acc = matrix([0., 10., 1.]).transpose()
bc_3dim_constraints = bezier3(waypoints, c, T_min, T_max)
# Derivative at time t
res = bc_3dim_constraints.derivate(T_max,1) # Equals to end_vel
res = bc_3dim_constraints.derivate(T_max,2) # Equals to end_acc
# Derivative & Primitive curve
bc_derived = bc_3dim_constraints.compute_derivate(1)
bc_primitive = bc_3dim_constraints.compute_primitive(1)
# Upper and Lower bound of definition interval
Tmin = bc_3dim_T_modified.min() # Equal to T_min
Tmax = bc_3dim_T_modified.max() # Equal to T_max
# Get waypoints
nb_waypoints = bc_3dim_T_modified.nbWaypoints
waypoint_first_index = bc_3dim.waypointAtIndex(0)
waypoint_last_index = bc_3dim.waypointAtIndex(nb_waypoints-1)
\end{lstlisting}
\subsection{Polynomial}
Represents a polynomial of an arbitrary order defined on the interval $[t_{min}, t_{max}]$. This polynomial follows the equation :
\begin{equation}\label{eq:polynomial}
x(t) = a + b\cdot(t - t_{min}) + ... + d\cdot(t - t_{min})^N
\end{equation}
where $N$ is the order and $t \in [t_{min}, t_{max}]$.\\
\subsubsection{Template and constructors}
The template for this class is composed of (See applications of polynomial.h) :\\
typename Time=double\\
typename Numeric=Time\\
bool Safe=false\\
typename Point=Eigen::Matrix$<$Numeric, Dim, $1>$\\
typename T\_Point=std::vector$<$Point,Eigen::aligned\_allocator$<$Point$> >$\\
The type of point used by default is of dynamic-size. Dimension of points given in the constructor as coefficients will be the same as all points returned by polynomial.
The class polynomial has the following constructors :
\begin{lstlisting}
polynomial() // Default constructor
\end{lstlisting}
\begin{lstlisting}
polynomial(const coeff_t& coefficients,
const time_t min, const time_t max)
\end{lstlisting}
\begin{lstlisting}
polynomial(const T_Point& coefficients,
const time_t min, const time_t max)
\end{lstlisting}
\begin{lstlisting}
template<typename In>
polynomial(In zeroOrderCoefficient, In out,
const time_t min, const time_t max)
\end{lstlisting}
\begin{lstlisting}
polynomial(const Point& init,const Point& d_init,
const Point& end, const Point& d_end,
const time_t min, const time_t max)
\end{lstlisting}
\begin{lstlisting}
polynomial(const Point& init,const Point& d_init,
const Point& dd_init,
const Point& end, const Point& d_end,
const Point& dd_end,
const time_t min, const time_t max )
\end{lstlisting}
\begin{lstlisting}
polynomial(const polynomial& other)
\end{lstlisting}
Parameters of constructors are :\\\\
- \textbf{\textit{coefficients}} : coefficients of polynomial represented in 2 different ways. In the first constructor, \textit{coeff\_t} is a reference to an Eigen Matrix where each column is a coefficient, from the zero order coefficient, up to the highest order. Polynomial order is given by the number of columns minus one. In the second constructor, \textit{T\_Point}\& is a container with all polynomial coefficients, starting with the zero coefficient, up to the highest order. Polynomial order is given by the size of a coefficient minus one.\\\\
- \textbf{\textit{min}} : lower bound on interval definition of the curve.\\\\
- \textbf{\textit{max}} : upper bound on interval definition of the curve.\\\\
- \textbf{\textit{zeroOrderCoefficient}} : iterator pointing to the first element of a structure containing the coefficients. It corresponds to the zero degree coefficient.\\\\
- \textbf{\textit{out}} : iterator pointing to the last element of a structure containing the coefficients.\\\\
- \textbf{\textit{init / end}} : initial and ending position of the curve.\\\\
- \textbf{\textit{d\_init / d\_end}} : initial and ending velocity of the curve.\\\\
- \textbf{\textit{dd\_init / dd\_end}} : initial and ending acceleration of the curve.\\\\
- \textbf{\textit{other}} : polynomial to copy.\\\\
\subsubsection{Polynomial in C++}
Here is an example of application using the class polynomial.h :
\begin{lstlisting}
#include "curves/polynomial.h"
using namespace curves;
\end{lstlisting}
\begin{lstlisting}
// Template
// We can create polynomial of any dimension
typedef Eigen::VectorXd pointX_t;
typedef std::vector<pointX_t,
Eigen::aligned_allocator<pointX_t>
> t_pointX_t;
typedef double time_t;
typdef double num_t;
typedef polynomial <time_t, num_t, true,
pointX_t, t_pointX_t> polynomial_t;
// Example with vector of dimension 3
typedef Eigen::Vector3d point_t;
// Creation of polynomial coefficients
point_t a(1,2,3);
point_t b(2,3,4);
point_t c(3,4,5);
polynomial_t::num_t T_min = 0.0;
polynomial_t::num_t T_max = 2.0;
// Polynomial order 1 : P(t) = a+b*t in [T_min,T_max]
std::vector<pointX_t> coefs;
coefs.push_back(a);
coefs.push_back(b);
polynomial_t pol1(coefs.begin(),coefs.end(),T_min,T_max);
// value of polynomial at time t=T_min and t=T_max
point_t res = pol1(T_min); // Equal to a
res = pol1(T_max); // Equal to a+b
// Polynomial order 2 : P(t) = a+b*t+c*t^2 in [T_min,T_max]
coefs.push_back(c);
polynomial_t pol2(coefs.begin(),coefs.end(),T_min,T_max);
// value of polynomial at time t=T_min and t=T_max
res = pol2(T_min); // Equal to a
res = pol2(T_max); // Equal to a+b+c
// Derivative order one at time t=0.5 in [T_min,T_max]
res = pol2.derivate(0.5, 1);
// compute derivate
polynomial_t pol_der1 = pol2.compute_derivate(1);
res = pol_der1(0.5) // Equal to pol2.derivate(0.5, 1)
// Upper and Lower bound of definition interval
polynomial_t::num_t T_min_check = pol2.min(); // Equal to T_min
polynomial_t::num_t T_max_check = pol2.max(); // Equal to T_max
// get coefficients
Eigen::MatrixXd coeffs = pol2.coeff();
polynomial_t::point_t coeff_order0 = pol2.coeffAtDegree(0);
// Creation of polynomial from discrete points
point_t p0(0.,1.,0.);
point_t p1(1.,2.,-3.);
point_t dp0(-8.,4.,6.);
point_t ddp0(-1.,7.,4.);
point_t dp1(-9.,5.,7.);
point_t ddp1(0.,8.,3.);
// Constraint on initial and final pos
polynomial_t pol_a = polynomial_t(p0,p1,
T_min,T_max);
// Constraint on initial and final pos/vel
polynomial_t pol_b = polynomial_t(p0,dp0,
p1,dp1,
T_min,T_max);
// Constraint on initial and final :pos/vel/acc
polynomial_t pol_c = polynomial_t(p0,dp0,ddp0,
p1,dp1,ddp1,
T_min,T_max);
\end{lstlisting}
\subsubsection{Polynomial in python}
We can use polynomial.h through curves.so as followed :
\begin{lstlisting}
from curves import polynomial
\end{lstlisting}
\begin{lstlisting}
# Creation of polynomial order 2
P0 = [1.,2.,3.]
P1 = [4.,5.,6.]
P2 = [7.,8.,9.]
T_min = 1.0
T_max = 3.0
coeffs = matrix([P0, P1, P2]).transpose()
pol = polynomial(coeffs) # defined in [0.,1.]
pol_time = polynomial(coeffs,T_min,T_max) # defined in [1.,3.]
# Value at time t=0.5
res = pol2(0.5)
# Derivative order 1 at time t=0.5
res = pol.derivate(0.5, 1)
# Compute derivate order 1
pol_der1 = pol2.compute_derivate(1)
res = pol_der1(0.5) # Equal to pol.derivate(0.5, 1)
# Upper and Lower bound of definition interval
T_min_check = pol_time.min() # Equal to T_min
T_max_check = pol_time.max() # Equal to T_max
# Get coefficients
res_coeff = pol.coeff()
res_coeff_order0 = pol.coeffAtDegree(0)
# Create polynomial from discrete points
p0 = matrix([1.,3.,-2.]).T
p1 = matrix([0.6,2.,2.5]).T
dp0 = matrix([-6.,2.,-1.]).T
dp1 = matrix([10.,10.,10.]).T
ddp0 = matrix([1.,-7.,4.5]).T
ddp1 = matrix([6.,-1.,-4]).T
# Polynomial C0
pol_C0 = polynomial(p0,p1,min,max)
# Polynomial C1
polC1 = polynomial(p0,dp0,p1,dp1,min,max)
# Polynomial C2
polC2 = polynomial(p0,dp0,ddp0,p1,dp1,ddp1,min,max)
\end{lstlisting}
\subsection{Cubic Hermite spline}
A cubic hermite spline is a polynomial curve of degree 3, defined by its initial and final positions and velocities : $P_i$, $P_{i+1}$ and $m_i$, $m_{i+1}$.\\
\subsubsection{Definition}
A hermite cubic spline has some of the following properties :
\begin{itemize}
\item cross each of the waypoints given in its initialization ($P_0$, $P_1$,...,$P_N$).
\item its derivatives $p'(t_{P_i}) = m_i$ and $p'(t_{P_{i+1}}) = m_{i+1}$.
\end{itemize}
A cubic hermite spline is defined by the polynom :
\begin{equation}\label{eq:hermiteSpline}
p(t)=h_{00}(t)\cdot P_0 + h_{10}(t) \cdot m_0 + h_{01}(t) \cdot P_1 + h_{11}(t) \cdot m_1
\end{equation}
where $h_{ij}$ are Hermite basis functions :
\begin{itemize}
\item $h_{00} = 2 \cdot t^3 - 3 \cdot t^2 + 1.0$
\item $h_{10} = t^3 - 2 \cdot t^2 + t$
\item $h_{01} = -2 \cdot t^3 + 3 \cdot t^2$
\item $h_{11} = t^3 - t^2$
\end{itemize}
If you want to create a cubic Hermite spline containing more than two waypoints,
we recommend you instead to create a piecewise cubic Hermite curve (See section piecewise curve).
\subsubsection{Template and constructors}
Template for this class is composed of (See applications of cubic\_hermite\_spline):
- typename Time=double\\
- typename Numeric=Time\\
- bool Safe=false\\
- typename Point=Eigen::Matrix$<$Numeric, Eigen::Dynamic, $1>$\\
\noindent
The class cubic\_hermite\_spline has the following constructors :
\begin{lstlisting}
cubic_hermite_spline() // Default constructor
\end{lstlisting}
\begin{lstlisting}
cubic_hermite_spline(In PairsBegin, In PairsEnd,
const Vector_time& time_control_points)
\end{lstlisting}
\begin{lstlisting}
cubic_hermite_spline(const cubic_hermite_spline& other)
\end{lstlisting}
\noindent
Where parameters of constructors are :\\\\
- \textbf{\textit{PairsBegin}} : iterator pointing to the first element of a pair(position, derivative) container.\\\\
- \textbf{\textit{PairsEnd}} : iterator pointing to the last element of a pair(position, derivative) container.\\\\
\textbf{\textit{time\_control\_points}} : container with time corresponding to each waypoint. To evaluate spline at time t, we find the interval $[T