Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Guilhem Saurel
ndcurves
Commits
00c41527
Unverified
Commit
00c41527
authored
Jul 06, 2020
by
Fernbach Pierre
Committed by
GitHub
Jul 06, 2020
Browse files
Merge pull request #41 from pFernbach/topic/fix_10
Add piecewise::load_from_text_file
parents
6bd5a1fc
a6fa3e7f
Changes
7
Expand all
Hide whitespace changes
Inline
Side-by-side
include/curves/piecewise_curve.h
View file @
00c41527
...
...
@@ -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.
...
...
python/curves/curves_python.cpp
View file @
00c41527
...
...
@@ -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"
,
...
...
tests/Main.cpp
View file @
00c41527
...
...
@@ -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
);
...
...
tests/data/discrete_points_acc.txt
0 → 100644
View file @
00c41527
This diff is collapsed.
Click to expand it.
tests/data/discrete_points_error.txt
0 → 100644
View file @
00c41527
-0.003860389372941039 0.0012353625242474164 0.009005041639999767
-0.003367975609860228 0.0012135142149832263 0.009005041639999767
-0.0028803627898293283 0.0011918668401150736
-0.0023975258947521072 0.0011704190312931094 0.009005041639999767
tests/data/discrete_points_pos.txt
0 → 100644
View file @
00c41527
-0.003860389372941039 0.0012353625242474164 0.009005041639999767
-0.003367975609860228 0.0012135142149832263 0.009005041639999767
-0.0028803627898293283 0.0011918668401150736 0.009005041639999767
-0.0023975258947521072 0.0011704190312931094 0.009005041639999767
tests/data/discrete_points_vel.txt
0 → 100644
View file @
00c41527
-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
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment