Unverified Commit 00c41527 authored by Fernbach Pierre's avatar Fernbach Pierre Committed by GitHub
Browse files

Merge pull request #41 from pFernbach/topic/fix_10

Add piecewise::load_from_text_file
parents 6bd5a1fc a6fa3e7f
......@@ -12,6 +12,8 @@
#include "curve_conversion.h"
#include <boost/smart_ptr/shared_ptr.hpp>
#include <boost/serialization/vector.hpp>
#include <fstream>
#include <sstream>
namespace curves {
/// \class PiecewiseCurve.
......@@ -362,6 +364,125 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point, Point_deri
return piecewise_res;
}
/**
* @brief load_piecewise_from_text_file build a piecewise polynomial from a list of discrete points read from a file.
* The file should contains one points per line, optionally with it's derivative and second derivatives.
* Each lines should then contains dim, 2*dim or 3*dim values
* @param filename the (absolute) name of the file to load
* @param dt the time step between each points in the file
* @param dim the dimension of the curve
* @return a piecewise curves containing polynomial connectiong all the points in the file
*/
template <typename Polynomial>
static piecewise_curve_t load_piecewise_from_text_file(const std::string& filename, const time_t dt, const size_t dim){
if(dim <= 0)
throw std::invalid_argument("The dimension should be strictly positive.");
if(dt <= 0.)
throw std::invalid_argument("The time step should be strictly positive.");
piecewise_curve_t piecewise_res;
std::ifstream file;
file.open(filename.c_str());
point_t last_pos = point_t::Zero(dim),
last_vel = point_t::Zero(dim),
last_acc = point_t::Zero(dim),
new_pos = point_t::Zero(dim),
new_vel = point_t::Zero(dim),
new_acc = point_t::Zero(dim);
bool use_vel, use_acc;
std::string line;
// read first line to found out if we use velocity / acceleration :
std::getline(file, line);
std::istringstream iss_length(line);
const size_t length = std::distance(std::istream_iterator<std::string>(iss_length), std::istream_iterator<std::string>());
if(length == dim){
use_vel = false;
use_acc = false;
}else if(length == dim * 2){
use_vel = true;
use_acc = false;
}else if(length == dim * 3){
use_vel = true;
use_acc = true;
}else{
std::stringstream error;
error<<"The first line of the file shold contains either " << dim << ", " << dim * 2 << " or " << dim * 3 <<
"values, got : " << length;
throw std::invalid_argument(error.str());
}
// initialize the first points of the trajectory:
num_t val;
std::istringstream iss(line);
for(size_t i = 0 ; i < dim ; ++i){
iss >> val;
last_pos[i] = val;
}
if(use_vel){
for(size_t i = 0 ; i < dim ; ++i){
iss >> val;
last_vel[i] = val;
}
}
if(use_acc){
for(size_t i = 0 ; i < dim ; ++i){
iss >> val;
last_acc[i] = val;
}
}
size_t current_length;
size_t line_id = 0;
// parse all lines of the file:
while (std::getline(file, line))
{
++line_id;
std::istringstream iss_length(line);
current_length = std::distance(std::istream_iterator<std::string>(iss_length), std::istream_iterator<std::string>());
if(current_length != length){
std::stringstream error;
error<<"Cannot parse line " << line_id << " got " << current_length << " values instead of " << length;
throw std::invalid_argument(error.str());
}
std::istringstream iss(line);
// parse the points values from the file:
for(size_t i = 0 ; i < dim ; ++i){
iss >> val;
new_pos[i] = val;
}
if(use_vel){
for(size_t i = 0 ; i < dim ; ++i){
iss >> val;
new_vel[i] = val;
}
}
if(use_acc){
for(size_t i = 0 ; i < dim ; ++i){
iss >> val;
new_acc[i] = val;
}
}
// append a new curves connectiong this points
if(use_acc){
piecewise_res.add_curve(Polynomial(last_pos, last_vel, last_acc,
new_pos, new_vel, new_acc,
dt * static_cast<time_t>(line_id - 1), dt * static_cast<time_t>(line_id)));
}else if(use_vel){
piecewise_res.add_curve(Polynomial(last_pos, last_vel,
new_pos, new_vel,
dt * static_cast<time_t>(line_id - 1), dt * static_cast<time_t>(line_id)));
}else{
piecewise_res.add_curve(Polynomial(last_pos, new_pos,
dt * static_cast<time_t>(line_id - 1), dt * static_cast<time_t>(line_id)));
}
last_pos = new_pos;
last_vel = new_vel;
last_acc = new_acc;
}
file.close();
return piecewise_res;
}
private:
/// \brief Get index of the interval corresponding to time t for the interpolation.
/// \param t : time where to look for interval.
......
......@@ -272,6 +272,9 @@ static piecewise_t discretPointToPolynomialC2(const pointX_list_t& points, const
return piecewise_t::convert_discrete_points_to_polynomial<polynomial_t>(
points_list, points_derivative_list, points_second_derivative_list, time_points_list);
}
static piecewise_t load_piecewise_from_text_file(const std::string& filename, const real dt, const std::size_t dim){
return piecewise_t::load_piecewise_from_text_file<polynomial_t>(filename, dt, dim);
}
void addFinalPointC0(piecewise_t& self, const pointX_t& end, const real time) {
......@@ -804,6 +807,11 @@ BOOST_PYTHON_MODULE(curves) {
"given points derivative and second derivative values. The created piecewise is C2 continuous.",
args("points", "points_derivative", "points_second_derivative", "time_points"))
.staticmethod("FromPointsList")
.def("FromPointsFile", &load_piecewise_from_text_file, args("filename", "dt", "dimension"),
"Create a piecewise-polynomial connecting exactly all the points in the given text file."
"The file should contains one points per line, optionally with it's derivative and second derivatives."
"Each lines should thus contains dim, 2*dim or 3*dim values")
.staticmethod("FromPointsFile")
.def("append", &addFinalPointC0,
"Append a new polynomial curve of degree 1 at the end of the piecewise curve, defined between self.max() "
"and time and connecting exactly self(self.max()) and end",
......
......@@ -1448,6 +1448,113 @@ void PiecewisePolynomialCurveFromDiscretePoints(bool& error) {
}
}
void PiecewisePolynomialCurveFromFile(bool& error){
std::string filename_pos(TEST_DATA_PATH "discrete_points_pos.txt");
std::string filename_vel(TEST_DATA_PATH "discrete_points_vel.txt");
std::string filename_acc(TEST_DATA_PATH "discrete_points_acc.txt");
std::string filename_error(TEST_DATA_PATH "discrete_points_error.txt");
piecewise_t c_pos = piecewise_t::load_piecewise_from_text_file<polynomial_t>(filename_pos, 0.01, 3);
if(c_pos.min() != 0.){
std::cout << "PiecewisePolynomialCurveFromFile, Error, t_min should be 0" << std::endl;
error = true;
}
if(c_pos.max() != 0.03){
std::cout << "PiecewisePolynomialCurveFromFile, Error, t_max should be 0.03" << std::endl;
error = true;
}
pointX_t p0(3), p2(3);
p0<<-0.003860389372941039, 0.0012353625242474164, 0.009005041639999767;
p2<<-0.0028803627898293283, 0.0011918668401150736, 0.009005041639999767;
if(! c_pos(0.).isApprox(p0)){
std::cout << "PiecewisePolynomialCurveFromFile, Error, points do not match" << std::endl;
error = true;
}
if(! c_pos(0.02).isApprox(p2)){
std::cout << "PiecewisePolynomialCurveFromFile, Error, points do not match" << std::endl;
error = true;
}
piecewise_t c_vel = piecewise_t::load_piecewise_from_text_file<polynomial_t>(filename_vel, 0.05, 3);
if(c_pos.min() != 0.){
std::cout << "PiecewisePolynomialCurveFromFile, Error, t_min should be 0" << std::endl;
error = true;
}
if(! QuasiEqual(c_vel.max(), 0.15)){
std::cout << "PiecewisePolynomialCurveFromFile, Error, t_max should be 0.15" << std::endl;
error = true;
}
pointX_t p3(3);
p3<<0.2968141884672718, 0.0012916907964522569, 0.00951023474821927;
if(! c_vel(0.).isApprox(p0)){
std::cout << "PiecewisePolynomialCurveFromFile, Error, points do not match" << std::endl;
error = true;
}
if(! c_vel(0.15).isApprox(p3)){
std::cout << "PiecewisePolynomialCurveFromFile, Error, points do not match" << std::endl;
error = true;
}
if(! c_vel.derivate(0., 1).isZero()){
std::cout << "PiecewisePolynomialCurveFromFile, Error, c_vel derivative at 0. should be null" << std::endl;
error = true;
}
if(! c_vel.derivate(0.1, 1).isZero()){
std::cout << "PiecewisePolynomialCurveFromFile, Error, c_vel derivative at 0.1 should be null" << std::endl;
error = true;
}
piecewise_t c_acc = piecewise_t::load_piecewise_from_text_file<polynomial_t>(filename_acc, 0.001, 3);
if(c_acc.min() != 0.){
std::cout << "PiecewisePolynomialCurveFromFile, Error, t_min should be 0" << std::endl;
error = true;
}
if(! QuasiEqual(c_acc.max(), 7.85)){
std::cout << "PiecewisePolynomialCurveFromFile, Error, t_max should be 7.85" << std::endl;
error = true;
}
if(! c_acc(0.).isApprox(p0)){
std::cout << "PiecewisePolynomialCurveFromFile, Error, points do not match" << std::endl;
error = true;
}
pointX_t p5200(3);
p5200<<0.30273356072723845, -0.07619420199174821, 0.010015348526727433;
if(! c_acc(5.2).isApprox(p5200)){
std::cout << "PiecewisePolynomialCurveFromFile, Error, points do not match" << std::endl;
error = true;
}
if(! c_acc.derivate(0., 1).isZero()){
std::cout << "PiecewisePolynomialCurveFromFile, Error, c_acc derivative at 0 should be null" << std::endl;
error = true;
}
if(! c_acc.derivate(0.5, 1).isZero()){
std::cout << "PiecewisePolynomialCurveFromFile, Error, c_acc derivative should at 0.5 be null" << std::endl;
error = true;
}
if(! c_acc.derivate(0., 2).isZero()){
std::cout << "PiecewisePolynomialCurveFromFile, Error, c_acc second derivative at 0 should be null" << std::endl;
error = true;
}
if(! c_acc.derivate(5., 2).isZero()){
std::cout << "PiecewisePolynomialCurveFromFile, Error, c_acc second derivative at 5 should be null" << std::endl;
error = true;
}
try {
piecewise_t c_error = piecewise_t::load_piecewise_from_text_file<polynomial_t>(filename_acc, 0.01, 4);
std::cout << "PiecewisePolynomialCurveFromFile, Error, dimension do not match, an error should be raised" << std::endl;
error = true;
} catch (std::invalid_argument e) {
}
try {
piecewise_t c_error = piecewise_t::load_piecewise_from_text_file<polynomial_t>(filename_error, 0.01, 3);
std::cout << "PiecewisePolynomialCurveFromFile, Error, discrete_points_error should not be parsed correctly" << std::endl;
error = true;
} catch (std::invalid_argument e) {
}
}
void serializationCurvesTest(bool& error) {
try {
std::string errMsg1("in serializationCurveTest, Error While serializing Polynomial : ");
......@@ -2769,6 +2876,7 @@ int main(int /*argc*/, char** /*argv[]*/) {
CubicHermitePairsPositionDerivativeTest(error);
piecewiseCurveTest(error);
PiecewisePolynomialCurveFromDiscretePoints(error);
PiecewisePolynomialCurveFromFile(error);
toPolynomialConversionTest(error);
cubicConversionTest(error);
curveAbcDimDynamicTest(error);
......
This diff is collapsed.
-0.003860389372941039 0.0012353625242474164 0.009005041639999767
-0.003367975609860228 0.0012135142149832263 0.009005041639999767
-0.0028803627898293283 0.0011918668401150736
-0.0023975258947521072 0.0011704190312931094 0.009005041639999767
-0.003860389372941039 0.0012353625242474164 0.009005041639999767
-0.003367975609860228 0.0012135142149832263 0.009005041639999767
-0.0028803627898293283 0.0011918668401150736 0.009005041639999767
-0.0023975258947521072 0.0011704190312931094 0.009005041639999767
-0.003860389372941039 0.0012353625242474164 0.009005041639999767 0 0 0
-0.003367975609860228 0.0012135142149832263 0.009005041639999767 0 0 0
0.2968095500861058 0.0013266763658670213 0.00951023474821927 0 0 0
0.2968141884672718 0.0012916907964522569 0.00951023474821927 0 0 0
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment