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
c1bb699c
Commit
c1bb699c
authored
Dec 02, 2016
by
Steve Tonneau
Browse files
spline with start and end vel and acc constraints working and tested
parent
727b52ea
Changes
4
Hide whitespace changes
Inline
Side-by-side
include/spline/exact_cubic.h
View file @
c1bb699c
...
...
@@ -155,7 +155,7 @@ struct exact_cubic : 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
()(
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
)
{
...
...
include/spline/exact_cubic_vel_acc_cons.h
View file @
c1bb699c
...
...
@@ -44,6 +44,7 @@ struct cubic_zero_vel : public exact_cubic<Time, Numeric, Dim, Safe, Point, T_Po
typedef
Point
point_t
;
typedef
T_Point
t_point_t
;
typedef
Eigen
::
Matrix
<
Numeric
,
Eigen
::
Dynamic
,
Eigen
::
Dynamic
>
MatrixX
;
typedef
Eigen
::
Matrix
<
Numeric
,
3
,
3
>
Matrix3
;
typedef
Time
time_t
;
typedef
Numeric
num_t
;
typedef
spline_curve
<
time_t
,
Numeric
,
Dim
,
Safe
,
point_t
,
t_point_t
>
spline_t
;
...
...
@@ -84,33 +85,13 @@ struct cubic_zero_vel : public exact_cubic<Time, Numeric, Dim, Safe, Point, T_Po
///\brief Destructor
~
cubic_zero_vel
(){}
/*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;
}*/
private:
template
<
typename
In
>
void
compute_one_spline
(
In
wayPointsBegin
,
In
wayPointsNext
,
spline_constraints
&
constraints
,
t_spline_t
&
subSplines
)
void
compute_one_spline
(
In
wayPointsBegin
,
In
wayPointsNext
,
spline_constraints
&
constraints
,
t_spline_t
&
subSplines
)
const
{
const
point_t
&
a0
=
wayPointsBegin
->
second
,
a1
=
wayPointsNext
->
second
;
const
point_t
&
b0
=
constraints
.
init_vel
,
c0
=
constraints
.
init_acc
;
const
num_t
&
init_t
=
wayPointsBegin
->
first
,
end_t
=
-
wayPointsNext
->
first
;
const
point_t
&
b0
=
constraints
.
init_vel
,
c0
=
constraints
.
init_acc
/
2.
;
const
num_t
&
init_t
=
wayPointsBegin
->
first
,
end_t
=
wayPointsNext
->
first
;
const
num_t
dt
=
end_t
-
init_t
,
dt_2
=
dt
*
dt
,
dt_3
=
dt_2
*
dt
;
const
point_t
d0
=
(
a1
-
a0
-
b0
*
dt
-
c0
*
dt_2
)
/
dt_3
;
subSplines
.
push_back
(
create_cubic
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
,
T_Point
>
...
...
@@ -120,106 +101,51 @@ struct cubic_zero_vel : public exact_cubic<Time, Numeric, Dim, Safe, Point, T_Po
}
template
<
typename
In
>
void
compute_end_spline
(
In
wayPointsBegin
,
In
wayPointsNext
,
spline_constraints
&
constraints
,
t_spline_t
&
subSplines
)
void
compute_end_spline
(
In
wayPointsBegin
,
In
wayPointsNext
,
spline_constraints
&
constraints
,
t_spline_t
&
subSplines
)
const
{
const
point_t
&
a0
=
wayPointsBegin
->
second
,
a1
=
wayPointsNext
->
second
;
const
point_t
&
b0
=
constraints
.
init_vel
,
b1
=
constraints
.
end_vel
,
c0
=
constraints
.
init_acc
,
c1
=
constraints
.
end_acc
;
const
num_t
&
init_t
=
wayPointsBegin
->
first
,
end_t
=
-
wayPointsNext
->
first
;
const
num_t
dt
=
end_t
-
init_t
,
dt_2
=
dt
*
dt
,
dt_3
=
dt_2
*
dt
;
c0
=
constraints
.
init_acc
/
2.
,
c1
=
constraints
.
end_acc
;
const
num_t
&
init_t
=
wayPointsBegin
->
first
,
end_t
=
wayPointsNext
->
first
;
const
num_t
dt
=
end_t
-
init_t
,
dt_2
=
dt
*
dt
,
dt_3
=
dt_2
*
dt
,
dt_4
=
dt_3
*
dt
,
dt_5
=
dt_4
*
dt
;
//solving a system of four linear eq with 4 unknows: d0, e0
const
num_t
alpha_0
=
a1
-
a0
+
b0
*
dt
+
c0
*
dt_2
;
const
num_t
alpha_1
=
b1
-
b0
+
b0
*
dt
+
c0
*
dt_2
;
/*subSplines.push_back(create_cubic<Time,Numeric,Dim,Safe,Point,T_Point>
(a0,b0,c0,d0,e0, init_t, end_t));
constraints.init_vel = subSplines.back().derivate(end_t, 1);
constraints.init_acc = subSplines.back().derivate(end_t, 2);*/
//solving
const
point_t
alpha_0
=
a1
-
a0
-
b0
*
dt
-
c0
*
dt_2
;
const
point_t
alpha_1
=
b1
-
b0
-
2
*
c0
*
dt
;
const
point_t
alpha_2
=
c1
-
2
*
c0
;
const
num_t
x_d_0
=
dt_3
,
x_d_1
=
3
*
dt_2
,
x_d_2
=
6
*
dt
;
const
num_t
x_e_0
=
dt_4
,
x_e_1
=
4
*
dt_3
,
x_e_2
=
12
*
dt_2
;
const
num_t
x_f_0
=
dt_5
,
x_f_1
=
5
*
dt_4
,
x_f_2
=
20
*
dt_3
;
point_t
d
,
e
,
f
;
MatrixX
rhs
=
MatrixX
::
Zero
(
3
,
Dim
);
rhs
.
row
(
0
)
=
alpha_0
;
rhs
.
row
(
1
)
=
alpha_1
;
rhs
.
row
(
2
)
=
alpha_2
;
Matrix3
eq
=
Matrix3
::
Zero
(
3
,
3
);
eq
(
0
,
0
)
=
x_d_0
;
eq
(
0
,
1
)
=
x_e_0
;
eq
(
0
,
2
)
=
x_f_0
;
eq
(
1
,
0
)
=
x_d_1
;
eq
(
1
,
1
)
=
x_e_1
;
eq
(
1
,
2
)
=
x_f_1
;
eq
(
2
,
0
)
=
x_d_2
;
eq
(
2
,
1
)
=
x_e_2
;
eq
(
2
,
2
)
=
x_f_2
;
rhs
=
eq
.
inverse
().
eval
()
*
rhs
;
d
=
rhs
.
row
(
0
);
e
=
rhs
.
row
(
1
);
f
=
rhs
.
row
(
2
);
subSplines
.
push_back
(
create_quintic
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
,
T_Point
>
(
a0
,
b0
,
c0
,
d
,
e
,
f
,
init_t
,
end_t
));
}
private:
template
<
typename
In
>
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
)
{
throw
;
// TODO
}
t_spline_t
subSplines
;
subSplines
.
reserve
(
size
-
1
);
spline_constraints
cons
=
constraints
;
In
it
(
wayPointsBegin
),
next
(
wayPointsBegin
),
end
(
wayPointsEnd
-
1
);
++
next
;
for
(
std
::
size_t
i
(
0
);
next
!=
end
;
++
next
,
++
it
,
++
i
)
{
compute_one_spline
<
In
>
(
it
,
next
,
cons
,
subSplines
);
}
compute_end_spline
<
In
>
(
it
,
next
,
cons
,
subSplines
);
return
subSplines
;
// then solving last step
/*// refer to the paper to understand all this.
MatrixX h1 = MatrixX::Zero(size, size);
MatrixX h2 = MatrixX::Zero(size, size);
MatrixX h3 = MatrixX::Zero(size, size);
MatrixX h4 = MatrixX::Zero(size, size);
MatrixX h5 = MatrixX::Zero(size, size);
MatrixX h6 = MatrixX::Zero(size, size);
MatrixX a = 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)
{
num_t const dTi((*next).first - (*it).first);
num_t const dTi_sqr(dTi * dTi);
num_t const dTi_cube(dTi_sqr * dTi);
// filling matrices values
h3(i,i) = -3 / dTi_sqr;
h3(i,i+1) = 3 / dTi_sqr;
h4(i,i) = -2 / dTi;
h4(i,i+1) = -1 / dTi;
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)
{
In it2(next); ++ it2;
num_t const dTi_1((*it2).first - (*next).first);
num_t const dTi_1sqr(dTi_1 * dTi_1);
// this can be optimized but let's focus on clarity as long as not needed
h1(i+1, i) = 2 / dTi;
h1(i+1, i+1) = 4 / dTi + 4 / dTi_1;
h1(i+1, i+2) = 2 / dTi_1;
h2(i+1, i) = -6 / dTi_sqr;
h2(i+1, i+1) = (6 / dTi_1sqr) - (6 / dTi_sqr);
h2(i+1, i+2) = 6 / dTi_1sqr;
}
x.row(i)= (*it).second.transpose();
}
// adding last x
x.row(size-1)= (*it).second.transpose();
a= 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;
for(int i=0; next != wayPointsEnd; ++i, ++it, ++next)
{
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));
return subSplines;*/
}
...
...
include/spline/spline_curve.h
View file @
c1bb699c
...
...
@@ -102,13 +102,9 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
if
(
Safe
)
{
if
(
t_min_
>
t_max_
)
{
std
::
out_of_range
(
"TODO"
);
}
if
(
coefficients_
.
size
()
!=
order_
+
1
)
{
std
::
runtime_error
(
"Spline order and coefficients do not match"
);
}
}
}
...
...
@@ -141,9 +137,7 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
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_
;
}
...
...
@@ -183,9 +177,7 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
std
::
size_t
size
=
std
::
distance
(
zeroOrderCoefficient
,
highestOrderCoefficient
);
coeff_t
res
=
coeff_t
(
Dim
,
size
);
int
i
=
0
;
for
(
In
cit
=
zeroOrderCoefficient
;
cit
!=
highestOrderCoefficient
;
++
cit
,
++
i
)
{
res
.
col
(
i
)
=
*
cit
;
}
return
res
;
}
};
//class spline_curve
...
...
src/tests/spline_test/Main.cpp
View file @
c1bb699c
...
...
@@ -325,8 +325,7 @@ void CheckWayPointConstraint(const std::string& errmsg, const double step, const
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
);
point_t
res1
=
curve
->
derivate
(
eval_point
,
order
);
ComparePoints
(
target
,
res1
,
errmsg
,
error
);
}
...
...
@@ -360,9 +359,13 @@ void ExactCubicVelocityConstraintsTest(bool& error)
// 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)"
);
cubic_zero_vel_t
exactCubic2
(
waypoints
.
begin
(),
waypoints
.
end
(),
constraints
);
CheckWayPointConstraint
(
errmsg2
,
0.2
,
waypoints
,
&
exactCubic2
,
error
);
...
...
@@ -370,11 +373,13 @@ void ExactCubicVelocityConstraintsTest(bool& 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
,
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
);
}
int
main
(
int
/*argc*/
,
char
*
/*argv[]*/
)
int
main
(
int
/*argc*/
,
char
*
*
/*argv[]*/
)
{
std
::
cout
<<
"performing tests...
\n
"
;
bool
error
=
false
;
...
...
@@ -384,7 +389,7 @@ int main(int /*argc*/, char* /*argv[]*/)
ExactCubicTwoPointsTest
(
error
);
ExactCubicOneDimTest
(
error
);
ExactCubicVelocityConstraintsTest
(
error
);
//BezierCurveTest(error);
//BezierCurveTest(error);
if
(
error
)
{
std
::
cout
<<
"There were some errors
\n
"
;
...
...
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