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
48067afc
Commit
48067afc
authored
Nov 25, 2016
by
Steve Tonneau
Browse files
zero init velocity
parent
efe1fbc2
Changes
3
Hide whitespace changes
Inline
Side-by-side
include/spline/exact_cubic.h
View file @
48067afc
...
...
@@ -41,7 +41,7 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
typedef
Point
point_t
;
typedef
T_Point
t_point_t
;
typedef
Eigen
::
Matrix
<
Numeric
,
Eigen
::
Dynamic
,
Eigen
::
Dynamic
>
MatrixX
;
typedef
Eigen
::
Matrix
<
Numeric
,
Eigen
::
Dynamic
,
Eigen
::
Dynamic
>
MatrixX
;
typedef
Time
time_t
;
typedef
Numeric
num_t
;
typedef
spline_curve
<
time_t
,
Numeric
,
Dim
,
Safe
,
point_t
,
t_point_t
>
spline_t
;
...
...
@@ -57,9 +57,7 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
///\param wayPointsEns : an iterator pointing to the end of a waypoint container
template
<
typename
In
>
exact_cubic
(
In
wayPointsBegin
,
In
wayPointsEnd
)
:
curve_abc_t
(),
subSplines_
(
computeWayPoints
<
In
>
(
wayPointsBegin
,
wayPointsEnd
))
{
}
:
curve_abc_t
(),
subSplines_
(
computeWayPoints
<
In
>
(
wayPointsBegin
,
wayPointsEnd
))
{}
///\brief Destructor
~
exact_cubic
(){}
...
...
include/spline/exact_cubic_vel_acc_cons.h
View file @
48067afc
...
...
@@ -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
exact_cubi
c
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
,
T_
Point
>
struct
cubic_zero_vel
:
public
curve_ab
c
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
>
{
typedef
Point
point_t
;
typedef
T_Point
t_point_t
;
...
...
@@ -47,7 +47,7 @@ struct cubic_zero_vel : public exact_cubic<Time, Numeric, Dim, Safe, Point, T_Po
typedef
Time
time_t
;
typedef
Numeric
num_t
;
typedef
spline_curve
<
time_t
,
Numeric
,
Dim
,
Safe
,
point_t
,
t_point_t
>
spline_t
;
typedef
exact_cubi
c
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
>
exact_cubi
c_t
;
typedef
curve_ab
c
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
>
curve_ab
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
;
...
...
@@ -59,18 +59,129 @@ struct cubic_zero_vel : public exact_cubic<Time, Numeric, Dim, Safe, Point, T_Po
///\param wayPointsEns : an iterator pointing to the end of a waypoint container
template
<
typename
In
>
cubic_zero_vel
(
In
wayPointsBegin
,
In
wayPointsEnd
)
:
exact_cubic_t
(
wayPointsBegin
,
wayPointsEnd
)
{
// NOTHING
}
:
curve_abc_t
(),
subSplines_
(
computeWayPoints
<
In
>
(
wayPointsBegin
,
wayPointsEnd
))
{}
///\brief Destructor
~
cubic_zero_vel
(){}
protected:
template
<
typename
In
>
t_spline_t
computeWayPoints
(
In
wayPointsBegin
,
In
wayPointsEnd
)
const
{
std
::
size_t
const
size
(
std
::
distance
(
wayPointsBegin
,
wayPointsEnd
));
if
(
Safe
&&
size
<
1
)
{
throw
;
// TODO
}
t_spline_t
subSplines
;
subSplines
.
reserve
(
size
);
// 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
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
)
{
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
;
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
;
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
;
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
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
;
}
private:
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_
;
/*Attributes*/
};
}
#endif //_CLASS_CUBICZEROVELACC
...
...
include/spline/spline_curve.h
View file @
48067afc
...
...
@@ -44,12 +44,12 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/* Constructors - destructors */
public:
///\brief Constructor
///\param coefficients : a
container containing all coefficients of the spline, starting
///
with
the zero order coefficient, up to the highest order. Spline order is given
/// by the
size
of the co
efficients
///\param coefficients : a
reference to an Eigen matrix where each column is a coefficient,
///
from
the zero order coefficient, up to the highest order. Spline order is given
/// by the
number
of the co
lumns -1.
///\param min: LOWER bound on interval definition of the spline
///\param max: UPPER bound on interval definition of the spline
spline_curve
(
const
coeff_t_ref
&
coefficients
,
const
time_t
min
,
const
time_t
max
)
spline_curve
(
const
coeff_t_ref
coefficients
,
const
time_t
min
,
const
time_t
max
)
:
curve_abc_t
(),
coefficients_
(
coefficients
),
t_min_
(
min
),
t_max_
(
max
),
dim_
(
Dim
),
order_
(
coefficients_
.
cols
()
-
1
)
{
...
...
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