Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Stack Of Tasks
parametric-curves
Commits
34ed9075
Commit
34ed9075
authored
Jan 05, 2018
by
Olivier Stasse
Browse files
Finishing removing parametriccurves directory.
parent
5abd4484
Changes
21
Hide whitespace changes
Inline
Side-by-side
include/parametriccurves/MathDefs.h
deleted
100644 → 0
View file @
5abd4484
/**
* \file Math.h
* \brief Linear algebra and other maths definitions. Based on Eigen 3 or more
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*
* This file contains math definitions used
* used throughout the library.
* Preprocessors definition are used to use either float
* or double values, and 3 dimensional vectors for
* the Point structure.
*/
#ifndef _SPLINEMATH
#define _SPLINEMATH
#include
<Eigen/Dense>
#include
<Eigen/SVD>
#include
<vector>
#include
<utility>
namespace
parametriccurves
{
//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
);
_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
());
for
(
long
i
=
0
;
i
<
m_sigma
.
rows
();
++
i
)
{
if
(
m_sigma
(
i
)
>
pinvtoler
)
m_sigma_inv
(
i
,
i
)
=
1.0
/
m_sigma
(
i
);
}
pinvmat
=
(
svd
.
matrixV
()
*
m_sigma_inv
*
svd
.
matrixU
().
transpose
());
}
}
// namespace spline
#endif //_SPLINEMATH
include/parametriccurves/abstract-curve.hpp
deleted
100644 → 0
View file @
5abd4484
/**
* \file AbstractCurve.hpp
* \brief interface for a Curve of arbitrary dimension.
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*
* Interface for a curve
*/
#include
<cstddef>
#ifndef _parameteric_curves_abstract_curve_hpp
#define _parameteric_curves_abstract_curve_hpp
namespace
parametriccurves
{
/// \struct AbstractCurve
/// \brief Represents a curve of dimension Dim
/// is Safe is false, no verification is made on the evaluation of the curve.
template
<
typename
Numeric
,
typename
Point
>
struct
AbstractCurve
{
typedef
Point
point_t
;
typedef
Numeric
time_t
;
typedef
Numeric
num_t
;
public:
/* Constructors - destructors */
AbstractCurve
(
time_t
t_min_
,
time_t
t_max_
)
:
t_min
(
t_min_
),
t_max
(
t_max_
)
{
}
AbstractCurve
(){
}
virtual
~
AbstractCurve
(){}
public:
/// \brief Evaluation of the cubic spline at time t.
/// \param t : the time when to evaluate the spine
/// \param return : the value x(t)
virtual
const
point_t
operator
()(
const
time_t
&
t
)
const
=
0
;
/// \brief Evaluation of the derivative spline at time t.
/// \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
;
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
);
}
/* Setters */
virtual
bool
setInitialPoint
(
const
point_t
&
/*x_init*/
)
=
0
;
virtual
bool
setInitialPoint
(
const
num_t
&
/*x_init*/
)
=
0
;
virtual
bool
setTimePeriod
(
const
time_t
&
traj_time_
)
{
t_min
=
0.0
;
t_max
=
traj_time_
;
return
true
;
}
protected:
time_t
t_min
;
time_t
t_max
;
};
}
#endif //_STRUCT_CURVE_ABC
include/parametriccurves/bernstein.h
deleted
100644 → 0
View file @
5abd4484
/**
* \file bezier_curve.h
* \brief class allowing to create a Bezier curve of dimension 1 <= n <= 3.
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*/
#ifndef _CLASS_BERNSTEIN
#define _CLASS_BERNSTEIN
#include
"curve_abc.h"
#include
"MathDefs.h"
#include
<math.h>
#include
<vector>
#include
<stdexcept>
namespace
spline
{
///
/// \brief Computes factorial of a number
///
unsigned
int
fact
(
const
unsigned
int
n
)
{
assert
(
n
>=
0
);
int
res
=
1
;
for
(
int
i
=
2
;
i
<=
n
;
++
i
)
res
*=
i
;
return
res
;
}
///
/// \brief Computes a binomal coefficient
///
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
(){}
Numeric
operator
()(
const
Numeric
u
)
const
{
assert
(
u
>=
0.
&&
u
<=
1.
);
return
bin_m_i_
*
(
pow
(
u
,
i_
))
*
pow
((
1
-
u
),
m_minus_i
);
}
Numeric
m_minus_i
;
Numeric
i_
;
Numeric
bin_m_i_
;
};
///
/// \brief Computes all Bernstein polynomes for a certain degree
///
template
<
typename
Numeric
>
std
::
vector
<
Bern
<
Numeric
>
>
makeBernstein
(
const
unsigned
int
n
)
{
std
::
vector
<
Bern
<
Numeric
>
>
res
;
for
(
unsigned
int
i
=
0
;
i
<=
n
;
++
i
)
res
.
push_back
(
Bern
<
Numeric
>
(
n
,
i
));
return
res
;
}
}
// namespace spline
#endif //_CLASS_BERNSTEIN
include/parametriccurves/bezier_curve.h
deleted
100644 → 0
View file @
5abd4484
/**
* \file bezier_curve.h
* \brief class allowing to create a Bezier curve of dimension 1 <= n <= 3.
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*/
#ifndef _CLASS_BEZIERCURVE
#define _CLASS_BEZIERCURVE
#include
"curve_abc.h"
#include
"bernstein.h"
#include
"curve_constraint.h"
#include
"MathDefs.h"
#include
<vector>
#include
<stdexcept>
#include
<iostream>
namespace
spline
{
/// \class BezierCurve
/// \brief Represents a Bezier curve of arbitrary dimension and order.
/// 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
,
std
::
size_t
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
;
typedef
Numeric
num_t
;
typedef
curve_constraints
<
point_t
>
curve_constraints_t
;
typedef
std
::
vector
<
point_t
,
Eigen
::
aligned_allocator
<
point_t
>
>
t_point_t
;
typedef
typename
t_point_t
::
const_iterator
cit_point_t
;
typedef
bezier_curve
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
>
bezier_curve_t
;
/* Constructors - destructors */
public:
///\brief Constructor
///\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
)
:
minBound_
(
minBound
)
,
maxBound_
(
maxBound
)
,
size_
(
std
::
distance
(
PointsBegin
,
PointsEnd
))
,
degree_
(
size_
-
1
)
,
bernstein_
(
spline
::
makeBernstein
<
num_t
>
(
degree_
))
{
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
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
///\param PointsBegin, PointsEnd : the points parametering the Bezier curve
///\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
)
:
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
);
}
///\brief Destructor
~
bezier_curve
()
{
// NOTHING
}
private:
// bezier_curve(const bezier_curve&);
// bezier_curve& operator=(const bezier_curve&);
/* Constructors - destructors */
/*Operations*/
public:
/// \brief Evaluation of the cubic spline at time t.
/// \param t : the time when to evaluate the spine
/// \param return : the value x(t)
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
}
else
{
num_t
dt
=
(
1
-
nT
);
switch
(
size_
)
{
case
1
:
return
pts_
[
0
];
case
2
:
return
pts_
[
0
]
*
dt
+
nT
*
pts_
[
1
];
break
;
case
3
:
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
;
default
:
return
evalHorner
(
nT
);
break
;
}
}
}
/// \brief Computes the derivative curve at order N.
/// \param order : order of the derivative
/// \param return : the value x(t)
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
)
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_
);
return
deriv
.
compute_derivate
(
order
-
1
);
}
/// \brief Computes the primitive of the curve at order N.
/// \param constant : value of the primitive at t = 0
/// \param return : the curve x_1(t) such that d/dt(x_1(t)) = x_1(t)
bezier_curve_t
compute_primitive
(
const
std
::
size_t
order
)
const
{
if
(
order
==
0
)
return
*
this
;
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
n_wp
.
push_back
(
current_sum
);
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
);
}
bezier_curve_t
integ
(
n_wp
.
begin
(),
n_wp
.
end
(),
minBound_
,
maxBound_
);
return
integ
.
compute_primitive
(
order
-
1
);
}
/// \brief Evaluates the derivative at order N of the curve.
/// If the derivative is to be evaluated several times, it is
/// rather recommended to compute the derivative curve using compute_derivate
/// \param order : order of the derivative
/// \param t : the time when to evaluate the spine
/// \param return : the value x(t)
virtual
point_t
derivate
(
const
time_t
t
,
const
std
::
size_t
order
)
const
{
bezier_curve_t
deriv
=
compute_derivate
(
order
);
return
deriv
(
t
);
}
///
/// \brief Evaluates all Bernstein polynomes for a certain degree
///
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
)
res
+=
cit
->
operator
()(
u
)
*
(
*
pts_it
);
return
res
;
}
///
/// \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
();
Numeric
u
,
bc
,
tn
;
u
=
1.0
-
t
;
bc
=
1
;
tn
=
1
;
point_t
tmp
=
(
*
pts_it
)
*
u
;
++
pts_it
;
for
(
int
i
=
1
;
i
<
degree_
;
i
++
,
++
pts_it
)
{
tn
=
tn
*
t
;
bc
=
bc
*
(
degree_
-
i
+
1
)
/
i
;
tmp
=
(
tmp
+
tn
*
bc
*
(
*
pts_it
))
*
u
;
}
return
(
tmp
+
tn
*
t
*
(
*
pts_it
));
}
const
t_point_t
&
waypoints
()
const
{
return
pts_
;}
private:
template
<
typename
In
>
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
;
PN
=
*
(
PointsEnd
-
1
);
P1
=
P0
+
constraints
.
init_vel
/
degree_
;
P_n_1
=
PN
-
constraints
.
end_vel
/
degree_
;
P2
=
constraints
.
init_acc
/
(
degree_
*
(
degree_
-
1
))
+
2
*
P1
-
P0
;
P_n_2
=
constraints
.
end_acc
/
(
degree_
*
(
degree_
-
1
))
+
2
*
P_n_1
-
PN
;
res
.
push_back
(
P0
);
res
.
push_back
(
P1
);
res
.
push_back
(
P2
);
for
(
In
it
=
PointsBegin
+
1
;
it
!=
PointsEnd
-
1
;
++
it
)
res
.
push_back
(
*
it
);
res
.
push_back
(
P_n_2
);
res
.
push_back
(
P_n_1
);
res
.
push_back
(
PN
);
return
res
;
}
/*Operations*/
/*Helpers*/
public:
virtual
time_t
min
()
const
{
return
minBound_
;}
virtual
time_t
max
()
const
{
return
maxBound_
;}
/*Helpers*/
public:
const
time_t
minBound_
,
maxBound_
;
const
std
::
size_t
size_
;
const
std
::
size_t
degree_
;
const
std
::
vector
<
Bern
<
Numeric
>
>
bernstein_
;
private:
t_point_t
pts_
;
//storing bernstein polynoms, even in low dimension
};
}
#endif //_CLASS_BEZIERCURVE
include/parametriccurves/constant.hpp
deleted
100644 → 0
View file @
5abd4484
/**
* \file sinusoid.hpp
* \brief Generates InfiniteSinusoidal trajectory
* \author Rohan Budhiraja
* \date 2017
*
*/
#ifndef _parameteric_curves_constant_hpp
#define _parameteric_curves_constant_hpp
#include
<parametriccurves/abstract-curve.hpp>
namespace
parametriccurves
{
/// \class InfiniteSinusoid
/// \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
,
std
::
size_t
Dim
=
1
,
typename
Point
=
Eigen
::
Matrix
<
Numeric
,
Dim
,
1
>
>
struct
Constant
:
public
AbstractCurve
<
Numeric
,
Point
>
{
typedef
Point
point_t
;
typedef
Numeric
time_t
;
typedef
Numeric
num_t
;
typedef
AbstractCurve
<
Numeric
,
Point
>
curve_abc_t
;
public:
///\brief Constructor
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
~
Constant
(){}
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
bool
setInitialPoint
(
const
point_t
&
x_init_
)
{
if
(
x_init
.
size
()
!=
x_init_
.
size
())
return
false
;
x_init
=
x_init_
;
}
virtual
bool
setInitialPoint
(
const
double
&
x_init_
)
{
if
(
Dim
!=
1
)
return
false
;
x_init
[
0
]
=
x_init_
;
return
true
;
}
protected:
/*Attributes*/
point_t
x_init
;
};
}
#endif //_CLASS_EXACTCUBIC
include/parametriccurves/curve-constraint.hpp
deleted
100644 → 0
View file @
5abd4484
/**
* \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
*
*/
#ifndef _CLASS_CURVE_CONSTRAINT
#define _CLASS_CURVE_CONSTRAINT
#include
<parametriccurves/MathDefs.h>
#include
<functional>
#include
<vector>
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
(){}
point_t
init_vel
;
point_t
init_acc
;
point_t
end_vel
;
point_t
end_acc
;
};
}
#endif //_CLASS_CUBICZEROVELACC
include/parametriccurves/helpers/effector_spline.h
deleted
100644 → 0
View file @
5abd4484
/**
* \file exact_cubic.h
* \brief class allowing to create an Exact cubic spline.
* \author Steve T.
* \version 0.1
* \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 :
* - 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
* - its acceleration is continous at t_i+1
* more details in paper "Task-Space Trajectories via Cubic Spline Optimization"
* By J. Zico Kolter and Andrew Y.ng (ICRA 2009)
*/
#ifndef _CLASS_EFFECTORSPLINE
#define _CLASS_EFFECTORSPLINE
#include
"spline/spline_deriv_constraint.h"
namespace
spline
{
namespace
helpers
{