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.
// 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
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)$\\
\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 :\\\\
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 :\\
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$.
\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) :\\
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)
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]
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 :
