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
Jason Chemin
curves
Commits
e9882b85
Commit
e9882b85
authored
Nov 28, 2016
by
Steve Tonneau
Browse files
start and end velocity constraints
parent
48067afc
Changes
6
Hide whitespace changes
Inline
Side-by-side
.gitignore
View file @
e9882b85
# Build and Release Folders
bin/
lib/
build/
# temp files
.*~
*.user
include/spline/curve_abc.h
View file @
e9882b85
...
...
@@ -42,7 +42,7 @@ struct curve_abc : std::unary_function<Time, Point>
/// \brief Evaluation of the cubic spline at time t.
/// \param t : the time when to evaluate the spine
/// \param return : the value x(t)
virtual
point_t
operator
()(
time_t
t
)
const
=
0
;
virtual
point_t
operator
()(
time_t
t
)
const
=
0
;
/*Operations*/
/*Helpers*/
...
...
include/spline/exact_cubic.h
View file @
e9882b85
...
...
@@ -59,10 +59,16 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
exact_cubic
(
In
wayPointsBegin
,
In
wayPointsEnd
)
:
curve_abc_t
(),
subSplines_
(
computeWayPoints
<
In
>
(
wayPointsBegin
,
wayPointsEnd
))
{}
///\brief Constructor
///\param subSplines: vector of subsplines
exact_cubic
(
const
t_spline_t
&
subSplines
)
:
curve_abc_t
(),
subSplines_
(
subSplines
)
{}
///\brief Destructor
~
exact_cubic
(){}
pr
otec
te
d
:
pr
iva
te:
template
<
typename
In
>
t_spline_t
computeWayPoints
(
In
wayPointsBegin
,
In
wayPointsEnd
)
const
{
...
...
@@ -158,7 +164,22 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
return
it
->
operator
()(
t
);
}
}
}
}
/// \brief Evaluation of the derivative spline at time t.
/// \param t : the time when to evaluate the spine
/// \param return : the value x(t)
virtual
point_t
derivate
(
time_t
t
,
std
::
size_t
order
)
const
{
if
(
Safe
&&
(
t
<
subSplines_
.
front
().
min
()
||
t
>
subSplines_
.
back
().
max
())){
throw
std
::
out_of_range
(
"TODO"
);}
for
(
cit_spline_t
it
=
subSplines_
.
begin
();
it
!=
subSplines_
.
end
();
++
it
)
{
if
(
t
>=
(
it
->
min
())
&&
t
<=
(
it
->
max
()))
{
return
it
->
derivate
(
t
,
order
);
}
}
}
/*Operations*/
/*Helpers*/
...
...
include/spline/exact_cubic_vel_acc_cons.h
View file @
e9882b85
...
...
@@ -39,7 +39,7 @@ namespace spline
template
<
typename
Time
=
double
,
typename
Numeric
=
Time
,
std
::
size_t
Dim
=
3
,
bool
Safe
=
false
,
typename
Point
=
Eigen
::
Matrix
<
Numeric
,
Dim
,
1
>,
typename
T_Point
=
std
::
vector
<
Point
,
Eigen
::
aligned_allocator
<
Point
>
>
>
struct
cubic_zero_vel
:
public
curve_ab
c
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
>
struct
cubic_zero_vel
:
public
exact_cubi
c
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
,
T_
Point
>
{
typedef
Point
point_t
;
typedef
T_Point
t_point_t
;
...
...
@@ -47,26 +47,65 @@ struct cubic_zero_vel : public curve_abc<Time, Numeric, Dim, Safe, Point>
typedef
Time
time_t
;
typedef
Numeric
num_t
;
typedef
spline_curve
<
time_t
,
Numeric
,
Dim
,
Safe
,
point_t
,
t_point_t
>
spline_t
;
typedef
curve_ab
c
<
T
ime
,
Numeric
,
Dim
,
Safe
,
P
oint
>
curve_ab
c_t
;
typedef
exact_cubi
c
<
t
ime
_t
,
Numeric
,
Dim
,
Safe
,
point_t
,
t_p
oint
_t
>
exact_cubi
c_t
;
typedef
typename
std
::
vector
<
spline_t
>
t_spline_t
;
typedef
typename
t_spline_t
::
iterator
it_spline_t
;
typedef
typename
t_spline_t
::
const_iterator
cit_spline_t
;
public:
struct
spline_constraints
{
spline_constraints
()
:
init_vel
(
point_t
::
Zero
()),
init_acc
(
init_vel
),
end_vel
(
init_vel
),
end_acc
(
init_vel
),
init_normal
(
init_vel
),
end_normal
(
init_vel
)
{}
spline_constraints
(
const
point_t
&
n0
,
point_t
&
n1
)
:
init_vel
(
point_t
::
Zero
()),
init_acc
(
init_vel
),
end_vel
(
init_vel
),
end_acc
(
init_vel
),
init_normal
(
n0
),
end_normal
(
n1
)
{}
~
spline_constraints
(){}
point_t
init_vel
;
point_t
init_acc
;
point_t
end_vel
;
point_t
end_acc
;
point_t
init_normal
;
point_t
end_normal
;
};
/* Constructors - destructors */
public:
///\brief Constructor
///\param wayPointsBegin : an iterator pointing to the first element of a waypoint container
///\param wayPointsEns : an iterator pointing to the end of a waypoint container
template
<
typename
In
>
cubic_zero_vel
(
In
wayPointsBegin
,
In
wayPointsEnd
)
:
curve_abc_t
(),
subSplines_
(
computeWayPoints
<
In
>
(
wayPointsBegin
,
wayPointsEnd
))
{}
template
<
typename
In
>
cubic_zero_vel
(
In
wayPointsBegin
,
In
wayPointsEnd
,
const
spline_constraints
&
constraints
=
spline_constraints
()
)
:
exact_cubic_t
(
computeWayPoints
<
In
>
(
wayPointsBegin
,
wayPointsEnd
,
constraints
))
{}
///\brief Destructor
~
cubic_zero_vel
(){}
protected:
private:
MatrixX
setVelConstraintsAndComputeB
(
const
spline_constraints
&
constraints
,
const
Eigen
::
Ref
<
MatrixX
>
x
,
Eigen
::
Ref
<
MatrixX
>
h1
,
Eigen
::
Ref
<
MatrixX
>
h2
)
const
{
std
::
size_t
size
(
h1
.
rows
());
MatrixX
cons
=
MatrixX
::
Zero
(
h1
.
rows
(),
Dim
);
// constraint matrix on b
cons
.
row
(
0
)
=
constraints
.
init_vel
;
cons
.
row
(
size
-
1
)
=
constraints
.
end_vel
;
h1
(
0
,
0
)
=
1
;
// activating init velocity constraint
h1
(
size
-
1
,
size
-
1
)
=
1
;
// activating end velocity constraint
MatrixX
b
=
MatrixX
::
Zero
(
size
,
Dim
);
MatrixX
h1inv
=
h1
.
inverse
();
b
=
h1inv
*
(
h2
*
x
+
cons
);
//h1 * b = h2 * x => b = (h1)^-1 * h2 * x
return
b
;
}
template
<
typename
In
>
t_spline_t
computeWayPoints
(
In
wayPointsBegin
,
In
wayPointsEnd
)
const
t_spline_t
computeWayPoints
(
In
wayPointsBegin
,
In
wayPointsEnd
,
const
spline_constraints
&
constraints
)
const
{
std
::
size_t
const
size
(
std
::
distance
(
wayPointsBegin
,
wayPointsEnd
));
if
(
Safe
&&
size
<
1
)
...
...
@@ -84,12 +123,10 @@ struct cubic_zero_vel : public curve_abc<Time, Numeric, Dim, Safe, Point>
MatrixX
h6
=
MatrixX
::
Zero
(
size
,
size
);
MatrixX
a
=
MatrixX
::
Zero
(
size
,
Dim
);
MatrixX
b
=
MatrixX
::
Zero
(
size
,
Dim
);
MatrixX
c
=
MatrixX
::
Zero
(
size
,
Dim
);
MatrixX
d
=
MatrixX
::
Zero
(
size
,
Dim
);
MatrixX
x
=
MatrixX
::
Zero
(
size
,
Dim
);
In
it
(
wayPointsBegin
),
next
(
wayPointsBegin
);
++
next
;
for
(
std
::
size_t
i
(
0
);
next
!=
wayPointsEnd
;
++
next
,
++
it
,
++
i
)
...
...
@@ -102,13 +139,8 @@ struct cubic_zero_vel : public curve_abc<Time, Numeric, Dim, Safe, Point>
h3
(
i
,
i
+
1
)
=
3
/
dTi_sqr
;
h4
(
i
,
i
)
=
-
2
/
dTi
;
h4
(
i
,
i
+
1
)
=
-
1
/
dTi
;
num_t
coeff_h5
(
-
2
);
if
(
i
==
0
)
{
coeff_h5
=
1
;
}
h5
(
i
,
i
)
=
-
coeff_h5
/
dTi_cube
;
h5
(
i
,
i
+
1
)
=
coeff_h5
/
dTi_cube
;
h5
(
i
,
i
)
=
2
/
dTi_cube
;
h5
(
i
,
i
+
1
)
=
-
2
/
dTi_cube
;
h6
(
i
,
i
)
=
1
/
dTi_sqr
;
h6
(
i
,
i
+
1
)
=
1
/
dTi_sqr
;
if
(
i
+
2
<
size
)
...
...
@@ -129,12 +161,8 @@ struct cubic_zero_vel : public curve_abc<Time, Numeric, Dim, Safe, Point>
// adding last x
x
.
row
(
size
-
1
)
=
(
*
it
).
second
.
transpose
();
a
=
x
;
h1
(
0
,
0
)
=
1
;
h3
(
0
,
0
)
=
0
;
h3
(
0
,
1
)
=
0
;
h4
(
0
,
0
)
=
0
;
h4
(
0
,
1
)
=
0
;
h6
(
0
,
0
)
=
0
;
h6
(
0
,
1
)
=
0
;
PseudoInverse
(
h1
);
b
=
h1
*
h2
*
x
;
//h1 * b = h2 * x => b = (h1)^-1 * h2 * x
// init velocity given by a constraint on b0
MatrixX
b
=
setVelConstraintsAndComputeB
(
constraints
,
x
,
h1
,
h2
);
c
=
h3
*
x
+
h4
*
b
;
d
=
h5
*
x
+
h6
*
b
;
it
=
wayPointsBegin
,
next
=
wayPointsBegin
;
++
next
;
...
...
@@ -143,8 +171,8 @@ struct cubic_zero_vel : public curve_abc<Time, Numeric, Dim, Safe, Point>
subSplines
.
push_back
(
create_cubic
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
,
T_Point
>
(
a
.
row
(
i
),
b
.
row
(
i
),
c
.
row
(
i
),
d
.
row
(
i
),(
*
it
).
first
,
(
*
next
).
first
));
}
subSplines
.
push_back
(
create_cubic
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
,
T_Point
>
(
a
.
row
(
size
-
1
),
b
.
row
(
size
-
1
),
c
.
row
(
size
-
1
),
d
.
row
(
size
-
1
),
(
*
it
).
first
,
(
*
it
).
first
));
//
subSplines.push_back(
//
create_cubic<Time,Numeric,Dim,Safe,Point,T_Point>(a.row(size-1), b.row(size-1), c.row(size-1), d.row(size-1), (*it).first, (*it).first));
return
subSplines
;
}
...
...
@@ -153,31 +181,6 @@ struct cubic_zero_vel : public curve_abc<Time, Numeric, Dim, Safe, Point>
cubic_zero_vel
(
const
cubic_zero_vel
&
);
cubic_zero_vel
&
operator
=
(
const
cubic_zero_vel
&
);
/* Constructors - destructors */
/*Operations*/
public:
/// \brief Evaluation of the cubic spline at time t.
/// \param t : the time when to evaluate the spine
/// \param return : the value x(t)
virtual
point_t
operator
()(
time_t
t
)
const
{
if
(
Safe
&&
(
t
<
subSplines_
.
front
().
min
()
||
t
>
subSplines_
.
back
().
max
())){
throw
std
::
out_of_range
(
"TODO"
);}
for
(
cit_spline_t
it
=
subSplines_
.
begin
();
it
!=
subSplines_
.
end
();
++
it
)
{
if
(
t
>=
(
it
->
min
())
&&
t
<=
(
it
->
max
()))
{
return
it
->
operator
()(
t
);
}
}
}
/*Operations*/
/*Helpers*/
public:
num_t
virtual
min
()
const
{
return
subSplines_
.
front
().
min
();}
num_t
virtual
max
()
const
{
return
subSplines_
.
back
().
max
();}
/*Helpers*/
/*Attributes*/
public:
const
t_spline_t
subSplines_
;
...
...
include/spline/spline_curve.h
View file @
e9882b85
...
...
@@ -129,6 +129,33 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
currentPoint_
+=
cdt
*
coefficients_
.
col
(
i
);
return
currentPoint_
;
}
/// \brief Evaluation of the derivative spline at time t.
/// \param t : the time when to evaluate the spine
/// \param return : the value x(t)
point_t
derivate
(
time_t
t
,
std
::
size_t
order
)
const
{
if
((
t
<
t_min_
||
t
>
t_max_
)
&&
Safe
){
throw
std
::
out_of_range
(
"TODO"
);}
time_t
const
dt
(
t
-
t_min_
);
time_t
cdt
(
1
);
point_t
currentPoint_
=
point_t
::
Zero
();
for
(
int
i
=
order
;
i
<
order_
+
1
;
++
i
,
cdt
*=
dt
)
{
currentPoint_
+=
cdt
*
coefficients_
.
col
(
i
)
*
fact
(
i
,
order
);
}
return
currentPoint_
;
}
private:
num_t
fact
(
const
std
::
size_t
n
,
const
std
::
size_t
order
)
const
{
std
::
size_t
res
(
1
);
for
(
std
::
size_t
i
=
0
;
i
<
order
;
++
i
)
res
*=
n
-
i
;
return
res
;
}
/*Operations*/
/*Helpers*/
...
...
src/tests/spline_test/Main.cpp
View file @
e9882b85
...
...
@@ -16,7 +16,9 @@ typedef Eigen::Vector3d point_t;
typedef
std
::
vector
<
point_t
,
Eigen
::
aligned_allocator
<
point_t
>
>
t_point_t
;
typedef
spline_curve
<
double
,
double
,
3
,
true
,
point_t
,
t_point_t
>
cubic_function_t
;
typedef
exact_cubic
<
double
,
double
,
3
,
true
,
point_t
>
exact_cubic_t
;
typedef
cubic_zero_vel
<
double
,
double
,
3
,
true
,
point_t
>
cubic_zero_vel_t
;
typedef
bezier_curve
<
double
,
double
,
3
,
true
,
point_t
>
bezier_curve_t
;
typedef
cubic_zero_vel_t
::
spline_constraints
spline_constraints_t
;
typedef
std
::
pair
<
double
,
point_t
>
Waypoint
;
typedef
std
::
vector
<
Waypoint
>
T_Waypoint
;
...
...
@@ -311,6 +313,24 @@ void ExactCubicOneDimTest(bool& error)
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
;
res1
=
curve
->
derivate
(
eval_point
,
order
);
ComparePoints
(
target
,
res1
,
errmsg
,
error
);
}
void
ExactCubicPointsCrossedTest
(
bool
&
error
)
{
spline
::
T_Waypoint
waypoints
;
...
...
@@ -318,25 +338,52 @@ void ExactCubicPointsCrossedTest(bool& error)
{
waypoints
.
push_back
(
std
::
make_pair
(
i
,
point_t
(
i
,
i
,
i
)));
}
exact_cubic_t
exactCubic
(
waypoints
.
begin
(),
waypoints
.
end
());
point_t
res1
;
for
(
double
i
=
0
;
i
<=
1
;
i
=
i
+
0.2
)
{
res1
=
exactCubic
(
i
);
std
::
string
errmsg
(
"Error While checking that given wayPoints are crossed (expected / obtained)"
);
ComparePoints
(
point_t
(
i
,
i
,
i
),
res1
,
errmsg
,
error
);
}
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
);
}
int
main
(
int
argc
,
char
*
argv
[])
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
;
cubic_zero_vel_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
);
constraints
.
end_vel
=
point_t
(
1
,
2
,
3
);
constraints
.
init_vel
=
point_t
(
-
1
,
-
2
,
-
3
);
std
::
string
errmsg2
(
"Error in ExactCubicVelocityConstraintsTest (3); while checking that given wayPoints are crossed (expected / obtained)"
);
cubic_zero_vel_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
);
}
int
main
(
int
/*argc*/
,
char
*
/*argv[]*/
)
{
std
::
cout
<<
"performing tests...
\n
"
;
bool
error
=
false
;
CubicFunctionTest
(
error
);
CubicFunctionTest
(
error
);
ExactCubicNoErrorTest
(
error
);
ExactCubicPointsCrossedTest
(
error
);
// checks that given wayPoints are crossed
ExactCubicTwoPointsTest
(
error
);
ExactCubicOneDimTest
(
error
);
ExactCubicOneDimTest
(
error
);
ExactCubicVelocityConstraintsTest
(
error
);
//BezierCurveTest(error);
if
(
error
)
{
...
...
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