diff --git a/.gitignore b/.gitignore
index 8339c71a25aa5d350133714045b58fe709b39702..8d3185aa12bbe04637c1d4a4c172a0e3607b9992 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,6 +1,4 @@
# Build and Release Folders
-bin/
-lib/
build/
build-rel/
diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
new file mode 100644
index 0000000000000000000000000000000000000000..5907879af588e7e7df003a06352c5fb96c6346cd
--- /dev/null
+++ b/.gitlab-ci.yml
@@ -0,0 +1 @@
+include: http://rainboard.laas.fr/project/hpp-spline/.gitlab-ci.yml
diff --git a/CMakeLists.txt b/CMakeLists.txt
index f8a28be561ca5c5834c2eb248fc69c7535cbebb3..c90b8af32fbfb6629f6439b2945b8ebb1c0d3f5d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,17 +1,15 @@
cmake_minimum_required(VERSION 2.6)
project(spline)
INCLUDE(cmake/base.cmake)
+INCLUDE(cmake/test.cmake)
INCLUDE(cmake/python.cmake)
+INCLUDE(cmake/hpp.cmake)
-SET(PROJECT_NAME spline)
+SET(PROJECT_NAME hpp-spline)
SET(PROJECT_DESCRIPTION
- "template based classes for creating and manipulating spline and bezier curves. Comes with extra options specific to end-effector trajectories in robotics."
- )
-SET(PROJECT_URL "")
+ "template based classes for creating and manipulating spline and bezier curves. Comes with extra options specific to end-effector trajectories in robotics."
+ )
-set(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/build/")
-set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${PROJECT_SOURCE_DIR}/bin/")
-set(LIBRARY_OUTPUT_PATH "${PROJECT_SOURCE_DIR}/lib/")
# Disable -Werror on Unix for now.
SET(CXX_DISABLE_WERROR True)
SET(CMAKE_VERBOSE_MAKEFILE True)
@@ -19,43 +17,23 @@ SET(CMAKE_VERBOSE_MAKEFILE True)
find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIR})
-SETUP_PROJECT()
+SETUP_HPP_PROJECT()
-OPTION (BUILD_PYTHON_INTERFACE "Build the python binding" OFF)
+OPTION (BUILD_PYTHON_INTERFACE "Build the python binding" ON)
IF(BUILD_PYTHON_INTERFACE)
-# search for python
- FINDPYTHON(2.7 REQUIRED)
- find_package( PythonLibs 2.7 REQUIRED )
- include_directories( ${PYTHON_INCLUDE_DIRS} )
+ # search for python
+ FINDPYTHON(2.7 REQUIRED)
+ find_package( PythonLibs 2.7 REQUIRED )
+ include_directories( ${PYTHON_INCLUDE_DIRS} )
- find_package( Boost COMPONENTS python REQUIRED )
- include_directories( ${Boost_INCLUDE_DIR} )
-
- add_subdirectory (python)
+ find_package( Boost COMPONENTS python REQUIRED )
+ include_directories( ${Boost_INCLUDE_DIR} )
+ add_subdirectory (python)
ENDIF(BUILD_PYTHON_INTERFACE)
-add_subdirectory (src/tests/spline_test)
-
-install(FILES
- ${CMAKE_SOURCE_DIR}/include/spline/bernstein.h
- ${CMAKE_SOURCE_DIR}/include/spline/bezier_polynom_conversion.h
- ${CMAKE_SOURCE_DIR}/include/spline/curve_abc.h
- ${CMAKE_SOURCE_DIR}/include/spline/exact_cubic.h
- ${CMAKE_SOURCE_DIR}/include/spline/MathDefs.h
- ${CMAKE_SOURCE_DIR}/include/spline/polynom.h
- ${CMAKE_SOURCE_DIR}/include/spline/spline_deriv_constraint.h
- ${CMAKE_SOURCE_DIR}/include/spline/bezier_curve.h
- ${CMAKE_SOURCE_DIR}/include/spline/cubic_spline.h
- ${CMAKE_SOURCE_DIR}/include/spline/curve_constraint.h
- ${CMAKE_SOURCE_DIR}/include/spline/quintic_spline.h
- DESTINATION ${CMAKE_INSTALL_PREFIX}/include/spline
- )
-
-install(FILES
- ${CMAKE_SOURCE_DIR}/include/spline/helpers/effector_spline.h
- ${CMAKE_SOURCE_DIR}/include/spline/helpers/effector_spline_rotation.h
- DESTINATION ${CMAKE_INSTALL_PREFIX}/include/spline/helpers
- )
-
-SETUP_PROJECT_FINALIZE()
+
+ADD_SUBDIRECTORY(include/hpp/spline)
+ADD_SUBDIRECTORY(tests)
+
+SETUP_HPP_PROJECT_FINALIZE()
diff --git a/README.md b/README.md
index eb436d080f393993f146fbbc5d3c4df003608ac8..946b947543537dcea3bae3a20bf38b7ca1a878f1 100644
--- a/README.md
+++ b/README.md
@@ -1,6 +1,9 @@
Spline
===================
+[](https://gepgitlab.laas.fr/humanoid-path-planner/hpp-spline/commits/master)
+[](http://projects.laas.fr/gepetto/doc/humanoid-path-planner/hpp-spline/master/coverage/)
+
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.
@@ -12,7 +15,7 @@ To do so, tools are provided to:
The library is template-based, thus generic: the curves can be of any dimension, and can be implemented in double, float ...
-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
----------
@@ -59,6 +62,9 @@ Please refer to the Main.cpp files to see all the unit tests and possibilities o
Installation
-------------
+
+This package is available as binary in [robotpkg/wip](http://robotpkg.openrobots.org/robotpkg-wip.html)
+
## Dependencies
* [Eigen (version >= 3.2.2)](http://eigen.tuxfamily.org/index.php?title=Main_Page)
@@ -75,14 +81,14 @@ The library is header only, so the build only serves to build the tests and pyth
```
cd $SPLINE_DIR && mkdir build && cd build
- cmake .. && make
+ cmake .. && make
../bin/tests
```
If everything went fine you should obtain the following output:
```
-performing tests...
-no errors found
+performing tests...
+no errors found
```
### Optional: Python bindings installation
To install the Python bindings, in the CMakeLists.txt file, first enable the BUILD_PYTHON_INTERFACE option:
diff --git a/cmake b/cmake
index 7b0b47cae2b082521ad674c8ee575f594f483cd7..cea261e3da7d383844530070356bca76d20197a8 160000
--- a/cmake
+++ b/cmake
@@ -1 +1 @@
-Subproject commit 7b0b47cae2b082521ad674c8ee575f594f483cd7
+Subproject commit cea261e3da7d383844530070356bca76d20197a8
diff --git a/include/hpp/spline/CMakeLists.txt b/include/hpp/spline/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..b417e3d2cbfec4e0f3955998bd44a9d3fb09e0dc
--- /dev/null
+++ b/include/hpp/spline/CMakeLists.txt
@@ -0,0 +1,20 @@
+SET(${PROJECT_NAME}_HEADERS
+ bernstein.h
+ bezier_polynom_conversion.h
+ curve_abc.h
+ exact_cubic.h
+ MathDefs.h
+ polynom.h
+ spline_deriv_constraint.h
+ bezier_curve.h
+ cubic_spline.h
+ curve_constraint.h
+ quintic_spline.h
+ )
+
+INSTALL(FILES
+ ${${PROJECT_NAME}_HEADERS}
+ DESTINATION include/hpp/spline
+ )
+
+ADD_SUBDIRECTORY(helpers)
diff --git a/include/spline/MathDefs.h b/include/hpp/spline/MathDefs.h
similarity index 100%
rename from include/spline/MathDefs.h
rename to include/hpp/spline/MathDefs.h
diff --git a/include/spline/bernstein.h b/include/hpp/spline/bernstein.h
similarity index 100%
rename from include/spline/bernstein.h
rename to include/hpp/spline/bernstein.h
diff --git a/include/spline/bezier_curve.h b/include/hpp/spline/bezier_curve.h
similarity index 84%
rename from include/spline/bezier_curve.h
rename to include/hpp/spline/bezier_curve.h
index 74e23ed148f8065815b22210fc451c47b6bb322e..c1e0e4b9460029c16cff57f5ecf3b1901d773ba2 100644
--- a/include/spline/bezier_curve.h
+++ b/include/hpp/spline/bezier_curve.h
@@ -139,37 +139,10 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \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 / T_;
- 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 mult_T_ * pts_[0];
- case 2 :
- return mult_T_ * (pts_[0] * dt + nT * pts_[1]);
- break;
- case 3 :
- return mult_T_ * (pts_[0] * dt * dt
- + 2 * pts_[1] * nT * dt
- + pts_[2] * nT * nT);
- break;
- case 4 :
- return mult_T_ * (pts_[0] * dt * dt * dt
- + 3 * pts_[1] * nT * dt * dt
- + 3 * pts_[2] * nT * nT * dt
- + pts_[3] * nT * nT *nT);
- default :
- return mult_T_ * evalHorner(nT);
- break;
- }
- }
+ if(Safe &! (0 <= t && t <= T_))
+ throw std::out_of_range("can't evaluate bezier curve, out of range"); // TODO
+ return evalHorner(t);
}
/// \brief Computes the derivative curve at order N.
@@ -225,14 +198,15 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// Warning: the horner scheme is about 100 times faster than this method.
/// This method will probably be removed in the future
///
- point_t evalBernstein(const Numeric u) const
+ point_t evalBernstein(const Numeric t) const
{
+ const Numeric u = t/T_;
point_t res = point_t::Zero(Dim);
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;
+ return res*mult_T_;
}
@@ -241,19 +215,20 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
///
point_t evalHorner(const Numeric t) const
{
+ const Numeric u = t/T_;
typename t_point_t::const_iterator pts_it = pts_.begin();
- Numeric u, bc, tn;
- u = 1.0 - t;
+ Numeric u_op, bc, tn;
+ u_op = 1.0 - u;
bc = 1;
tn = 1;
- point_t tmp =(*pts_it)*u; ++pts_it;
+ point_t tmp =(*pts_it)*u_op; ++pts_it;
for(unsigned int i=1; i<degree_; i++, ++pts_it)
{
- tn = tn*t;
+ tn = tn*u;
bc = bc*((num_t)(degree_-i+1))/i;
- tmp = (tmp + tn*bc*(*pts_it))*u;
+ tmp = (tmp + tn*bc*(*pts_it))*u_op;
}
- return (tmp + tn*t*(*pts_it));
+ return (tmp + tn*u*(*pts_it))*mult_T_;
}
const t_point_t& waypoints() const {return pts_;}
@@ -264,12 +239,12 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
* @param t unNormalized time
* @return the point at time t
*/
- point_t evalDeCasteljau(const Numeric T) const {
+ point_t evalDeCasteljau(const Numeric t) const {
// normalize time :
- const Numeric t = T/T_;
- t_point_t pts = deCasteljauReduction(waypoints(),t);
+ const Numeric u = t/T_;
+ t_point_t pts = deCasteljauReduction(waypoints(),u);
while(pts.size() > 1){
- pts = deCasteljauReduction(pts,t);
+ pts = deCasteljauReduction(pts,u);
}
return pts[0]*mult_T_;
}
@@ -281,18 +256,18 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/**
* @brief deCasteljauReduction compute the de Casteljau's reduction of the given list of points at time t
* @param pts the original list of points
- * @param t the NORMALIZED time
+ * @param u the NORMALIZED time
* @return the reduced list of point (size of pts - 1)
*/
- t_point_t deCasteljauReduction(const t_point_t& pts, const Numeric t) const{
- if(t < 0 || t > 1)
- throw std::out_of_range("In deCasteljau reduction : t is not in [0;1]");
+ t_point_t deCasteljauReduction(const t_point_t& pts, const Numeric u) const{
+ if(u < 0 || u > 1)
+ throw std::out_of_range("In deCasteljau reduction : u is not in [0;1]");
if(pts.size() == 1)
return pts;
t_point_t new_pts;
for(cit_point_t cit = pts.begin() ; cit != (pts.end() - 1) ; ++cit){
- new_pts.push_back((1-t) * (*cit) + t*(*(cit+1)));
+ new_pts.push_back((1-u) * (*cit) + u*(*(cit+1)));
}
return new_pts;
}
@@ -303,24 +278,24 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
* @param t
* @return
*/
- std::pair<bezier_curve_t,bezier_curve_t> split(const Numeric T){
- if (T == T_)
+ std::pair<bezier_curve_t,bezier_curve_t> split(const Numeric t){
+ if (t == T_)
throw std::runtime_error("can't split curve, interval range is equal to original curve");
t_point_t wps_first(size_),wps_second(size_);
- const double t = T/T_;
+ const double u = t/T_;
wps_first[0] = pts_.front();
wps_second[degree_] = pts_.back();
t_point_t casteljau_pts = waypoints();
size_t id = 1;
while(casteljau_pts.size() > 1){
- casteljau_pts = deCasteljauReduction(casteljau_pts,t);
+ casteljau_pts = deCasteljauReduction(casteljau_pts,u);
wps_first[id] = casteljau_pts.front();
wps_second[degree_-id] = casteljau_pts.back();
++id;
}
- bezier_curve_t c_first(wps_first.begin(), wps_first.end(), T,mult_T_);
- bezier_curve_t c_second(wps_second.begin(), wps_second.end(), T_-T,mult_T_);
+ bezier_curve_t c_first(wps_first.begin(), wps_first.end(), t,mult_T_);
+ bezier_curve_t c_second(wps_second.begin(), wps_second.end(), T_-t,mult_T_);
return std::make_pair(c_first,c_second);
}
diff --git a/include/spline/bezier_polynom_conversion.h b/include/hpp/spline/bezier_polynom_conversion.h
similarity index 100%
rename from include/spline/bezier_polynom_conversion.h
rename to include/hpp/spline/bezier_polynom_conversion.h
diff --git a/include/spline/cubic_spline.h b/include/hpp/spline/cubic_spline.h
similarity index 100%
rename from include/spline/cubic_spline.h
rename to include/hpp/spline/cubic_spline.h
diff --git a/include/spline/curve_abc.h b/include/hpp/spline/curve_abc.h
similarity index 100%
rename from include/spline/curve_abc.h
rename to include/hpp/spline/curve_abc.h
diff --git a/include/spline/curve_constraint.h b/include/hpp/spline/curve_constraint.h
similarity index 100%
rename from include/spline/curve_constraint.h
rename to include/hpp/spline/curve_constraint.h
diff --git a/include/spline/exact_cubic.h b/include/hpp/spline/exact_cubic.h
similarity index 100%
rename from include/spline/exact_cubic.h
rename to include/hpp/spline/exact_cubic.h
diff --git a/include/hpp/spline/helpers/CMakeLists.txt b/include/hpp/spline/helpers/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..737295b2636e4d6f74d54ec1f660ffefb75286a4
--- /dev/null
+++ b/include/hpp/spline/helpers/CMakeLists.txt
@@ -0,0 +1,9 @@
+SET(${PROJECT_NAME}_HELPERS_HEADERS
+ effector_spline.h
+ effector_spline_rotation.h
+ )
+
+INSTALL(FILES
+ ${${PROJECT_NAME}_HELPERS_HEADERS}
+ DESTINATION include/hpp/spline/helpers
+ )
diff --git a/include/spline/helpers/effector_spline.h b/include/hpp/spline/helpers/effector_spline.h
similarity index 96%
rename from include/spline/helpers/effector_spline.h
rename to include/hpp/spline/helpers/effector_spline.h
index 11e7a2c9a3bf47fe08e1d9015808f2c74dc3e1df..0f286d3bc5b86f62da8d4a266ad2fe8a0812bc51 100644
--- a/include/spline/helpers/effector_spline.h
+++ b/include/hpp/spline/helpers/effector_spline.h
@@ -1,123 +1,122 @@
-/**
-* \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
-{
-typedef double Numeric;
-typedef double Time;
-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_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
- Numeric norm = normal.norm();
- assert(norm>0.);
- 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, const Time time_offset)
-{
- // compute spline from land way point to end point
- // constraints are null velocity and acceleration
- Numeric norm = normal.norm();
- assert(norm>0.);
- Point n = normal / norm;
- Point d = offset / (time_offset*time_offset*time_offset) * -n;
- Point c = -3 * d * time_offset;
- Point b = -c * time_offset;
-
- T_Point points;
- points.push_back(from);
- points.push_back(b);
- points.push_back(c);
- points.push_back(d);
-
- 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*/)
-{
- //computing end velocity: along landing normal and respecting time
- spline_constraints_t constraints;
- constraints.end_acc = end_spline.derivate(end_spline.min(),2);
- constraints.end_vel = end_spline.derivate(end_spline.min(),1);
- return constraints;
-}
-
-
-/// \brief Helper method to create a spline typically used to
-/// guide the 3d trajectory of a robot end effector.
-/// 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
-///
-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, 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));
- //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);
- waypoints.push_back(landWaypoint);
- //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);
- t_spline_t splines = all_but_end.subSplines_;
- splines.push_back(end_spline);
- return new exact_cubic_t(splines);
-}
-}
-}
-#endif //_CLASS_EFFECTORSPLINE
-
+/**
+* \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 "hpp/spline/spline_deriv_constraint.h"
+
+namespace spline
+{
+namespace helpers
+{
+typedef double Numeric;
+typedef double Time;
+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_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
+ Numeric norm = normal.norm();
+ assert(norm>0.);
+ 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, const Time time_offset)
+{
+ // compute spline from land way point to end point
+ // constraints are null velocity and acceleration
+ Numeric norm = normal.norm();
+ assert(norm>0.);
+ Point n = normal / norm;
+ Point d = offset / (time_offset*time_offset*time_offset) * -n;
+ Point c = -3 * d * time_offset;
+ Point b = -c * time_offset;
+
+ T_Point points;
+ points.push_back(from);
+ points.push_back(b);
+ points.push_back(c);
+ points.push_back(d);
+
+ 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*/)
+{
+ //computing end velocity: along landing normal and respecting time
+ spline_constraints_t constraints;
+ constraints.end_acc = end_spline.derivate(end_spline.min(),2);
+ constraints.end_vel = end_spline.derivate(end_spline.min(),1);
+ return constraints;
+}
+
+
+/// \brief Helper method to create a spline typically used to
+/// guide the 3d trajectory of a robot end effector.
+/// 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
+///
+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, 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));
+ //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);
+ waypoints.push_back(landWaypoint);
+ //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);
+ t_spline_t splines = all_but_end.subSplines_;
+ splines.push_back(end_spline);
+ return new exact_cubic_t(splines);
+}
+}
+}
+#endif //_CLASS_EFFECTORSPLINE
diff --git a/include/spline/helpers/effector_spline_rotation.h b/include/hpp/spline/helpers/effector_spline_rotation.h
similarity index 97%
rename from include/spline/helpers/effector_spline_rotation.h
rename to include/hpp/spline/helpers/effector_spline_rotation.h
index efc007ec8eca01b63f2d02fa16cdbaa747cfd04e..508716228558c283c70c7f515a2bf40126531d2c 100644
--- a/include/spline/helpers/effector_spline_rotation.h
+++ b/include/hpp/spline/helpers/effector_spline_rotation.h
@@ -1,257 +1,256 @@
-/**
-* \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_EFFECTOR_SPLINE_ROTATION
-#define _CLASS_EFFECTOR_SPLINE_ROTATION
-
-#include "spline/helpers/effector_spline.h"
-#include "spline/curve_abc.h"
-#include <Eigen/Geometry>
-
-namespace spline
-{
-namespace helpers
-{
-
-typedef Eigen::Matrix<Numeric, 4, 1> quat_t;
-typedef Eigen::Ref<quat_t> quat_ref_t;
-typedef const Eigen::Ref<const quat_t> quat_ref_const_t;
-typedef Eigen::Matrix<Numeric, 7, 1> config_t;
-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 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),
- const double min = 0., const double max = 1.)
- : curve_abc_quat_t()
- , quat_from_(quat_from.data()), quat_to_(quat_to.data()), min_(min), max_(max)
- , time_reparam_(computeWayPoints())
- {}
-
- ~rotation_spline() {}
-
- /* Copy Constructors / operator=*/
- rotation_spline& operator=(const rotation_spline& from)
- {
- quat_from_ = from.quat_from_;
- quat_to_ = from.quat_to_;
- min_ =from.min_; max_ = from.max_;
- time_reparam_ = spline_deriv_constraint_one_dim(from.time_reparam_);
- return *this;
- }
- /* Copy Constructors / operator=*/
-
- quat_t operator()(const Numeric t) const
- {
- if(t<=min()) return quat_from_.coeffs();
- if(t>=max()) return quat_to_.coeffs();
- //normalize u
- Numeric u = (t - min()) /(max() - min());
- // reparametrize u
- return quat_from_.slerp(time_reparam_(u)[0], quat_to_).coeffs();
- }
-
- virtual quat_t derivate(time_t /*t*/, std::size_t /*order*/) const
- {
- throw std::runtime_error("TODO quaternion spline does not implement derivate");
- }
-
- spline_deriv_constraint_one_dim computeWayPoints() const
- {
- // initializing time reparametrization for spline
- t_waypoint_one_dim_t waypoints;
- waypoints.push_back(std::make_pair(0,point_one_dim_t::Zero()));
- waypoints.push_back(std::make_pair(1,point_one_dim_t::Ones()));
- return spline_deriv_constraint_one_dim(waypoints.begin(), waypoints.end());
- }
-
- virtual time_t min() const{return min_;}
- virtual time_t max() const{return max_;}
-
- public:
- Eigen::Quaterniond quat_from_; //const
- Eigen::Quaterniond quat_to_; //const
- double min_; //const
- double max_; //const
- spline_deriv_constraint_one_dim time_reparam_; //const
-};
-
-
-typedef exact_cubic<Time, Numeric, 4, false, quat_t, std::vector<quat_t,Eigen::aligned_allocator<quat_t> >, rotation_spline> exact_cubic_quat_t;
-
-
-/// \class effector_spline_rotation
-/// \brief Represents a trajectory for and end effector
-/// uses the method effector_spline to create a spline trajectory.
-/// Additionally, handles the rotation of the effector as follows:
-/// does not rotate during the take off and landing phase,
-/// then uses a SLERP algorithm to interpolate the rotation in the quaternion
-/// space.
-class effector_spline_rotation
-{
- /* Constructors - destructors */
- public:
- /// \brief Constructor.
- /// 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 to_quat : 4D vector, quaternion indicating rotation at take off(x, y, z, w)
- /// \param land_quat : 4D vector, quaternion indicating rotation at landing (x, y, z, w)
- /// \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>
- effector_spline_rotation(In wayPointsBegin, In wayPointsEnd,
- quat_ref_const_t& to_quat=quat_t(0,0,0,1), quat_ref_const_t& land_quat=quat_t(0,0,0,1),
- 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)
- : spline_(effector_spline(wayPointsBegin, wayPointsEnd, lift_normal, land_normal, lift_offset, land_offset, lift_offset_duration, land_offset_duration))
- , to_quat_(to_quat.data()), land_quat_(land_quat.data())
- , time_lift_offset_(spline_->min()+lift_offset_duration)
- , time_land_offset_(spline_->max()-land_offset_duration)
- , quat_spline_(simple_quat_spline())
- {
- // NOTHING
- }
-
- /// \brief Constructor.
- /// 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 quatWayPointsBegin : en iterator pointing to the first element of a 4D vector (x, y, z, w) container of
- /// quaternions indicating rotation at specific time steps.
- /// \param quatWayPointsEnd : en iterator pointing to the end of a 4D vector (x, y, z, w) container of
- /// quaternions indicating rotation at specific time steps.
- /// \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, typename InQuat>
- effector_spline_rotation(In wayPointsBegin, In wayPointsEnd,
- InQuat quatWayPointsBegin, InQuat quatWayPointsEnd,
- 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)
- : spline_(effector_spline(wayPointsBegin, wayPointsEnd, lift_normal, land_normal, lift_offset, land_offset, lift_offset_duration, land_offset_duration))
- , to_quat_((quatWayPointsBegin->second).data()), land_quat_(((quatWayPointsEnd-1)->second).data())
- , time_lift_offset_(spline_->min()+lift_offset_duration)
- , time_land_offset_(spline_->max()-land_offset_duration)
- , quat_spline_(quat_spline(quatWayPointsBegin, quatWayPointsEnd))
- {
- // NOTHING
- }
-
- ~effector_spline_rotation() {delete spline_;}
- /* Constructors - destructors */
-
- /*Helpers*/
- public:
- Numeric min() const{return spline_->min();}
- Numeric max() const{return spline_->max();}
- /*Helpers*/
-
- /*Operations*/
- public:
- /// \brief Evaluation of the effector position and rotation at time t.
- /// \param t : the time when to evaluate the spline
- /// \param return : a 7D vector; The 3 first values are the 3D position, the 4 last the
- /// quaternion describing the rotation
- ///
- config_t operator()(const Numeric t) const
- {
- config_t res;
- res.head<3>() = (*spline_)(t);
- res.tail<4>() = interpolate_quat(t);
- return res;
- }
-
- public:
- quat_t interpolate_quat(const Numeric t) const
- {
- if(t<=time_lift_offset_) return to_quat_.coeffs();
- if(t>=time_land_offset_) return land_quat_.coeffs();
- return quat_spline_(t);
- }
-
- private:
- exact_cubic_quat_t simple_quat_spline() const
- {
- std::vector<rotation_spline> splines;
- splines.push_back(rotation_spline(to_quat_.coeffs(),land_quat_.coeffs(),time_lift_offset_, time_land_offset_));
- return exact_cubic_quat_t(splines);
- }
-
- template <typename InQuat>
- exact_cubic_quat_t quat_spline(InQuat quatWayPointsBegin, InQuat quatWayPointsEnd) const
- {
- if(std::distance(quatWayPointsBegin, quatWayPointsEnd) < 1)
- return simple_quat_spline();
- exact_cubic_quat_t::t_spline_t splines;
- InQuat it(quatWayPointsBegin);
- Time init = time_lift_offset_;
- Eigen::Quaterniond current_quat = to_quat_;
- for(; it != quatWayPointsEnd; ++it)
- {
- splines.push_back(rotation_spline(current_quat.coeffs(), it->second, init, it->first));
- current_quat = it->second;
- init = it->first;
- }
- splines.push_back(rotation_spline(current_quat.coeffs(), land_quat_.coeffs(), init, time_land_offset_));
- return exact_cubic_quat_t(splines);
- }
-
- /*Operations*/
-
- /*Attributes*/
- public:
- const exact_cubic_t* spline_;
- const Eigen::Quaterniond to_quat_;
- const Eigen::Quaterniond land_quat_;
- const double time_lift_offset_;
- const double time_land_offset_;
- const exact_cubic_quat_t quat_spline_;
- /*Attributes*/
-};
-
-} // namespace helpers
-} // namespace spline
-#endif //_CLASS_EFFECTOR_SPLINE_ROTATION
-
+/**
+* \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_EFFECTOR_SPLINE_ROTATION
+#define _CLASS_EFFECTOR_SPLINE_ROTATION
+
+#include "hpp/spline/helpers/effector_spline.h"
+#include "hpp/spline/curve_abc.h"
+#include <Eigen/Geometry>
+
+namespace spline
+{
+namespace helpers
+{
+
+typedef Eigen::Matrix<Numeric, 4, 1> quat_t;
+typedef Eigen::Ref<quat_t> quat_ref_t;
+typedef const Eigen::Ref<const quat_t> quat_ref_const_t;
+typedef Eigen::Matrix<Numeric, 7, 1> config_t;
+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 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),
+ const double min = 0., const double max = 1.)
+ : curve_abc_quat_t()
+ , quat_from_(quat_from.data()), quat_to_(quat_to.data()), min_(min), max_(max)
+ , time_reparam_(computeWayPoints())
+ {}
+
+ ~rotation_spline() {}
+
+ /* Copy Constructors / operator=*/
+ rotation_spline& operator=(const rotation_spline& from)
+ {
+ quat_from_ = from.quat_from_;
+ quat_to_ = from.quat_to_;
+ min_ =from.min_; max_ = from.max_;
+ time_reparam_ = spline_deriv_constraint_one_dim(from.time_reparam_);
+ return *this;
+ }
+ /* Copy Constructors / operator=*/
+
+ quat_t operator()(const Numeric t) const
+ {
+ if(t<=min()) return quat_from_.coeffs();
+ if(t>=max()) return quat_to_.coeffs();
+ //normalize u
+ Numeric u = (t - min()) /(max() - min());
+ // reparametrize u
+ return quat_from_.slerp(time_reparam_(u)[0], quat_to_).coeffs();
+ }
+
+ virtual quat_t derivate(time_t /*t*/, std::size_t /*order*/) const
+ {
+ throw std::runtime_error("TODO quaternion spline does not implement derivate");
+ }
+
+ spline_deriv_constraint_one_dim computeWayPoints() const
+ {
+ // initializing time reparametrization for spline
+ t_waypoint_one_dim_t waypoints;
+ waypoints.push_back(std::make_pair(0,point_one_dim_t::Zero()));
+ waypoints.push_back(std::make_pair(1,point_one_dim_t::Ones()));
+ return spline_deriv_constraint_one_dim(waypoints.begin(), waypoints.end());
+ }
+
+ virtual time_t min() const{return min_;}
+ virtual time_t max() const{return max_;}
+
+ public:
+ Eigen::Quaterniond quat_from_; //const
+ Eigen::Quaterniond quat_to_; //const
+ double min_; //const
+ double max_; //const
+ spline_deriv_constraint_one_dim time_reparam_; //const
+};
+
+
+typedef exact_cubic<Time, Numeric, 4, false, quat_t, std::vector<quat_t,Eigen::aligned_allocator<quat_t> >, rotation_spline> exact_cubic_quat_t;
+
+
+/// \class effector_spline_rotation
+/// \brief Represents a trajectory for and end effector
+/// uses the method effector_spline to create a spline trajectory.
+/// Additionally, handles the rotation of the effector as follows:
+/// does not rotate during the take off and landing phase,
+/// then uses a SLERP algorithm to interpolate the rotation in the quaternion
+/// space.
+class effector_spline_rotation
+{
+ /* Constructors - destructors */
+ public:
+ /// \brief Constructor.
+ /// 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 to_quat : 4D vector, quaternion indicating rotation at take off(x, y, z, w)
+ /// \param land_quat : 4D vector, quaternion indicating rotation at landing (x, y, z, w)
+ /// \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>
+ effector_spline_rotation(In wayPointsBegin, In wayPointsEnd,
+ quat_ref_const_t& to_quat=quat_t(0,0,0,1), quat_ref_const_t& land_quat=quat_t(0,0,0,1),
+ 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)
+ : spline_(effector_spline(wayPointsBegin, wayPointsEnd, lift_normal, land_normal, lift_offset, land_offset, lift_offset_duration, land_offset_duration))
+ , to_quat_(to_quat.data()), land_quat_(land_quat.data())
+ , time_lift_offset_(spline_->min()+lift_offset_duration)
+ , time_land_offset_(spline_->max()-land_offset_duration)
+ , quat_spline_(simple_quat_spline())
+ {
+ // NOTHING
+ }
+
+ /// \brief Constructor.
+ /// 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 quatWayPointsBegin : en iterator pointing to the first element of a 4D vector (x, y, z, w) container of
+ /// quaternions indicating rotation at specific time steps.
+ /// \param quatWayPointsEnd : en iterator pointing to the end of a 4D vector (x, y, z, w) container of
+ /// quaternions indicating rotation at specific time steps.
+ /// \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, typename InQuat>
+ effector_spline_rotation(In wayPointsBegin, In wayPointsEnd,
+ InQuat quatWayPointsBegin, InQuat quatWayPointsEnd,
+ 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)
+ : spline_(effector_spline(wayPointsBegin, wayPointsEnd, lift_normal, land_normal, lift_offset, land_offset, lift_offset_duration, land_offset_duration))
+ , to_quat_((quatWayPointsBegin->second).data()), land_quat_(((quatWayPointsEnd-1)->second).data())
+ , time_lift_offset_(spline_->min()+lift_offset_duration)
+ , time_land_offset_(spline_->max()-land_offset_duration)
+ , quat_spline_(quat_spline(quatWayPointsBegin, quatWayPointsEnd))
+ {
+ // NOTHING
+ }
+
+ ~effector_spline_rotation() {delete spline_;}
+ /* Constructors - destructors */
+
+ /*Helpers*/
+ public:
+ Numeric min() const{return spline_->min();}
+ Numeric max() const{return spline_->max();}
+ /*Helpers*/
+
+ /*Operations*/
+ public:
+ /// \brief Evaluation of the effector position and rotation at time t.
+ /// \param t : the time when to evaluate the spline
+ /// \param return : a 7D vector; The 3 first values are the 3D position, the 4 last the
+ /// quaternion describing the rotation
+ ///
+ config_t operator()(const Numeric t) const
+ {
+ config_t res;
+ res.head<3>() = (*spline_)(t);
+ res.tail<4>() = interpolate_quat(t);
+ return res;
+ }
+
+ public:
+ quat_t interpolate_quat(const Numeric t) const
+ {
+ if(t<=time_lift_offset_) return to_quat_.coeffs();
+ if(t>=time_land_offset_) return land_quat_.coeffs();
+ return quat_spline_(t);
+ }
+
+ private:
+ exact_cubic_quat_t simple_quat_spline() const
+ {
+ std::vector<rotation_spline> splines;
+ splines.push_back(rotation_spline(to_quat_.coeffs(),land_quat_.coeffs(),time_lift_offset_, time_land_offset_));
+ return exact_cubic_quat_t(splines);
+ }
+
+ template <typename InQuat>
+ exact_cubic_quat_t quat_spline(InQuat quatWayPointsBegin, InQuat quatWayPointsEnd) const
+ {
+ if(std::distance(quatWayPointsBegin, quatWayPointsEnd) < 1)
+ return simple_quat_spline();
+ exact_cubic_quat_t::t_spline_t splines;
+ InQuat it(quatWayPointsBegin);
+ Time init = time_lift_offset_;
+ Eigen::Quaterniond current_quat = to_quat_;
+ for(; it != quatWayPointsEnd; ++it)
+ {
+ splines.push_back(rotation_spline(current_quat.coeffs(), it->second, init, it->first));
+ current_quat = it->second;
+ init = it->first;
+ }
+ splines.push_back(rotation_spline(current_quat.coeffs(), land_quat_.coeffs(), init, time_land_offset_));
+ return exact_cubic_quat_t(splines);
+ }
+
+ /*Operations*/
+
+ /*Attributes*/
+ public:
+ const exact_cubic_t* spline_;
+ const Eigen::Quaterniond to_quat_;
+ const Eigen::Quaterniond land_quat_;
+ const double time_lift_offset_;
+ const double time_land_offset_;
+ const exact_cubic_quat_t quat_spline_;
+ /*Attributes*/
+};
+
+} // namespace helpers
+} // namespace spline
+#endif //_CLASS_EFFECTOR_SPLINE_ROTATION
diff --git a/include/spline/optimization/OptimizeSpline.h b/include/hpp/spline/optimization/OptimizeSpline.h
similarity index 100%
rename from include/spline/optimization/OptimizeSpline.h
rename to include/hpp/spline/optimization/OptimizeSpline.h
diff --git a/include/spline/polynom.h b/include/hpp/spline/polynom.h
similarity index 100%
rename from include/spline/polynom.h
rename to include/hpp/spline/polynom.h
diff --git a/include/spline/quintic_spline.h b/include/hpp/spline/quintic_spline.h
similarity index 100%
rename from include/spline/quintic_spline.h
rename to include/hpp/spline/quintic_spline.h
diff --git a/include/spline/spline_deriv_constraint.h b/include/hpp/spline/spline_deriv_constraint.h
similarity index 100%
rename from include/spline/spline_deriv_constraint.h
rename to include/hpp/spline/spline_deriv_constraint.h
diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
index 51dde9504ca3c4e6b35873cc2523784418eefe29..499561c0144a27e1daad52e197d5568264dfa848 100644
--- a/python/CMakeLists.txt
+++ b/python/CMakeLists.txt
@@ -1,24 +1,21 @@
-cmake_minimum_required( VERSION 2.8 )
-
-include_directories("${EIGEN3_INCLUDE_DIR}")
+STRING(REGEX REPLACE "-" "_" PY_NAME ${PROJECT_NAME})
ADD_REQUIRED_DEPENDENCY("eigenpy")
-include_directories("${PROJECT_SOURCE_DIR}/include")
-FILE(GLOB_RECURSE HeaderFiles "${PROJECT_SOURCE_DIR}/include/spline/*.h")
-
# Define the wrapper library that wraps our library
-add_library( spline SHARED spline_python.cpp )
+add_library( ${PY_NAME} SHARED spline_python.cpp )
#~ target_link_libraries( centroidal_dynamics ${Boost_LIBRARIES} ${PROJECT_NAME} )
# don't prepend wrapper library name with lib
-set_target_properties( spline PROPERTIES PREFIX "" )
+set_target_properties( ${PY_NAME} PROPERTIES PREFIX "" )
IF(APPLE)
- # We need to change the extension for python bindings
- SET_TARGET_PROPERTIES(spline PROPERTIES SUFFIX ".so")
+ # We need to change the extension for python bindings
+ SET_TARGET_PROPERTIES(${PY_NAME} PROPERTIES SUFFIX ".so")
ENDIF(APPLE)
-PKG_CONFIG_USE_DEPENDENCY(spline eigenpy)
+PKG_CONFIG_USE_DEPENDENCY(${PY_NAME} eigenpy)
INSTALL(
- FILES ${LIBRARY_OUTPUT_PATH}spline.so DESTINATION ${PYTHON_SITELIB}
-)
+ TARGETS ${PY_NAME} DESTINATION ${PYTHON_SITELIB}
+ )
+
+ADD_PYTHON_UNIT_TEST("python-spline" "python/test/test.py" "python")
diff --git a/python/spline_python.cpp b/python/spline_python.cpp
index 67f3623568dee9eb5d3d1bfbadaa549d48db5b60..764b5cc78e360fd6bbc0e216569e0da3edfa8574 100644
--- a/python/spline_python.cpp
+++ b/python/spline_python.cpp
@@ -1,10 +1,10 @@
-#include "spline/bezier_curve.h"
-#include "spline/polynom.h"
-#include "spline/exact_cubic.h"
-#include "spline/spline_deriv_constraint.h"
-#include "spline/curve_constraint.h"
-#include "spline/bezier_polynom_conversion.h"
-#include "spline/bernstein.h"
+#include "hpp/spline/bezier_curve.h"
+#include "hpp/spline/polynom.h"
+#include "hpp/spline/exact_cubic.h"
+#include "hpp/spline/spline_deriv_constraint.h"
+#include "hpp/spline/curve_constraint.h"
+#include "hpp/spline/bezier_polynom_conversion.h"
+#include "hpp/spline/bernstein.h"
#include <vector>
@@ -205,7 +205,7 @@ void set_end_acc(curve_constraints_t& c, const point_t& val)
-BOOST_PYTHON_MODULE(spline)
+BOOST_PYTHON_MODULE(hpp_spline)
{
/** BEGIN eigenpy init**/
eigenpy::enableEigenPy();
diff --git a/python/test/test.py b/python/test/test.py
index f34b4133aef82fe0878733b2d815ed57d1712e63..38e775957d5b9e18c958878e2982f9887d8f1e6f 100644
--- a/python/test/test.py
+++ b/python/test/test.py
@@ -1,103 +1,111 @@
-from spline import bezier, bezier6, polynom, exact_cubic, curve_constraints, spline_deriv_constraint, from_bezier
+#!/usr/bin/env python
+
+import unittest
from numpy import matrix
from numpy.linalg import norm
-
-__EPS = 1e-6
-
-waypoints = matrix([[1.,2.,3.],[4.,5.,6.]]).transpose()
-waypoints6 = matrix([[1.,2.,3.,7.,5.,5.],[4.,5.,6.,4.,5.,6.]]).transpose()
-time_waypoints = matrix([0.,1.])
-
-#testing bezier curve
-a = bezier6(waypoints6)
-a = bezier(waypoints, 3.)
-
-assert(a.degree == a.nbWaypoints -1)
-a.min()
-a.max()
-a(0.4)
-assert((a.derivate(0.4,0) == a(0.4)).all())
-a.derivate(0.4,2)
-a = a.compute_derivate(100)
-
-prim = a.compute_primitive(1)
-
-
-for i in range(10):
- t = float(i) / 10.
- assert(a(t) == prim.derivate(t,1)).all()
-assert(prim(0) == matrix([0.,0.,0.])).all()
-
-prim = a.compute_primitive(2)
-for i in range(10):
- t = float(i) / 10.
- assert(a(t) == prim.derivate(t,2)).all()
-assert(prim(0) == matrix([0.,0.,0.])).all()
-
-waypoints = matrix([[1.,2.,3.],[4.,5.,6.],[4.,5.,6.],[4.,5.,6.],[4.,5.,6.]]).transpose()
-a0 = bezier(waypoints)
-a1 = bezier(waypoints, 3.)
-prim0 = a0.compute_primitive(1)
-prim1 = a1.compute_primitive(1)
-
-for i in range(10):
- t = float(i) / 10.
- assert norm(a0(t) - a1(3*t)) < __EPS
- assert norm(a0.derivate(t,1) - a1.derivate(3*t,1) * 3.) < __EPS
- assert norm(a0.derivate(t,2) - a1.derivate(3*t,2) * 9.) < __EPS
- assert norm(prim0(t) - prim1(t*3) / 3.) < __EPS
-assert(prim(0) == matrix([0.,0.,0.])).all()
-
-
-#testing bezier with constraints
-c = curve_constraints();
-c.init_vel = matrix([0.,1.,1.]);
-c.end_vel = matrix([0.,1.,1.]);
-c.init_acc = matrix([0.,1.,-1.]);
-c.end_acc = matrix([0.,100.,1.]);
-
-waypoints = matrix([[1.,2.,3.],[4.,5.,6.]]).transpose()
-a = bezier(waypoints,c)
-assert norm(a.derivate(0,1) - c.init_vel) < 1e-10
-assert norm(a.derivate(1,2) - c.end_acc) < 1e-10
-
-
-#testing polynom function
-a = polynom(waypoints)
-a = polynom(waypoints, -1., 3.)
-a.min()
-a.max()
-a(0.4)
-assert((a.derivate(0.4,0) == a(0.4)).all())
-a.derivate(0.4,2)
-
-#testing exact_cubic function
-a = exact_cubic(waypoints, time_waypoints)
-a.min()
-a.max()
-a(0.4)
-assert((a.derivate(0.4,0) == a(0.4)).all())
-a.derivate(0.4,2)
-
-#testing spline_deriv_constraints
-c = curve_constraints();
-c.init_vel;
-c.end_vel;
-c.init_acc;
-c.end_acc;
-
-
-c.init_vel = matrix([0.,1.,1.]);
-c.end_vel = matrix([0.,1.,1.]);
-c.init_acc = matrix([0.,1.,1.]);
-c.end_acc = matrix([0.,1.,1.]);
-
-a = spline_deriv_constraint (waypoints, time_waypoints)
-a = spline_deriv_constraint (waypoints, time_waypoints, c)
-
-#converting bezier to polynom
-
-a = bezier(waypoints)
-a_pol = from_bezier(a)
-assert norm(a(0.3) - a_pol(0.3)) < __EPS
+from numpy.testing import assert_allclose
+
+from hpp_spline import bezier, bezier6, curve_constraints, exact_cubic, from_bezier, polynom, spline_deriv_constraint
+
+
+class TestSpline(unittest.TestCase):
+ def test_spline(self):
+ waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).T
+ waypoints6 = matrix([[1., 2., 3., 7., 5., 5.], [4., 5., 6., 4., 5., 6.]]).T
+ time_waypoints = matrix([0., 1.]).T
+
+ # testing bezier curve
+ a = bezier6(waypoints6)
+ a = bezier(waypoints, 3.)
+
+ self.assertEqual(a.degree, a.nbWaypoints - 1)
+ a.min()
+ a.max()
+ a(0.4)
+ assert_allclose(a.derivate(0.4, 0), a(0.4))
+ a.derivate(0.4, 2)
+ a = a.compute_derivate(100)
+
+ prim = a.compute_primitive(1)
+
+ for i in range(10):
+ t = float(i) / 10.
+ assert_allclose(a(t), prim.derivate(t, 1))
+ assert_allclose(prim(0), matrix([0., 0., 0.]).T)
+
+ prim = a.compute_primitive(2)
+ for i in range(10):
+ t = float(i) / 10.
+ assert_allclose(a(t), prim.derivate(t, 2), atol=1e-20)
+ assert_allclose(prim(0), matrix([0., 0., 0.]).T)
+
+ waypoints = matrix([[1., 2., 3.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.]]).T
+ a0 = bezier(waypoints)
+ a1 = bezier(waypoints, 3.)
+ prim0 = a0.compute_primitive(1)
+ prim1 = a1.compute_primitive(1)
+
+ for i in range(10):
+ t = float(i) / 10.
+ assert_allclose(a0(t), a1(3 * t))
+ assert_allclose(a0.derivate(t, 1), a1.derivate(3 * t, 1) * 3.)
+ assert_allclose(a0.derivate(t, 2), a1.derivate(3 * t, 2) * 9.)
+ assert_allclose(prim0(t), prim1(t * 3) / 3.)
+ assert_allclose(prim(0), matrix([0., 0., 0.]).T)
+ with self.assertRaises(AssertionError):
+ assert_allclose(prim(0), matrix([0., 0., 0.]))
+
+ # testing bezier with constraints
+ c = curve_constraints()
+ c.init_vel = matrix([0., 1., 1.]).T
+ c.end_vel = matrix([0., 1., 1.]).T
+ c.init_acc = matrix([0., 1., -1.]).T
+ c.end_acc = matrix([0., 100., 1.]).T
+
+ waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).T
+ a = bezier(waypoints, c)
+ assert_allclose(a.derivate(0, 1), c.init_vel)
+ assert_allclose(a.derivate(1, 2), c.end_acc)
+
+ # testing polynom function
+ a = polynom(waypoints)
+ a = polynom(waypoints, -1., 3.)
+ a.min()
+ a.max()
+ a(0.4)
+ assert_allclose(a.derivate(0.4, 0), a(0.4))
+ a.derivate(0.4, 2)
+
+ # testing exact_cubic function
+ a = exact_cubic(waypoints, time_waypoints)
+ a.min()
+ a.max()
+ a(0.4)
+ assert_allclose(a.derivate(0.4, 0), a(0.4))
+ a.derivate(0.4, 2)
+
+ # testing spline_deriv_constraints
+ c = curve_constraints()
+ c.init_vel
+ c.end_vel
+ c.init_acc
+ c.end_acc
+
+ c.init_vel = matrix([0., 1., 1.]).T
+ c.end_vel = matrix([0., 1., 1.]).T
+ c.init_acc = matrix([0., 1., 1.]).T
+ c.end_acc = matrix([0., 1., 1.]).T
+
+ a = spline_deriv_constraint(waypoints, time_waypoints)
+ a = spline_deriv_constraint(waypoints, time_waypoints, c)
+
+ # converting bezier to polynom
+
+ a = bezier(waypoints)
+ a_pol = from_bezier(a)
+ assert_allclose(a(0.3), a_pol(0.3))
+
+
+if __name__ == '__main__':
+ unittest.main()
diff --git a/src/tests/spline_test/CMakeLists.txt b/src/tests/spline_test/CMakeLists.txt
deleted file mode 100644
index 874c7e363295e096b41304df17f09baa6fb01355..0000000000000000000000000000000000000000
--- a/src/tests/spline_test/CMakeLists.txt
+++ /dev/null
@@ -1,10 +0,0 @@
-cmake_minimum_required(VERSION 2.6)
-
-include_directories("${PROJECT_SOURCE_DIR}/include")
-
-FILE(GLOB_RECURSE HeaderFiles "${PROJECT_SOURCE_DIR}/include/spline/*.h")
-
-add_executable(
- spline_tests ${HeaderFiles} Main.cpp
-)
-
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..70960bbbfd3df38d9fe09ab0138fc8c6e1ef02d9
--- /dev/null
+++ b/tests/CMakeLists.txt
@@ -0,0 +1,3 @@
+ADD_UNIT_TEST(
+ spline_tests Main.cpp
+)
diff --git a/src/tests/spline_test/Main.cpp b/tests/Main.cpp
similarity index 95%
rename from src/tests/spline_test/Main.cpp
rename to tests/Main.cpp
index 686e8f6ab631d7bce4834e0e931a5e8bf50b39f3..5046ad1c85f21378c10893be9f0dbc49b06ebb97 100644
--- a/src/tests/spline_test/Main.cpp
+++ b/tests/Main.cpp
@@ -1,957 +1,959 @@
-
-#include "spline/exact_cubic.h"
-#include "spline/bezier_curve.h"
-#include "spline/polynom.h"
-#include "spline/spline_deriv_constraint.h"
-#include "spline/helpers/effector_spline.h"
-#include "spline/helpers/effector_spline_rotation.h"
-#include "spline/bezier_polynom_conversion.h"
-
-#include <string>
-#include <iostream>
-#include <cmath>
-
-using namespace std;
-
-namespace spline
-{
-typedef Eigen::Vector3d point_t;
-typedef std::vector<point_t,Eigen::aligned_allocator<point_t> > t_point_t;
-typedef polynom <double, double, 3, true, point_t, t_point_t> polynom_t;
-typedef exact_cubic <double, double, 3, true, point_t> exact_cubic_t;
-typedef spline_deriv_constraint <double, double, 3, true, point_t> spline_deriv_constraint_t;
-typedef bezier_curve <double, double, 3, true, point_t> bezier_curve_t;
-typedef spline_deriv_constraint_t::spline_constraints spline_constraints_t;
-typedef std::pair<double, point_t> Waypoint;
-typedef std::vector<Waypoint> T_Waypoint;
-
-
-typedef Eigen::Matrix<double,1,1> point_one;
-typedef polynom<double, double, 1, true, point_one> polynom_one;
-typedef exact_cubic <double, double, 1, true, point_one> exact_cubic_one;
-typedef std::pair<double, point_one> WaypointOne;
-typedef std::vector<WaypointOne> T_WaypointOne;
-
-bool QuasiEqual(const double a, const double b, const float margin)
-{
- if ((a <= 0 && b <= 0) || (a >= 0 && b>= 0))
- {
- return (abs(a-b)) <= margin;
- }
- else
- {
- return abs(a) + abs(b) <= margin;
- }
-}
-
-const double margin = 0.001;
-
-} // namespace spline
-
-using namespace spline;
-
-ostream& operator<<(ostream& os, const point_t& pt)
-{
- os << "(" << pt.x() << ", " << pt.y() << ", " << pt.z() << ")";
- return os;
-}
-
-void ComparePoints(const Eigen::VectorXd& pt1, const Eigen::VectorXd& pt2, const std::string& errmsg, bool& error, bool notequal = false)
-{
- if((pt1-pt2).norm() > margin && !notequal)
- {
- error = true;
- std::cout << errmsg << pt1.transpose() << " ; " << pt2.transpose() << std::endl;
- }
-}
-
-/*Cubic Function tests*/
-
-void CubicFunctionTest(bool& error)
-{
- std::string errMsg("In test CubicFunctionTest ; unexpected result for x ");
- point_t a(1,2,3);
- point_t b(2,3,4);
- point_t c(3,4,5);
- point_t d(3,6,7);
- t_point_t vec;
- vec.push_back(a);
- vec.push_back(b);
- vec.push_back(c);
- vec.push_back(d);
- polynom_t cf(vec.begin(), vec.end(), 0, 1);
- point_t res1;
- res1 =cf(0);
- point_t x0(1,2,3);
- ComparePoints(x0, res1, errMsg + "(0) ", error);
-
- point_t x1(9,15,19);
- res1 =cf(1);
- ComparePoints(x1, res1, errMsg + "(1) ", error);
-
- point_t x2(3.125,5.25,7.125);
- res1 =cf(0.5);
- ComparePoints(x2, res1, errMsg + "(0.5) ", error);
-
- vec.clear();
- vec.push_back(a);
- vec.push_back(b);
- vec.push_back(c);
- vec.push_back(d);
- polynom_t cf2(vec, 0.5, 1);
- res1 = cf2(0.5);
- ComparePoints(x0, res1, errMsg + "x3 ", error);
- error = true;
- try
- {
- cf2(0.4);
- }
- catch(...)
- {
- error = false;
- }
- if(error)
- {
- std::cout << "Evaluation of cubic cf2 error, 0.4 should be an out of range value\n";
- }
- error = true;
- try
- {
- cf2(1.1);
- }
- catch(...)
- {
- error = false;
- }
- if(error)
- {
- std::cout << "Evaluation of cubic cf2 error, 1.1 should be an out of range value\n";
- }
- if(cf.max() != 1)
- {
- error = true;
- std::cout << "Evaluation of exactCubic error, MaxBound should be equal to 1\n";
- }
- if(cf.min() != 0)
- {
- error = true;
- std::cout << "Evaluation of exactCubic error, MinBound should be equal to 1\n";
- }
-}
-
-/*bezier_curve Function tests*/
-
-void BezierCurveTest(bool& error)
-{
- std::string errMsg("In test BezierCurveTest ; unexpected result for x ");
- point_t a(1,2,3);
- point_t b(2,3,4);
- point_t c(3,4,5);
- point_t d(3,6,7);
-
- std::vector<point_t> params;
- params.push_back(a);
- params.push_back(b);
-
- // 2d curve
- bezier_curve_t cf(params.begin(), params.end());
- point_t res1;
- res1 = cf(0);
- point_t x20 = a ;
- ComparePoints(x20, res1, errMsg + "2(0) ", error);
-
- point_t x21 = b;
- res1 = cf(1);
- ComparePoints(x21, res1, errMsg + "2(1) ", error);
-
- //3d curve
- params.push_back(c);
- bezier_curve_t cf3(params.begin(), params.end());
- res1 = cf3(0);
- ComparePoints(a, res1, errMsg + "3(0) ", error);
-
- res1 = cf3(1);
- ComparePoints(c, res1, errMsg + "3(1) ", error);
-
- //4d curve
- params.push_back(d);
- bezier_curve_t cf4(params.begin(), params.end(), 2);
-
- res1 = cf4(2);
- ComparePoints(d, res1, errMsg + "3(1) ", error);
-
- //testing bernstein polynomes
- std::string errMsg2("In test BezierCurveTest ; Bernstein polynoms do not evaluate as analytical evaluation");
- for(double d = 0.; d <1.; d+=0.1)
- {
- ComparePoints( cf3.evalBernstein(d) , cf3 (d), errMsg2, error);
- ComparePoints( cf3.evalHorner(d) , cf3 (d), errMsg2, error);
- }
-
- bool error_in(true);
- try
- {
- cf(-0.4);
- }
- catch(...)
- {
- error_in = false;
- }
- if(error_in)
- {
- std::cout << "Evaluation of bezier cf error, -0.4 should be an out of range value\n";
- error = true;
- }
- error_in = true;
- try
- {
- cf(1.1);
- }
- catch(...)
- {
- error_in = false;
- }
- if(error_in)
- {
- std::cout << "Evaluation of bezier cf error, 1.1 should be an out of range value\n";
- error = true;
- }
- if(cf.max() != 1)
- {
- error = true;
- std::cout << "Evaluation of exactCubic error, MaxBound should be equal to 1\n";
- }
- if(cf.min() != 0)
- {
- error = true;
- std::cout << "Evaluation of exactCubic error, MinBound should be equal to 1\n";
- }
-}
-
-#include <ctime>
-void BezierCurveTestCompareHornerAndBernstein(bool& /*error*/)
-{
- using namespace std;
- std::vector<double> values;
- for (int i =0; i < 100000; ++i)
- values.push_back(rand()/RAND_MAX);
-
- //first compare regular evaluation (low dim pol)
- point_t a(1,2,3);
- point_t b(2,3,4);
- point_t c(3,4,5);
- point_t d(3,6,7);
- point_t e(3,61,7);
- point_t f(3,56,7);
- point_t g(3,36,7);
- point_t h(43,6,7);
- point_t i(3,6,77);
-
- std::vector<point_t> params;
- params.push_back(a);
- params.push_back(b);
- params.push_back(c);
-
- // 3d curve
- bezier_curve_t cf(params.begin(), params.end());
-
- clock_t s0,e0,s1,e1,s2,e2,s3,e3;
- s0 = clock();
- for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
- {
- cf(*cit);
- }
- e0 = clock();
-
- s1 = clock();
- for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
- {
- cf.evalBernstein(*cit);
- }
- e1 = clock();
-
- s2 = clock();
- for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
- {
- cf.evalHorner(*cit);
- }
- e2 = clock();
-
- s3 = clock();
- for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
- {
- cf.evalDeCasteljau(*cit);
- }
- e3 = clock();
-
-
- std::cout << "time for analytical eval " << double(e0 - s0) / CLOCKS_PER_SEC << std::endl;
- std::cout << "time for bernstein eval " << double(e1 - s1) / CLOCKS_PER_SEC << std::endl;
- std::cout << "time for horner eval " << double(e2 - s2) / CLOCKS_PER_SEC << std::endl;
- std::cout << "time for deCasteljau eval " << double(e3 - s3) / CLOCKS_PER_SEC << std::endl;
-
- std::cout << "now with high order polynom " << std::endl;
-
-
- params.push_back(d);
- params.push_back(e);
- params.push_back(f);
- params.push_back(g);
- params.push_back(h);
- params.push_back(i);
-
- bezier_curve_t cf2(params.begin(), params.end());
-
- s1 = clock();
- for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
- {
- cf2.evalBernstein(*cit);
- }
- e1 = clock();
-
- s2 = clock();
- for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
- {
- cf2.evalHorner(*cit);
- }
- e2 = clock();
-
- s0 = clock();
- for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
- {
- cf2(*cit);
- }
- e0 = clock();
-
- s3 = clock();
- for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
- {
- cf2.evalDeCasteljau(*cit);
- }
- e3 = clock();
-
-
-
- std::cout << "time for analytical eval " << double(e0 - s0) / CLOCKS_PER_SEC << std::endl;
- std::cout << "time for bernstein eval " << double(e1 - s1) / CLOCKS_PER_SEC << std::endl;
- std::cout << "time for horner eval " << double(e2 - s2) / CLOCKS_PER_SEC << std::endl;
- std::cout << "time for deCasteljau eval " << double(e3 - s3) / CLOCKS_PER_SEC << std::endl;
-
-}
-
-void BezierDerivativeCurveTest(bool& error)
-{
- std::string errMsg("In test BezierDerivativeCurveTest ; unexpected result for x ");
- point_t a(1,2,3);
- point_t b(2,3,4);
- point_t c(3,4,5);
-
- std::vector<point_t> params;
- params.push_back(a);
- params.push_back(b);
-
- params.push_back(c);
- bezier_curve_t cf3(params.begin(), params.end());
-
- ComparePoints(cf3(0), cf3.derivate(0.,0), errMsg, error);
- ComparePoints(cf3(0), cf3.derivate(0.,1), errMsg, error, true);
- ComparePoints(point_t::Zero(), cf3.derivate(0.,100), errMsg, error);
-}
-
-void BezierDerivativeCurveTimeReparametrizationTest(bool& error)
-{
- std::string errMsg("In test BezierDerivativeCurveTimeReparametrizationTest ; unexpected result for x ");
- point_t a(1,2,3);
- point_t b(2,3,4);
- point_t c(3,4,5);
- point_t d(3,4,5);
- point_t e(3,4,5);
- point_t f(3,4,5);
-
- std::vector<point_t> params;
- params.push_back(a);
- params.push_back(b);
- params.push_back(c);
- params.push_back(d);
- params.push_back(e);
- params.push_back(f);
- double T = 2.;
- bezier_curve_t cf(params.begin(), params.end());
- bezier_curve_t cfT(params.begin(), params.end(),T);
-
- ComparePoints(cf(0.5), cfT(1), errMsg, error);
- ComparePoints(cf.derivate(0.5,1), cfT.derivate(1,1) * T, errMsg, error);
- ComparePoints(cf.derivate(0.5,2), cfT.derivate(1,2) * T*T, errMsg, error);
-}
-
-void BezierDerivativeCurveConstraintTest(bool& error)
-{
- std::string errMsg("In test BezierDerivativeCurveConstraintTest ; unexpected result for x ");
- point_t a(1,2,3);
- point_t b(2,3,4);
- point_t c(3,4,5);
-
- bezier_curve_t::curve_constraints_t constraints;
- constraints.init_vel = point_t(-1,-1,-1);
- constraints.init_acc = point_t(-2,-2,-2);
- constraints.end_vel = point_t(-10,-10,-10);
- constraints.end_acc = point_t(-20,-20,-20);
-
- std::vector<point_t> params;
- params.push_back(a);
- params.push_back(b);
-
- params.push_back(c);
- bezier_curve_t cf3(params.begin(), params.end(), constraints);
-
- assert(cf3.degree_ == params.size() + 3);
- assert(cf3.size_ == params.size() + 4);
-
- ComparePoints(a, cf3(0), errMsg, error);
- ComparePoints(c, cf3(1), errMsg, error);
- ComparePoints(constraints.init_vel, cf3.derivate(0.,1), errMsg, error);
- ComparePoints(constraints.end_vel , cf3.derivate(1.,1), errMsg, error);
- ComparePoints(constraints.init_acc, cf3.derivate(0.,2), errMsg, error);
- ComparePoints(constraints.end_vel, cf3.derivate(1.,1), errMsg, error);
- ComparePoints(constraints.end_acc, cf3.derivate(1.,2), errMsg, error);
-}
-
-
-void BezierToPolynomConversionTest(bool& error)
-{
- std::string errMsg("In test BezierToPolynomConversionTest ; unexpected result for x ");
- point_t a(1,2,3);
- point_t b(2,3,4);
- point_t c(3,4,5);
- point_t d(3,6,7);
- point_t e(3,61,7);
- point_t f(3,56,7);
- point_t g(3,36,7);
- point_t h(43,6,7);
- point_t i(3,6,77);
-
- std::vector<point_t> params;
- params.push_back(a);
- params.push_back(b);
- params.push_back(c);
- params.push_back(d);
- params.push_back(e);
- params.push_back(f);
- params.push_back(g);
- params.push_back(h);
- params.push_back(i);
-
- bezier_curve_t cf(params.begin(), params.end());
- polynom_t pol =from_bezier<bezier_curve_t, polynom_t>(cf);
- for(double i =0.; i<1.; i+=0.01)
- {
- ComparePoints(cf(i),pol(i),errMsg, error, true);
- ComparePoints(cf(i),pol(i),errMsg, error, false);
- }
-}
-
-/*Exact Cubic Function tests*/
-void ExactCubicNoErrorTest(bool& error)
-{
- spline::T_Waypoint waypoints;
- for(double i = 0; i <= 1; i = i + 0.2)
- {
- waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
- }
- exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
- point_t res1;
- try
- {
- exactCubic(0);
- exactCubic(1);
- }
- catch(...)
- {
- error = true;
- std::cout << "Evaluation of ExactCubicNoErrorTest error\n";
- }
- error = true;
- try
- {
- exactCubic(1.2);
- }
- catch(...)
- {
- error = false;
- }
- if(error)
- {
- std::cout << "Evaluation of exactCubic cf error, 1.2 should be an out of range value\n";
- }
- if(exactCubic.max() != 1)
- {
- error = true;
- std::cout << "Evaluation of exactCubic error, MaxBound should be equal to 1\n";
- }
- if(exactCubic.min() != 0)
- {
- error = true;
- std::cout << "Evaluation of exactCubic error, MinBound should be equal to 1\n";
- }
-}
-
-
-/*Exact Cubic Function tests*/
-void ExactCubicTwoPointsTest(bool& error)
-{
- spline::T_Waypoint waypoints;
- for(double i = 0; i < 2; ++i)
- {
- waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
- }
- exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
-
- point_t res1 = exactCubic(0);
- std::string errmsg("in ExactCubic 2 points Error While checking that given wayPoints are crossed (expected / obtained)");
- ComparePoints(point_t(0,0,0), res1, errmsg, error);
-
- res1 = exactCubic(1);
- ComparePoints(point_t(1,1,1), res1, errmsg, error);
-}
-
-void ExactCubicOneDimTest(bool& error)
-{
- spline::T_WaypointOne waypoints;
- point_one zero; zero(0,0) = 9;
- point_one one; one(0,0) = 14;
- point_one two; two(0,0) = 25;
- waypoints.push_back(std::make_pair(0., zero));
- waypoints.push_back(std::make_pair(1., one));
- waypoints.push_back(std::make_pair(2., two));
- exact_cubic_one exactCubic(waypoints.begin(), waypoints.end());
-
- point_one res1 = exactCubic(0);
- std::string errmsg("in ExactCubicOneDim Error While checking that given wayPoints are crossed (expected / obtained)");
- ComparePoints(zero, res1, errmsg, error);
-
- res1 = exactCubic(1);
- ComparePoints(one, res1, errmsg, error);
-}
-
-void CheckWayPointConstraint(const std::string& errmsg, const double step, const spline::T_Waypoint& /*wayPoints*/, const exact_cubic_t* curve, bool& error )
-{
- point_t res1;
- for(double i = 0; i <= 1; i = i + step)
- {
- res1 = (*curve)(i);
- ComparePoints(point_t(i,i,i), res1, errmsg, error);
- }
-}
-
-void CheckDerivative(const std::string& errmsg, const double eval_point, const std::size_t order, const point_t& target, const exact_cubic_t* curve, bool& error )
-{
- point_t res1 = curve->derivate(eval_point,order);
- ComparePoints(target, res1, errmsg, error);
-}
-
-
-void ExactCubicPointsCrossedTest(bool& error)
-{
- spline::T_Waypoint waypoints;
- for(double i = 0; i <= 1; i = i + 0.2)
- {
- waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
- }
- exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
- std::string errmsg("Error While checking that given wayPoints are crossed (expected / obtained)");
- CheckWayPointConstraint(errmsg, 0.2, waypoints, &exactCubic, error);
-
-}
-
-void ExactCubicVelocityConstraintsTest(bool& error)
-{
- spline::T_Waypoint waypoints;
- for(double i = 0; i <= 1; i = i + 0.2)
- {
- waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
- }
- std::string errmsg("Error in ExactCubicVelocityConstraintsTest (1); while checking that given wayPoints are crossed (expected / obtained)");
- spline_constraints_t constraints;
- spline_deriv_constraint_t exactCubic(waypoints.begin(), waypoints.end());
- // now check that init and end velocity are 0
- CheckWayPointConstraint(errmsg, 0.2, waypoints, &exactCubic, error);
- std::string errmsg3("Error in ExactCubicVelocityConstraintsTest (2); while checking derivative (expected / obtained)");
- // now check derivatives
- CheckDerivative(errmsg3,0,1,constraints.init_vel,&exactCubic, error);
- CheckDerivative(errmsg3,1,1,constraints.end_vel,&exactCubic, error);
- CheckDerivative(errmsg3,0,2,constraints.init_acc,&exactCubic, error);
- CheckDerivative(errmsg3,1,2,constraints.end_acc,&exactCubic, error);
-
- constraints.end_vel = point_t(1,2,3);
- constraints.init_vel = point_t(-1,-2,-3);
- constraints.end_acc = point_t(4,5,6);
- constraints.init_acc = point_t(-4,-4,-6);
- std::string errmsg2("Error in ExactCubicVelocityConstraintsTest (3); while checking that given wayPoints are crossed (expected / obtained)");
- spline_deriv_constraint_t exactCubic2(waypoints.begin(), waypoints.end(),constraints);
- CheckWayPointConstraint(errmsg2, 0.2, waypoints, &exactCubic2, error);
-
- std::string errmsg4("Error in ExactCubicVelocityConstraintsTest (4); while checking derivative (expected / obtained)");
- // now check derivatives
- CheckDerivative(errmsg4,0,1,constraints.init_vel,&exactCubic2, error);
- CheckDerivative(errmsg4,1,1,constraints.end_vel ,&exactCubic2, error);
- CheckDerivative(errmsg4,0,2,constraints.init_acc,&exactCubic2, error);
- CheckDerivative(errmsg4,1,2,constraints.end_acc ,&exactCubic2, error);
-}
-
-void CheckPointOnline(const std::string& errmsg, const point_t& A, const point_t& B, const double target, const exact_cubic_t* curve, bool& error )
-{
- point_t res1 = curve->operator ()(target);
- point_t ar =(res1-A); ar.normalize();
- point_t rb =(B-res1); rb.normalize();
- if(ar.dot(rb) < 0.99999)
- {
- error = true;
- std::cout << errmsg << " ; " << A.transpose() << "\n ; " << B.transpose() << "\n ; " <<
- target << " ; " << res1.transpose() << std::endl;
- }
-}
-
-void EffectorTrajectoryTest(bool& error)
-{
- // create arbitrary trajectory
- spline::T_Waypoint waypoints;
- for(double i = 0; i <= 10; i = i + 2)
- {
- waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
- }
- helpers::exact_cubic_t* eff_traj = helpers::effector_spline(waypoints.begin(),waypoints.end(),
- Eigen::Vector3d::UnitZ(),Eigen::Vector3d(0,0,2),
- 1,0.02,1,0.5);
- point_t zero(0,0,0);
- point_t off1(0,0,1);
- point_t off2(10,10,10.02);
- point_t end(10,10,10);
- std::string errmsg("Error in EffectorTrajectoryTest; while checking waypoints (expected / obtained)");
- std::string errmsg2("Error in EffectorTrajectoryTest; while checking derivative (expected / obtained)");
- //first check start / goal positions
- ComparePoints(zero, (*eff_traj)(0), errmsg, error);
- ComparePoints(off1, (*eff_traj)(1), errmsg, error);
- ComparePoints(off2, (*eff_traj)(9.5), errmsg, error);
- ComparePoints(end , (*eff_traj)(10), errmsg, error);
-
- //then check offset at start / goal positions
- // now check derivatives
- CheckDerivative(errmsg2,0,1,zero,eff_traj, error);
- CheckDerivative(errmsg2,10,1,zero ,eff_traj, error);
- CheckDerivative(errmsg2,0,2,zero,eff_traj, error);
- CheckDerivative(errmsg2,10,2,zero ,eff_traj, error);
-
- //check that end and init splines are line
- std::string errmsg3("Error in EffectorTrajectoryTest; while checking that init/end splines are line (point A/ point B, time value / point obtained) \n");
- for(double i = 0.1; i<1; i+=0.1)
- {
- CheckPointOnline(errmsg3,(*eff_traj)(0),(*eff_traj)(1),i,eff_traj,error);
- }
-
- for(double i = 9.981; i<10; i+=0.002)
- {
- CheckPointOnline(errmsg3,(*eff_traj)(9.5),(*eff_traj)(10),i,eff_traj,error);
- }
- delete eff_traj;
-}
-
-helpers::quat_t GetXRotQuat(const double theta)
-{
- Eigen::AngleAxisd m (theta, Eigen::Vector3d::UnitX());
- return helpers::quat_t(Eigen::Quaterniond(m).coeffs().data());
-}
-
-double GetXRotFromQuat(helpers::quat_ref_const_t q)
-{
- Eigen::Quaterniond quat (q.data());
- Eigen::AngleAxisd m (quat);
- return m.angle() / M_PI * 180.;
-}
-
-void EffectorSplineRotationNoRotationTest(bool& error)
-{
- // create arbitrary trajectory
- spline::T_Waypoint waypoints;
- for(double i = 0; i <= 10; i = i + 2)
- {
- waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
- }
- helpers::effector_spline_rotation eff_traj(waypoints.begin(),waypoints.end());
- helpers::config_t q_init; q_init << 0.,0.,0.,0.,0.,0.,1.;
- helpers::config_t q_end; q_end << 10.,10.,10.,0.,0.,0.,1.;
- helpers::config_t q_to; q_to << 0.,0,0.02,0.,0.,0.,1.;
- helpers::config_t q_land; q_land << 10,10, 10.02, 0, 0.,0.,1.;
- helpers::config_t q_mod; q_mod << 6.,6.,6.,0.,0.,0.,1.;
- std::string errmsg("Error in EffectorSplineRotationNoRotationTest; while checking waypoints (expected / obtained)");
- ComparePoints(q_init , eff_traj(0), errmsg,error);
- ComparePoints(q_to , eff_traj(0.02), errmsg,error);
- ComparePoints(q_land , eff_traj(9.98), errmsg,error);
- ComparePoints(q_mod , eff_traj(6), errmsg,error);
- ComparePoints(q_end , eff_traj(10), errmsg,error);
-}
-
-void EffectorSplineRotationRotationTest(bool& error)
-{
- // create arbitrary trajectory
- spline::T_Waypoint waypoints;
- for(double i = 0; i <= 10; i = i + 2)
- {
- waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
- }
- helpers::quat_t init_quat = GetXRotQuat(M_PI);
- helpers::effector_spline_rotation eff_traj(waypoints.begin(),waypoints.end(), init_quat);
- helpers::config_t q_init = helpers::config_t::Zero(); q_init.tail<4>() = init_quat;
- helpers::config_t q_end; q_end << 10.,10.,10.,0.,0.,0.,1.;
- helpers::config_t q_to = q_init; q_to(2) +=0.02;
- helpers::config_t q_land = q_end ; q_land(2)+=0.02;
- helpers::quat_t q_mod = GetXRotQuat(M_PI_2);;
- std::string errmsg("Error in EffectorSplineRotationRotationTest; while checking waypoints (expected / obtained)");
- ComparePoints(q_init, eff_traj(0), errmsg,error);
- ComparePoints(q_to , eff_traj(0.02), errmsg,error);
- ComparePoints(q_land, eff_traj(9.98), errmsg,error);
- ComparePoints(q_mod , eff_traj(5).tail<4>(), errmsg,error);
- ComparePoints(q_end , eff_traj(10), errmsg,error);
-}
-
-void EffectorSplineRotationWayPointRotationTest(bool& error)
-{
- // create arbitrary trajectory
- spline::T_Waypoint waypoints;
- for(double i = 0; i <= 10; i = i + 2)
- {
- waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
- }
- helpers::quat_t init_quat = GetXRotQuat(0);
- helpers::t_waypoint_quat_t quat_waypoints_;
-
-
- helpers::quat_t q_pi_0 = GetXRotQuat(0);
- helpers::quat_t q_pi_2 = GetXRotQuat(M_PI_2);
- helpers::quat_t q_pi = GetXRotQuat(M_PI);
-
- quat_waypoints_.push_back(std::make_pair(0.4,q_pi_0));
- quat_waypoints_.push_back(std::make_pair(6,q_pi_2));
- quat_waypoints_.push_back(std::make_pair(8,q_pi));
-
-
- helpers::effector_spline_rotation eff_traj(waypoints.begin(),waypoints.end(),
- quat_waypoints_.begin(), quat_waypoints_.end());
- helpers::config_t q_init = helpers::config_t::Zero(); q_init.tail<4>() = init_quat;
- helpers::config_t q_end; q_end << 10.,10.,10.,0.,0.,0.,1.; q_end.tail<4>() = q_pi;
- helpers::config_t q_mod; q_mod.head<3>() = point_t(6,6,6) ; q_mod.tail<4>() = q_pi_2;
- helpers::config_t q_to = q_init; q_to(2) +=0.02;
- helpers::config_t q_land = q_end ; q_land(2)+=0.02;
- std::string errmsg("Error in EffectorSplineRotationWayPointRotationTest; while checking waypoints (expected / obtained)");
- ComparePoints(q_init, eff_traj(0), errmsg,error);
- ComparePoints(q_to , eff_traj(0.02), errmsg,error);
- ComparePoints(q_land, eff_traj(9.98), errmsg,error);
- ComparePoints(q_mod , eff_traj(6), errmsg,error);
- ComparePoints(q_end , eff_traj(10), errmsg,error);
-}
-
-void TestReparametrization(bool& error)
-{
- helpers::rotation_spline s;
- const helpers::spline_deriv_constraint_one_dim& sp = s.time_reparam_;
- if(sp.min() != 0)
- {
- std::cout << "in TestReparametrization; min value is not 0, got " << sp.min() << std::endl;
- error = true;
- }
- if(sp.max() != 1)
- {
- std::cout << "in TestReparametrization; max value is not 1, got " << sp.max() << std::endl;
- error = true;
- }
- if(sp(1)[0] != 1.)
- {
- std::cout << "in TestReparametrization; end value is not 1, got " << sp(1)[0] << std::endl;
- error = true;
- }
- if(sp(0)[0] != 0.)
- {
- std::cout << "in TestReparametrization; init value is not 0, got " << sp(0)[0] << std::endl;
- error = true;
- }
- for(double i =0; i<1; i+=0.002)
- {
- if(sp(i)[0]>sp(i+0.002)[0])
- {
- std::cout << "in TestReparametrization; reparametrization not monotonous " << sp.max() << std::endl;
- error = true;
- }
- }
-}
-
-point_t randomPoint(const double min, const double max ){
- point_t p;
- for(size_t i = 0 ; i < 3 ; ++i)
- p[i] = (rand()/(double)RAND_MAX ) * (max-min) + min;
- return p;
-}
-
-void BezierEvalDeCasteljau(bool& error){
- using namespace std;
- std::vector<double> values;
- for (int i =0; i < 100000; ++i)
- values.push_back(rand()/RAND_MAX);
-
- //first compare regular evaluation (low dim pol)
- point_t a(1,2,3);
- point_t b(2,3,4);
- point_t c(3,4,5);
- point_t d(3,6,7);
- point_t e(3,61,7);
- point_t f(3,56,7);
- point_t g(3,36,7);
- point_t h(43,6,7);
- point_t i(3,6,77);
-
- std::vector<point_t> params;
- params.push_back(a);
- params.push_back(b);
- params.push_back(c);
-
- // 3d curve
- bezier_curve_t cf(params.begin(), params.end());
-
- for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
- {
- if(cf.evalDeCasteljau(*cit) != cf(*cit)){
- error = true;
- std::cout<<" De Casteljau evaluation did not return the same value as analytical"<<std::endl;
- }
- }
-
- params.push_back(d);
- params.push_back(e);
- params.push_back(f);
- params.push_back(g);
- params.push_back(h);
- params.push_back(i);
-
- bezier_curve_t cf2(params.begin(), params.end());
-
- for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
- {
- if(cf.evalDeCasteljau(*cit) != cf(*cit)){
- error = true;
- std::cout<<" De Casteljau evaluation did not return the same value as analytical"<<std::endl;
- }
- }
-
-}
-
-
-/**
- * @brief BezierSplitCurve test the 'split' method of bezier curve
- * @param error
- */
-void BezierSplitCurve(bool& error){
- // test for degree 5
- int n = 5;
- double t_min = 0.2;
- double t_max = 10;
- for(size_t i = 0 ; i < 1 ; ++i){
- // build a random curve and split it at random time :
- //std::cout<<"build a random curve"<<std::endl;
- point_t a;
- std::vector<point_t> wps;
- for(size_t j = 0 ; j <= n ; ++j){
- wps.push_back(randomPoint(-10.,10.));
- }
- double t = (rand()/(double)RAND_MAX )*(t_max-t_min) + t_min;
- double ts = (rand()/(double)RAND_MAX )*(t);
-
- bezier_curve_t c(wps.begin(), wps.end(),t);
- std::pair<bezier_curve_t,bezier_curve_t> cs = c.split(ts);
- //std::cout<<"split curve of duration "<<t<<" at "<<ts<<std::endl;
-
- // test on splitted curves :
-
- if(! ((c.degree_ == cs.first.degree_) && (c.degree_ == cs.second.degree_) )){
- error = true;
- std::cout<<" Degree of the splitted curve are not the same as the original curve"<<std::endl;
- }
-
- if(c.max() != (cs.first.max() + cs.second.max())){
- error = true;
- std::cout<<"Duration of the splitted curve doesn't correspond to the original"<<std::endl;
- }
-
- if(c(0) != cs.first(0)){
- error = true;
- std::cout<<"initial point of the splitted curve doesn't correspond to the original"<<std::endl;
- }
-
- if(c(t) != cs.second(cs.second.max())){
- error = true;
- std::cout<<"final point of the splitted curve doesn't correspond to the original"<<std::endl;
- }
-
- if(cs.first.max() != ts){
- error = true;
- std::cout<<"timing of the splitted curve doesn't correspond to the original"<<std::endl;
- }
-
- if(cs.first(ts) != cs.second(0.)){
- error = true;
- std::cout<<"splitting point of the splitted curve doesn't correspond to the original"<<std::endl;
- }
-
- // check along curve :
- double ti = 0.;
- while(ti <= ts){
- if((cs.first(ti) - c(ti)).norm() > 1e-14){
- error = true;
- std::cout<<"first splitted curve and original curve doesn't correspond, error = "<<cs.first(ti) - c(ti) <<std::endl;
- }
- ti += 0.01;
- }
- while(ti <= t){
- if((cs.second(ti-ts) - c(ti)).norm() > 1e-14){
- error = true;
- std::cout<<"second splitted curve and original curve doesn't correspond, error = "<<cs.second(ti-ts) - c(ti)<<std::endl;
- }
- ti += 0.01;
- }
- }
-}
-
-
-
-int main(int /*argc*/, char** /*argv[]*/)
-{
- std::cout << "performing tests... \n";
- bool error = false;
- CubicFunctionTest(error);
- ExactCubicNoErrorTest(error);
- ExactCubicPointsCrossedTest(error); // checks that given wayPoints are crossed
- ExactCubicTwoPointsTest(error);
- ExactCubicOneDimTest(error);
- ExactCubicVelocityConstraintsTest(error);
- EffectorTrajectoryTest(error);
- EffectorSplineRotationNoRotationTest(error);
- EffectorSplineRotationRotationTest(error);
- TestReparametrization(error);
- EffectorSplineRotationWayPointRotationTest(error);
- BezierCurveTest(error);
- BezierDerivativeCurveTest(error);
- BezierDerivativeCurveConstraintTest(error);
- BezierCurveTestCompareHornerAndBernstein(error);
- BezierDerivativeCurveTimeReparametrizationTest(error);
- BezierToPolynomConversionTest(error);
- BezierEvalDeCasteljau(error);
- BezierSplitCurve(error);
- if(error)
- {
- std::cout << "There were some errors\n";
- return -1;
- }
- else
- {
- std::cout << "no errors found \n";
- return 0;
- }
-}
-
+
+#include "hpp/spline/exact_cubic.h"
+#include "hpp/spline/bezier_curve.h"
+#include "hpp/spline/polynom.h"
+#include "hpp/spline/spline_deriv_constraint.h"
+#include "hpp/spline/helpers/effector_spline.h"
+#include "hpp/spline/helpers/effector_spline_rotation.h"
+#include "hpp/spline/bezier_polynom_conversion.h"
+
+#include <string>
+#include <iostream>
+#include <cmath>
+
+using namespace std;
+
+namespace spline
+{
+typedef Eigen::Vector3d point_t;
+typedef std::vector<point_t,Eigen::aligned_allocator<point_t> > t_point_t;
+typedef polynom <double, double, 3, true, point_t, t_point_t> polynom_t;
+typedef exact_cubic <double, double, 3, true, point_t> exact_cubic_t;
+typedef spline_deriv_constraint <double, double, 3, true, point_t> spline_deriv_constraint_t;
+typedef bezier_curve <double, double, 3, true, point_t> bezier_curve_t;
+typedef spline_deriv_constraint_t::spline_constraints spline_constraints_t;
+typedef std::pair<double, point_t> Waypoint;
+typedef std::vector<Waypoint> T_Waypoint;
+
+
+typedef Eigen::Matrix<double,1,1> point_one;
+typedef polynom<double, double, 1, true, point_one> polynom_one;
+typedef exact_cubic <double, double, 1, true, point_one> exact_cubic_one;
+typedef std::pair<double, point_one> WaypointOne;
+typedef std::vector<WaypointOne> T_WaypointOne;
+
+bool QuasiEqual(const double a, const double b, const float margin)
+{
+ if ((a <= 0 && b <= 0) || (a >= 0 && b>= 0))
+ {
+ return (abs(a-b)) <= margin;
+ }
+ else
+ {
+ return abs(a) + abs(b) <= margin;
+ }
+}
+
+const double margin = 0.001;
+
+} // namespace spline
+
+using namespace spline;
+
+ostream& operator<<(ostream& os, const point_t& pt)
+{
+ os << "(" << pt.x() << ", " << pt.y() << ", " << pt.z() << ")";
+ return os;
+}
+
+void ComparePoints(const Eigen::VectorXd& pt1, const Eigen::VectorXd& pt2, const std::string& errmsg, bool& error, bool notequal = false)
+{
+ if((pt1-pt2).norm() > margin && !notequal)
+ {
+ error = true;
+ std::cout << errmsg << pt1.transpose() << " ; " << pt2.transpose() << std::endl;
+ }
+}
+
+/*Cubic Function tests*/
+
+void CubicFunctionTest(bool& error)
+{
+ std::string errMsg("In test CubicFunctionTest ; unexpected result for x ");
+ point_t a(1,2,3);
+ point_t b(2,3,4);
+ point_t c(3,4,5);
+ point_t d(3,6,7);
+ t_point_t vec;
+ vec.push_back(a);
+ vec.push_back(b);
+ vec.push_back(c);
+ vec.push_back(d);
+ polynom_t cf(vec.begin(), vec.end(), 0, 1);
+ point_t res1;
+ res1 =cf(0);
+ point_t x0(1,2,3);
+ ComparePoints(x0, res1, errMsg + "(0) ", error);
+
+ point_t x1(9,15,19);
+ res1 =cf(1);
+ ComparePoints(x1, res1, errMsg + "(1) ", error);
+
+ point_t x2(3.125,5.25,7.125);
+ res1 =cf(0.5);
+ ComparePoints(x2, res1, errMsg + "(0.5) ", error);
+
+ vec.clear();
+ vec.push_back(a);
+ vec.push_back(b);
+ vec.push_back(c);
+ vec.push_back(d);
+ polynom_t cf2(vec, 0.5, 1);
+ res1 = cf2(0.5);
+ ComparePoints(x0, res1, errMsg + "x3 ", error);
+ error = true;
+ try
+ {
+ cf2(0.4);
+ }
+ catch(...)
+ {
+ error = false;
+ }
+ if(error)
+ {
+ std::cout << "Evaluation of cubic cf2 error, 0.4 should be an out of range value\n";
+ }
+ error = true;
+ try
+ {
+ cf2(1.1);
+ }
+ catch(...)
+ {
+ error = false;
+ }
+ if(error)
+ {
+ std::cout << "Evaluation of cubic cf2 error, 1.1 should be an out of range value\n";
+ }
+ if(cf.max() != 1)
+ {
+ error = true;
+ std::cout << "Evaluation of exactCubic error, MaxBound should be equal to 1\n";
+ }
+ if(cf.min() != 0)
+ {
+ error = true;
+ std::cout << "Evaluation of exactCubic error, MinBound should be equal to 1\n";
+ }
+}
+
+/*bezier_curve Function tests*/
+
+void BezierCurveTest(bool& error)
+{
+ std::string errMsg("In test BezierCurveTest ; unexpected result for x ");
+ point_t a(1,2,3);
+ point_t b(2,3,4);
+ point_t c(3,4,5);
+ point_t d(3,6,7);
+
+ std::vector<point_t> params;
+ params.push_back(a);
+ params.push_back(b);
+
+ // 2d curve
+ bezier_curve_t cf(params.begin(), params.end());
+ point_t res1;
+ res1 = cf(0);
+ point_t x20 = a ;
+ ComparePoints(x20, res1, errMsg + "2(0) ", error);
+
+ point_t x21 = b;
+ res1 = cf(1);
+ ComparePoints(x21, res1, errMsg + "2(1) ", error);
+
+ //3d curve
+ params.push_back(c);
+ bezier_curve_t cf3(params.begin(), params.end());
+ res1 = cf3(0);
+ ComparePoints(a, res1, errMsg + "3(0) ", error);
+
+ res1 = cf3(1);
+ ComparePoints(c, res1, errMsg + "3(1) ", error);
+
+ //4d curve
+ params.push_back(d);
+ bezier_curve_t cf4(params.begin(), params.end(), 2);
+
+ res1 = cf4(2);
+ ComparePoints(d, res1, errMsg + "3(1) ", error);
+
+ //testing bernstein polynomes
+ bezier_curve_t cf5(params.begin(), params.end(),2.);
+ std::string errMsg2("In test BezierCurveTest ; Bernstein polynoms do not evaluate as analytical evaluation");
+ for(double d = 0.; d <2.; d+=0.1)
+ {
+ ComparePoints( cf5.evalBernstein(d) , cf5 (d), errMsg2, error);
+ ComparePoints( cf5.evalHorner(d) , cf5 (d), errMsg2, error);
+ ComparePoints( cf5.compute_derivate(1).evalBernstein(d) , cf5.compute_derivate(1) (d), errMsg2, error);
+ ComparePoints( cf5.compute_derivate(1).evalHorner(d) , cf5.compute_derivate(1) (d), errMsg2, error);
+ }
+
+ bool error_in(true);
+ try
+ {
+ cf(-0.4);
+ }
+ catch(...)
+ {
+ error_in = false;
+ }
+ if(error_in)
+ {
+ std::cout << "Evaluation of bezier cf error, -0.4 should be an out of range value\n";
+ error = true;
+ }
+ error_in = true;
+ try
+ {
+ cf(1.1);
+ }
+ catch(...)
+ {
+ error_in = false;
+ }
+ if(error_in)
+ {
+ std::cout << "Evaluation of bezier cf error, 1.1 should be an out of range value\n";
+ error = true;
+ }
+ if(cf.max() != 1)
+ {
+ error = true;
+ std::cout << "Evaluation of exactCubic error, MaxBound should be equal to 1\n";
+ }
+ if(cf.min() != 0)
+ {
+ error = true;
+ std::cout << "Evaluation of exactCubic error, MinBound should be equal to 1\n";
+ }
+}
+
+#include <ctime>
+void BezierCurveTestCompareHornerAndBernstein(bool& /*error*/)
+{
+ using namespace std;
+ std::vector<double> values;
+ for (int i =0; i < 100000; ++i)
+ values.push_back(rand()/RAND_MAX);
+
+ //first compare regular evaluation (low dim pol)
+ point_t a(1,2,3);
+ point_t b(2,3,4);
+ point_t c(3,4,5);
+ point_t d(3,6,7);
+ point_t e(3,61,7);
+ point_t f(3,56,7);
+ point_t g(3,36,7);
+ point_t h(43,6,7);
+ point_t i(3,6,77);
+
+ std::vector<point_t> params;
+ params.push_back(a);
+ params.push_back(b);
+ params.push_back(c);
+
+ // 3d curve
+ bezier_curve_t cf(params.begin(), params.end());
+
+ clock_t s0,e0,s1,e1,s2,e2,s3,e3;
+ s0 = clock();
+ for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
+ {
+ cf(*cit);
+ }
+ e0 = clock();
+
+ s1 = clock();
+ for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
+ {
+ cf.evalBernstein(*cit);
+ }
+ e1 = clock();
+
+ s2 = clock();
+ for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
+ {
+ cf.evalHorner(*cit);
+ }
+ e2 = clock();
+
+ s3 = clock();
+ for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
+ {
+ cf.evalDeCasteljau(*cit);
+ }
+ e3 = clock();
+
+
+ std::cout << "time for analytical eval " << double(e0 - s0) / CLOCKS_PER_SEC << std::endl;
+ std::cout << "time for bernstein eval " << double(e1 - s1) / CLOCKS_PER_SEC << std::endl;
+ std::cout << "time for horner eval " << double(e2 - s2) / CLOCKS_PER_SEC << std::endl;
+ std::cout << "time for deCasteljau eval " << double(e3 - s3) / CLOCKS_PER_SEC << std::endl;
+
+ std::cout << "now with high order polynom " << std::endl;
+
+
+ params.push_back(d);
+ params.push_back(e);
+ params.push_back(f);
+ params.push_back(g);
+ params.push_back(h);
+ params.push_back(i);
+
+ bezier_curve_t cf2(params.begin(), params.end());
+
+ s1 = clock();
+ for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
+ {
+ cf2.evalBernstein(*cit);
+ }
+ e1 = clock();
+
+ s2 = clock();
+ for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
+ {
+ cf2.evalHorner(*cit);
+ }
+ e2 = clock();
+
+ s0 = clock();
+ for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
+ {
+ cf2(*cit);
+ }
+ e0 = clock();
+
+ s3 = clock();
+ for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
+ {
+ cf2.evalDeCasteljau(*cit);
+ }
+ e3 = clock();
+
+
+
+ std::cout << "time for analytical eval " << double(e0 - s0) / CLOCKS_PER_SEC << std::endl;
+ std::cout << "time for bernstein eval " << double(e1 - s1) / CLOCKS_PER_SEC << std::endl;
+ std::cout << "time for horner eval " << double(e2 - s2) / CLOCKS_PER_SEC << std::endl;
+ std::cout << "time for deCasteljau eval " << double(e3 - s3) / CLOCKS_PER_SEC << std::endl;
+
+}
+
+void BezierDerivativeCurveTest(bool& error)
+{
+ std::string errMsg("In test BezierDerivativeCurveTest ; unexpected result for x ");
+ point_t a(1,2,3);
+ point_t b(2,3,4);
+ point_t c(3,4,5);
+
+ std::vector<point_t> params;
+ params.push_back(a);
+ params.push_back(b);
+
+ params.push_back(c);
+ bezier_curve_t cf3(params.begin(), params.end());
+
+ ComparePoints(cf3(0), cf3.derivate(0.,0), errMsg, error);
+ ComparePoints(cf3(0), cf3.derivate(0.,1), errMsg, error, true);
+ ComparePoints(point_t::Zero(), cf3.derivate(0.,100), errMsg, error);
+}
+
+void BezierDerivativeCurveTimeReparametrizationTest(bool& error)
+{
+ std::string errMsg("In test BezierDerivativeCurveTimeReparametrizationTest ; unexpected result for x ");
+ point_t a(1,2,3);
+ point_t b(2,3,4);
+ point_t c(3,4,5);
+ point_t d(3,4,5);
+ point_t e(3,4,5);
+ point_t f(3,4,5);
+
+ std::vector<point_t> params;
+ params.push_back(a);
+ params.push_back(b);
+ params.push_back(c);
+ params.push_back(d);
+ params.push_back(e);
+ params.push_back(f);
+ double T = 2.;
+ bezier_curve_t cf(params.begin(), params.end());
+ bezier_curve_t cfT(params.begin(), params.end(),T);
+
+ ComparePoints(cf(0.5), cfT(1), errMsg, error);
+ ComparePoints(cf.derivate(0.5,1), cfT.derivate(1,1) * T, errMsg, error);
+ ComparePoints(cf.derivate(0.5,2), cfT.derivate(1,2) * T*T, errMsg, error);
+}
+
+void BezierDerivativeCurveConstraintTest(bool& error)
+{
+ std::string errMsg("In test BezierDerivativeCurveConstraintTest ; unexpected result for x ");
+ point_t a(1,2,3);
+ point_t b(2,3,4);
+ point_t c(3,4,5);
+
+ bezier_curve_t::curve_constraints_t constraints;
+ constraints.init_vel = point_t(-1,-1,-1);
+ constraints.init_acc = point_t(-2,-2,-2);
+ constraints.end_vel = point_t(-10,-10,-10);
+ constraints.end_acc = point_t(-20,-20,-20);
+
+ std::vector<point_t> params;
+ params.push_back(a);
+ params.push_back(b);
+
+ params.push_back(c);
+ bezier_curve_t cf3(params.begin(), params.end(), constraints);
+
+ assert(cf3.degree_ == params.size() + 3);
+ assert(cf3.size_ == params.size() + 4);
+
+ ComparePoints(a, cf3(0), errMsg, error);
+ ComparePoints(c, cf3(1), errMsg, error);
+ ComparePoints(constraints.init_vel, cf3.derivate(0.,1), errMsg, error);
+ ComparePoints(constraints.end_vel , cf3.derivate(1.,1), errMsg, error);
+ ComparePoints(constraints.init_acc, cf3.derivate(0.,2), errMsg, error);
+ ComparePoints(constraints.end_vel, cf3.derivate(1.,1), errMsg, error);
+ ComparePoints(constraints.end_acc, cf3.derivate(1.,2), errMsg, error);
+}
+
+
+void BezierToPolynomConversionTest(bool& error)
+{
+ std::string errMsg("In test BezierToPolynomConversionTest ; unexpected result for x ");
+ point_t a(1,2,3);
+ point_t b(2,3,4);
+ point_t c(3,4,5);
+ point_t d(3,6,7);
+ point_t e(3,61,7);
+ point_t f(3,56,7);
+ point_t g(3,36,7);
+ point_t h(43,6,7);
+ point_t i(3,6,77);
+
+ std::vector<point_t> params;
+ params.push_back(a);
+ params.push_back(b);
+ params.push_back(c);
+ params.push_back(d);
+ params.push_back(e);
+ params.push_back(f);
+ params.push_back(g);
+ params.push_back(h);
+ params.push_back(i);
+
+ bezier_curve_t cf(params.begin(), params.end());
+ polynom_t pol =from_bezier<bezier_curve_t, polynom_t>(cf);
+ for(double i =0.; i<1.; i+=0.01)
+ {
+ ComparePoints(cf(i),pol(i),errMsg, error, true);
+ ComparePoints(cf(i),pol(i),errMsg, error, false);
+ }
+}
+
+/*Exact Cubic Function tests*/
+void ExactCubicNoErrorTest(bool& error)
+{
+ spline::T_Waypoint waypoints;
+ for(double i = 0; i <= 1; i = i + 0.2)
+ {
+ waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
+ }
+ exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
+ point_t res1;
+ try
+ {
+ exactCubic(0);
+ exactCubic(1);
+ }
+ catch(...)
+ {
+ error = true;
+ std::cout << "Evaluation of ExactCubicNoErrorTest error\n";
+ }
+ error = true;
+ try
+ {
+ exactCubic(1.2);
+ }
+ catch(...)
+ {
+ error = false;
+ }
+ if(error)
+ {
+ std::cout << "Evaluation of exactCubic cf error, 1.2 should be an out of range value\n";
+ }
+ if(exactCubic.max() != 1)
+ {
+ error = true;
+ std::cout << "Evaluation of exactCubic error, MaxBound should be equal to 1\n";
+ }
+ if(exactCubic.min() != 0)
+ {
+ error = true;
+ std::cout << "Evaluation of exactCubic error, MinBound should be equal to 1\n";
+ }
+}
+
+
+/*Exact Cubic Function tests*/
+void ExactCubicTwoPointsTest(bool& error)
+{
+ spline::T_Waypoint waypoints;
+ for(double i = 0; i < 2; ++i)
+ {
+ waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
+ }
+ exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
+
+ point_t res1 = exactCubic(0);
+ std::string errmsg("in ExactCubic 2 points Error While checking that given wayPoints are crossed (expected / obtained)");
+ ComparePoints(point_t(0,0,0), res1, errmsg, error);
+
+ res1 = exactCubic(1);
+ ComparePoints(point_t(1,1,1), res1, errmsg, error);
+}
+
+void ExactCubicOneDimTest(bool& error)
+{
+ spline::T_WaypointOne waypoints;
+ point_one zero; zero(0,0) = 9;
+ point_one one; one(0,0) = 14;
+ point_one two; two(0,0) = 25;
+ waypoints.push_back(std::make_pair(0., zero));
+ waypoints.push_back(std::make_pair(1., one));
+ waypoints.push_back(std::make_pair(2., two));
+ exact_cubic_one exactCubic(waypoints.begin(), waypoints.end());
+
+ point_one res1 = exactCubic(0);
+ std::string errmsg("in ExactCubicOneDim Error While checking that given wayPoints are crossed (expected / obtained)");
+ ComparePoints(zero, res1, errmsg, error);
+
+ res1 = exactCubic(1);
+ ComparePoints(one, res1, errmsg, error);
+}
+
+void CheckWayPointConstraint(const std::string& errmsg, const double step, const spline::T_Waypoint& /*wayPoints*/, const exact_cubic_t* curve, bool& error )
+{
+ point_t res1;
+ for(double i = 0; i <= 1; i = i + step)
+ {
+ res1 = (*curve)(i);
+ ComparePoints(point_t(i,i,i), res1, errmsg, error);
+ }
+}
+
+void CheckDerivative(const std::string& errmsg, const double eval_point, const std::size_t order, const point_t& target, const exact_cubic_t* curve, bool& error )
+{
+ point_t res1 = curve->derivate(eval_point,order);
+ ComparePoints(target, res1, errmsg, error);
+}
+
+
+void ExactCubicPointsCrossedTest(bool& error)
+{
+ spline::T_Waypoint waypoints;
+ for(double i = 0; i <= 1; i = i + 0.2)
+ {
+ waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
+ }
+ exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
+ std::string errmsg("Error While checking that given wayPoints are crossed (expected / obtained)");
+ CheckWayPointConstraint(errmsg, 0.2, waypoints, &exactCubic, error);
+
+}
+
+void ExactCubicVelocityConstraintsTest(bool& error)
+{
+ spline::T_Waypoint waypoints;
+ for(double i = 0; i <= 1; i = i + 0.2)
+ {
+ waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
+ }
+ std::string errmsg("Error in ExactCubicVelocityConstraintsTest (1); while checking that given wayPoints are crossed (expected / obtained)");
+ spline_constraints_t constraints;
+ spline_deriv_constraint_t exactCubic(waypoints.begin(), waypoints.end());
+ // now check that init and end velocity are 0
+ CheckWayPointConstraint(errmsg, 0.2, waypoints, &exactCubic, error);
+ std::string errmsg3("Error in ExactCubicVelocityConstraintsTest (2); while checking derivative (expected / obtained)");
+ // now check derivatives
+ CheckDerivative(errmsg3,0,1,constraints.init_vel,&exactCubic, error);
+ CheckDerivative(errmsg3,1,1,constraints.end_vel,&exactCubic, error);
+ CheckDerivative(errmsg3,0,2,constraints.init_acc,&exactCubic, error);
+ CheckDerivative(errmsg3,1,2,constraints.end_acc,&exactCubic, error);
+
+ constraints.end_vel = point_t(1,2,3);
+ constraints.init_vel = point_t(-1,-2,-3);
+ constraints.end_acc = point_t(4,5,6);
+ constraints.init_acc = point_t(-4,-4,-6);
+ std::string errmsg2("Error in ExactCubicVelocityConstraintsTest (3); while checking that given wayPoints are crossed (expected / obtained)");
+ spline_deriv_constraint_t exactCubic2(waypoints.begin(), waypoints.end(),constraints);
+ CheckWayPointConstraint(errmsg2, 0.2, waypoints, &exactCubic2, error);
+
+ std::string errmsg4("Error in ExactCubicVelocityConstraintsTest (4); while checking derivative (expected / obtained)");
+ // now check derivatives
+ CheckDerivative(errmsg4,0,1,constraints.init_vel,&exactCubic2, error);
+ CheckDerivative(errmsg4,1,1,constraints.end_vel ,&exactCubic2, error);
+ CheckDerivative(errmsg4,0,2,constraints.init_acc,&exactCubic2, error);
+ CheckDerivative(errmsg4,1,2,constraints.end_acc ,&exactCubic2, error);
+}
+
+void CheckPointOnline(const std::string& errmsg, const point_t& A, const point_t& B, const double target, const exact_cubic_t* curve, bool& error )
+{
+ point_t res1 = curve->operator ()(target);
+ point_t ar =(res1-A); ar.normalize();
+ point_t rb =(B-res1); rb.normalize();
+ if(ar.dot(rb) < 0.99999)
+ {
+ error = true;
+ std::cout << errmsg << " ; " << A.transpose() << "\n ; " << B.transpose() << "\n ; " <<
+ target << " ; " << res1.transpose() << std::endl;
+ }
+}
+
+void EffectorTrajectoryTest(bool& error)
+{
+ // create arbitrary trajectory
+ spline::T_Waypoint waypoints;
+ for(double i = 0; i <= 10; i = i + 2)
+ {
+ waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
+ }
+ helpers::exact_cubic_t* eff_traj = helpers::effector_spline(waypoints.begin(),waypoints.end(),
+ Eigen::Vector3d::UnitZ(),Eigen::Vector3d(0,0,2),
+ 1,0.02,1,0.5);
+ point_t zero(0,0,0);
+ point_t off1(0,0,1);
+ point_t off2(10,10,10.02);
+ point_t end(10,10,10);
+ std::string errmsg("Error in EffectorTrajectoryTest; while checking waypoints (expected / obtained)");
+ std::string errmsg2("Error in EffectorTrajectoryTest; while checking derivative (expected / obtained)");
+ //first check start / goal positions
+ ComparePoints(zero, (*eff_traj)(0), errmsg, error);
+ ComparePoints(off1, (*eff_traj)(1), errmsg, error);
+ ComparePoints(off2, (*eff_traj)(9.5), errmsg, error);
+ ComparePoints(end , (*eff_traj)(10), errmsg, error);
+
+ //then check offset at start / goal positions
+ // now check derivatives
+ CheckDerivative(errmsg2,0,1,zero,eff_traj, error);
+ CheckDerivative(errmsg2,10,1,zero ,eff_traj, error);
+ CheckDerivative(errmsg2,0,2,zero,eff_traj, error);
+ CheckDerivative(errmsg2,10,2,zero ,eff_traj, error);
+
+ //check that end and init splines are line
+ std::string errmsg3("Error in EffectorTrajectoryTest; while checking that init/end splines are line (point A/ point B, time value / point obtained) \n");
+ for(double i = 0.1; i<1; i+=0.1)
+ {
+ CheckPointOnline(errmsg3,(*eff_traj)(0),(*eff_traj)(1),i,eff_traj,error);
+ }
+
+ for(double i = 9.981; i<10; i+=0.002)
+ {
+ CheckPointOnline(errmsg3,(*eff_traj)(9.5),(*eff_traj)(10),i,eff_traj,error);
+ }
+ delete eff_traj;
+}
+
+helpers::quat_t GetXRotQuat(const double theta)
+{
+ Eigen::AngleAxisd m (theta, Eigen::Vector3d::UnitX());
+ return helpers::quat_t(Eigen::Quaterniond(m).coeffs().data());
+}
+
+double GetXRotFromQuat(helpers::quat_ref_const_t q)
+{
+ Eigen::Quaterniond quat (q.data());
+ Eigen::AngleAxisd m (quat);
+ return m.angle() / M_PI * 180.;
+}
+
+void EffectorSplineRotationNoRotationTest(bool& error)
+{
+ // create arbitrary trajectory
+ spline::T_Waypoint waypoints;
+ for(double i = 0; i <= 10; i = i + 2)
+ {
+ waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
+ }
+ helpers::effector_spline_rotation eff_traj(waypoints.begin(),waypoints.end());
+ helpers::config_t q_init; q_init << 0.,0.,0.,0.,0.,0.,1.;
+ helpers::config_t q_end; q_end << 10.,10.,10.,0.,0.,0.,1.;
+ helpers::config_t q_to; q_to << 0.,0,0.02,0.,0.,0.,1.;
+ helpers::config_t q_land; q_land << 10,10, 10.02, 0, 0.,0.,1.;
+ helpers::config_t q_mod; q_mod << 6.,6.,6.,0.,0.,0.,1.;
+ std::string errmsg("Error in EffectorSplineRotationNoRotationTest; while checking waypoints (expected / obtained)");
+ ComparePoints(q_init , eff_traj(0), errmsg,error);
+ ComparePoints(q_to , eff_traj(0.02), errmsg,error);
+ ComparePoints(q_land , eff_traj(9.98), errmsg,error);
+ ComparePoints(q_mod , eff_traj(6), errmsg,error);
+ ComparePoints(q_end , eff_traj(10), errmsg,error);
+}
+
+void EffectorSplineRotationRotationTest(bool& error)
+{
+ // create arbitrary trajectory
+ spline::T_Waypoint waypoints;
+ for(double i = 0; i <= 10; i = i + 2)
+ {
+ waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
+ }
+ helpers::quat_t init_quat = GetXRotQuat(M_PI);
+ helpers::effector_spline_rotation eff_traj(waypoints.begin(),waypoints.end(), init_quat);
+ helpers::config_t q_init = helpers::config_t::Zero(); q_init.tail<4>() = init_quat;
+ helpers::config_t q_end; q_end << 10.,10.,10.,0.,0.,0.,1.;
+ helpers::config_t q_to = q_init; q_to(2) +=0.02;
+ helpers::config_t q_land = q_end ; q_land(2)+=0.02;
+ helpers::quat_t q_mod = GetXRotQuat(M_PI_2);;
+ std::string errmsg("Error in EffectorSplineRotationRotationTest; while checking waypoints (expected / obtained)");
+ ComparePoints(q_init, eff_traj(0), errmsg,error);
+ ComparePoints(q_to , eff_traj(0.02), errmsg,error);
+ ComparePoints(q_land, eff_traj(9.98), errmsg,error);
+ ComparePoints(q_mod , eff_traj(5).tail<4>(), errmsg,error);
+ ComparePoints(q_end , eff_traj(10), errmsg,error);
+}
+
+void EffectorSplineRotationWayPointRotationTest(bool& error)
+{
+ // create arbitrary trajectory
+ spline::T_Waypoint waypoints;
+ for(double i = 0; i <= 10; i = i + 2)
+ {
+ waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
+ }
+ helpers::quat_t init_quat = GetXRotQuat(0);
+ helpers::t_waypoint_quat_t quat_waypoints_;
+
+
+ helpers::quat_t q_pi_0 = GetXRotQuat(0);
+ helpers::quat_t q_pi_2 = GetXRotQuat(M_PI_2);
+ helpers::quat_t q_pi = GetXRotQuat(M_PI);
+
+ quat_waypoints_.push_back(std::make_pair(0.4,q_pi_0));
+ quat_waypoints_.push_back(std::make_pair(6,q_pi_2));
+ quat_waypoints_.push_back(std::make_pair(8,q_pi));
+
+
+ helpers::effector_spline_rotation eff_traj(waypoints.begin(),waypoints.end(),
+ quat_waypoints_.begin(), quat_waypoints_.end());
+ helpers::config_t q_init = helpers::config_t::Zero(); q_init.tail<4>() = init_quat;
+ helpers::config_t q_end; q_end << 10.,10.,10.,0.,0.,0.,1.; q_end.tail<4>() = q_pi;
+ helpers::config_t q_mod; q_mod.head<3>() = point_t(6,6,6) ; q_mod.tail<4>() = q_pi_2;
+ helpers::config_t q_to = q_init; q_to(2) +=0.02;
+ helpers::config_t q_land = q_end ; q_land(2)+=0.02;
+ std::string errmsg("Error in EffectorSplineRotationWayPointRotationTest; while checking waypoints (expected / obtained)");
+ ComparePoints(q_init, eff_traj(0), errmsg,error);
+ ComparePoints(q_to , eff_traj(0.02), errmsg,error);
+ ComparePoints(q_land, eff_traj(9.98), errmsg,error);
+ ComparePoints(q_mod , eff_traj(6), errmsg,error);
+ ComparePoints(q_end , eff_traj(10), errmsg,error);
+}
+
+void TestReparametrization(bool& error)
+{
+ helpers::rotation_spline s;
+ const helpers::spline_deriv_constraint_one_dim& sp = s.time_reparam_;
+ if(sp.min() != 0)
+ {
+ std::cout << "in TestReparametrization; min value is not 0, got " << sp.min() << std::endl;
+ error = true;
+ }
+ if(sp.max() != 1)
+ {
+ std::cout << "in TestReparametrization; max value is not 1, got " << sp.max() << std::endl;
+ error = true;
+ }
+ if(sp(1)[0] != 1.)
+ {
+ std::cout << "in TestReparametrization; end value is not 1, got " << sp(1)[0] << std::endl;
+ error = true;
+ }
+ if(sp(0)[0] != 0.)
+ {
+ std::cout << "in TestReparametrization; init value is not 0, got " << sp(0)[0] << std::endl;
+ error = true;
+ }
+ for(double i =0; i<1; i+=0.002)
+ {
+ if(sp(i)[0]>sp(i+0.002)[0])
+ {
+ std::cout << "in TestReparametrization; reparametrization not monotonous " << sp.max() << std::endl;
+ error = true;
+ }
+ }
+}
+
+point_t randomPoint(const double min, const double max ){
+ point_t p;
+ for(size_t i = 0 ; i < 3 ; ++i)
+ p[i] = (rand()/(double)RAND_MAX ) * (max-min) + min;
+ return p;
+}
+
+void BezierEvalDeCasteljau(bool& error){
+ using namespace std;
+ std::vector<double> values;
+ for (int i =0; i < 100000; ++i)
+ values.push_back(rand()/RAND_MAX);
+
+ //first compare regular evaluation (low dim pol)
+ point_t a(1,2,3);
+ point_t b(2,3,4);
+ point_t c(3,4,5);
+ point_t d(3,6,7);
+ point_t e(3,61,7);
+ point_t f(3,56,7);
+ point_t g(3,36,7);
+ point_t h(43,6,7);
+ point_t i(3,6,77);
+
+ std::vector<point_t> params;
+ params.push_back(a);
+ params.push_back(b);
+ params.push_back(c);
+
+ // 3d curve
+ bezier_curve_t cf(params.begin(), params.end());
+
+ for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
+ {
+ if(cf.evalDeCasteljau(*cit) != cf(*cit)){
+ error = true;
+ std::cout<<" De Casteljau evaluation did not return the same value as analytical"<<std::endl;
+ }
+ }
+
+ params.push_back(d);
+ params.push_back(e);
+ params.push_back(f);
+ params.push_back(g);
+ params.push_back(h);
+ params.push_back(i);
+
+ bezier_curve_t cf2(params.begin(), params.end());
+
+ for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
+ {
+ if(cf.evalDeCasteljau(*cit) != cf(*cit)){
+ error = true;
+ std::cout<<" De Casteljau evaluation did not return the same value as analytical"<<std::endl;
+ }
+ }
+
+}
+
+
+/**
+ * @brief BezierSplitCurve test the 'split' method of bezier curve
+ * @param error
+ */
+void BezierSplitCurve(bool& error){
+ // test for degree 5
+ int n = 5;
+ double t_min = 0.2;
+ double t_max = 10;
+ for(size_t i = 0 ; i < 1 ; ++i){
+ // build a random curve and split it at random time :
+ //std::cout<<"build a random curve"<<std::endl;
+ point_t a;
+ std::vector<point_t> wps;
+ for(size_t j = 0 ; j <= n ; ++j){
+ wps.push_back(randomPoint(-10.,10.));
+ }
+ double t = (rand()/(double)RAND_MAX )*(t_max-t_min) + t_min;
+ double ts = (rand()/(double)RAND_MAX )*(t);
+
+ bezier_curve_t c(wps.begin(), wps.end(),t);
+ std::pair<bezier_curve_t,bezier_curve_t> cs = c.split(ts);
+ //std::cout<<"split curve of duration "<<t<<" at "<<ts<<std::endl;
+
+ // test on splitted curves :
+
+ if(! ((c.degree_ == cs.first.degree_) && (c.degree_ == cs.second.degree_) )){
+ error = true;
+ std::cout<<" Degree of the splitted curve are not the same as the original curve"<<std::endl;
+ }
+
+ if(c.max() != (cs.first.max() + cs.second.max())){
+ error = true;
+ std::cout<<"Duration of the splitted curve doesn't correspond to the original"<<std::endl;
+ }
+
+ if(c(0) != cs.first(0)){
+ error = true;
+ std::cout<<"initial point of the splitted curve doesn't correspond to the original"<<std::endl;
+ }
+
+ if(c(t) != cs.second(cs.second.max())){
+ error = true;
+ std::cout<<"final point of the splitted curve doesn't correspond to the original"<<std::endl;
+ }
+
+ if(cs.first.max() != ts){
+ error = true;
+ std::cout<<"timing of the splitted curve doesn't correspond to the original"<<std::endl;
+ }
+
+ if(cs.first(ts) != cs.second(0.)){
+ error = true;
+ std::cout<<"splitting point of the splitted curve doesn't correspond to the original"<<std::endl;
+ }
+
+ // check along curve :
+ double ti = 0.;
+ while(ti <= ts){
+ if((cs.first(ti) - c(ti)).norm() > 1e-14){
+ error = true;
+ std::cout<<"first splitted curve and original curve doesn't correspond, error = "<<cs.first(ti) - c(ti) <<std::endl;
+ }
+ ti += 0.01;
+ }
+ while(ti <= t){
+ if((cs.second(ti-ts) - c(ti)).norm() > 1e-14){
+ error = true;
+ std::cout<<"second splitted curve and original curve doesn't correspond, error = "<<cs.second(ti-ts) - c(ti)<<std::endl;
+ }
+ ti += 0.01;
+ }
+ }
+}
+
+
+
+int main(int /*argc*/, char** /*argv[]*/)
+{
+ std::cout << "performing tests... \n";
+ bool error = false;
+ CubicFunctionTest(error);
+ ExactCubicNoErrorTest(error);
+ ExactCubicPointsCrossedTest(error); // checks that given wayPoints are crossed
+ ExactCubicTwoPointsTest(error);
+ ExactCubicOneDimTest(error);
+ ExactCubicVelocityConstraintsTest(error);
+ EffectorTrajectoryTest(error);
+ EffectorSplineRotationNoRotationTest(error);
+ EffectorSplineRotationRotationTest(error);
+ TestReparametrization(error);
+ EffectorSplineRotationWayPointRotationTest(error);
+ BezierCurveTest(error);
+ BezierDerivativeCurveTest(error);
+ BezierDerivativeCurveConstraintTest(error);
+ BezierCurveTestCompareHornerAndBernstein(error);
+ BezierDerivativeCurveTimeReparametrizationTest(error);
+ BezierToPolynomConversionTest(error);
+ BezierEvalDeCasteljau(error);
+ BezierSplitCurve(error);
+ if(error)
+ {
+ std::cout << "There were some errors\n";
+ return -1;
+ }
+ else
+ {
+ std::cout << "no errors found \n";
+ return 0;
+ }
+}