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
c8fd823c
Commit
c8fd823c
authored
Jan 03, 2019
by
Guilhem Saurel
Browse files
finishing rename into hpp-spline
parent
1924448a
Changes
19
Expand all
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
c8fd823c
...
...
@@ -34,27 +34,8 @@ IF(BUILD_PYTHON_INTERFACE)
ENDIF
(
BUILD_PYTHON_INTERFACE
)
add_subdirectory
(
tests
)
install
(
FILES
${
CMAKE_SOURCE_DIR
}
/include/spline/bernstein.h
${
CMAKE_SOURCE_DIR
}
/include/spline/bezier_polynom_conversion.h
${
CMAKE_SOURCE_DIR
}
/include/spline/curve_abc.h
${
CMAKE_SOURCE_DIR
}
/include/spline/exact_cubic.h
${
CMAKE_SOURCE_DIR
}
/include/spline/MathDefs.h
${
CMAKE_SOURCE_DIR
}
/include/spline/polynom.h
${
CMAKE_SOURCE_DIR
}
/include/spline/spline_deriv_constraint.h
${
CMAKE_SOURCE_DIR
}
/include/spline/bezier_curve.h
${
CMAKE_SOURCE_DIR
}
/include/spline/cubic_spline.h
${
CMAKE_SOURCE_DIR
}
/include/spline/curve_constraint.h
${
CMAKE_SOURCE_DIR
}
/include/spline/quintic_spline.h
DESTINATION
${
CMAKE_INSTALL_PREFIX
}
/include/spline
)
install
(
FILES
${
CMAKE_SOURCE_DIR
}
/include/spline/helpers/effector_spline.h
${
CMAKE_SOURCE_DIR
}
/include/spline/helpers/effector_spline_rotation.h
DESTINATION
${
CMAKE_INSTALL_PREFIX
}
/include/spline/helpers
)
ADD_SUBDIRECTORY
(
include/hpp/spline
)
ADD_SUBDIRECTORY
(
tests
)
SETUP_PROJECT_FINALIZE
()
include/hpp/spline/CMakeLists.txt
0 → 100644
View file @
c8fd823c
SET
(
${
PROJECT_NAME
}
_HEADERS
bernstein.h
bezier_polynom_conversion.h
curve_abc.h
exact_cubic.h
MathDefs.h
polynom.h
spline_deriv_constraint.h
bezier_curve.h
cubic_spline.h
curve_constraint.h
quintic_spline.h
)
INSTALL
(
FILES
${${
PROJECT_NAME
}
_HEADERS
}
DESTINATION include/hpp/spline
)
ADD_SUBDIRECTORY
(
helpers
)
include/spline/MathDefs.h
→
include/
hpp/
spline/MathDefs.h
View file @
c8fd823c
File moved
include/spline/bernstein.h
→
include/
hpp/
spline/bernstein.h
View file @
c8fd823c
File moved
include/spline/bezier_curve.h
→
include/
hpp/
spline/bezier_curve.h
View file @
c8fd823c
File moved
include/spline/bezier_polynom_conversion.h
→
include/
hpp/
spline/bezier_polynom_conversion.h
View file @
c8fd823c
File moved
include/spline/cubic_spline.h
→
include/
hpp/
spline/cubic_spline.h
View file @
c8fd823c
File moved
include/spline/curve_abc.h
→
include/
hpp/
spline/curve_abc.h
View file @
c8fd823c
File moved
include/spline/curve_constraint.h
→
include/
hpp/
spline/curve_constraint.h
View file @
c8fd823c
File moved
include/spline/exact_cubic.h
→
include/
hpp/
spline/exact_cubic.h
View file @
c8fd823c
File moved
include/hpp/spline/helpers/CMakeLists.txt
0 → 100644
View file @
c8fd823c
SET
(
${
PROJECT_NAME
}
_HELPERS_HEADERS
effector_spline.h
effector_spline_rotation.h
)
INSTALL
(
FILES
${${
PROJECT_NAME
}
_HELPERS_HEADERS
}
DESTINATION include/hpp/spline/helpers
)
include/spline/helpers/effector_spline.h
→
include/
hpp/
spline/helpers/effector_spline.h
View file @
c8fd823c
/**
* \file exact_cubic.h
* \brief class allowing to create an Exact cubic spline.
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*
* This file contains definitions for the ExactCubic class.
* Given a set of waypoints (x_i*) and timestep (t_i), it provides the unique set of
* cubic splines fulfulling those 4 restrictions :
* - x_i(t_i) = x_i* ; this means that the curve passes trough each waypoint
* - x_i(t_i+1) = x_i+1* ;
* - its derivative is continous at t_i+1
* - its acceleration is continous at t_i+1
* more details in paper "Task-Space Trajectories via Cubic Spline Optimization"
* By J. Zico Kolter and Andrew Y.ng (ICRA 2009)
*/
#ifndef _CLASS_EFFECTORSPLINE
#define _CLASS_EFFECTORSPLINE
#include
"spline/spline_deriv_constraint.h"
namespace
spline
{
namespace
helpers
{
typedef
double
Numeric
;
typedef
double
Time
;
typedef
Eigen
::
Matrix
<
Numeric
,
3
,
1
>
Point
;
typedef
std
::
vector
<
Point
,
Eigen
::
aligned_allocator
<
Point
>
>
T_Point
;
typedef
std
::
pair
<
double
,
Point
>
Waypoint
;
typedef
std
::
vector
<
Waypoint
>
T_Waypoint
;
typedef
spline_deriv_constraint
<
Time
,
Numeric
,
3
,
true
,
Point
,
T_Point
>
spline_deriv_constraint_t
;
typedef
spline_deriv_constraint_t
::
spline_constraints
spline_constraints_t
;
typedef
spline_deriv_constraint_t
::
exact_cubic_t
exact_cubic_t
;
typedef
spline_deriv_constraint_t
::
t_spline_t
t_spline_t
;
typedef
spline_deriv_constraint_t
::
spline_t
spline_t
;
Waypoint
compute_offset
(
const
Waypoint
&
source
,
const
Point
&
normal
,
const
Numeric
offset
,
const
Time
time_offset
)
{
//compute time such that the equation from source to offsetpoint is necessarily a line
Numeric
norm
=
normal
.
norm
();
assert
(
norm
>
0.
);
return
std
::
make_pair
(
source
.
first
+
time_offset
,(
source
.
second
+
normal
/
norm
*
offset
));
}
spline_t
make_end_spline
(
const
Point
&
normal
,
const
Point
&
from
,
const
Numeric
offset
,
const
Time
init_time
,
const
Time
time_offset
)
{
// compute spline from land way point to end point
// constraints are null velocity and acceleration
Numeric
norm
=
normal
.
norm
();
assert
(
norm
>
0.
);
Point
n
=
normal
/
norm
;
Point
d
=
offset
/
(
time_offset
*
time_offset
*
time_offset
)
*
-
n
;
Point
c
=
-
3
*
d
*
time_offset
;
Point
b
=
-
c
*
time_offset
;
T_Point
points
;
points
.
push_back
(
from
);
points
.
push_back
(
b
);
points
.
push_back
(
c
);
points
.
push_back
(
d
);
return
spline_t
(
points
.
begin
(),
points
.
end
(),
init_time
,
init_time
+
time_offset
);
}
spline_constraints_t
compute_required_offset_velocity_acceleration
(
const
spline_t
&
end_spline
,
const
Time
/*time_offset*/
)
{
//computing end velocity: along landing normal and respecting time
spline_constraints_t
constraints
;
constraints
.
end_acc
=
end_spline
.
derivate
(
end_spline
.
min
(),
2
);
constraints
.
end_vel
=
end_spline
.
derivate
(
end_spline
.
min
(),
1
);
return
constraints
;
}
/// \brief Helper method to create a spline typically used to
/// guide the 3d trajectory of a robot end effector.
/// Given a set of waypoints, and the normal vector of the start and
/// ending positions, automatically create the spline such that:
/// + init and end velocities / accelerations are 0.
/// + the effector lifts and lands exactly in the direction of the specified normals
/// \param wayPointsBegin : an iterator pointing to the first element of a waypoint container
/// \param wayPointsEnd : an iterator pointing to the end of a waypoint container
/// \param lift_normal : normal to be followed by end effector at take-off
/// \param land_normal : normal to be followed by end effector at landing
/// \param lift_offset : length of the straight line along normal at take-off
/// \param land_offset : length of the straight line along normal at landing
/// \param lift_offset_duration : time travelled along straight line at take-off
/// \param land_offset_duration : time travelled along straight line at landing
///
template
<
typename
In
>
exact_cubic_t
*
effector_spline
(
In
wayPointsBegin
,
In
wayPointsEnd
,
const
Point
&
lift_normal
=
Eigen
::
Vector3d
::
UnitZ
(),
const
Point
&
land_normal
=
Eigen
::
Vector3d
::
UnitZ
(),
const
Numeric
lift_offset
=
0.02
,
const
Numeric
land_offset
=
0.02
,
const
Time
lift_offset_duration
=
0.02
,
const
Time
land_offset_duration
=
0.02
)
{
T_Waypoint
waypoints
;
const
Waypoint
&
inPoint
=*
wayPointsBegin
,
endPoint
=*
(
wayPointsEnd
-
1
);
waypoints
.
push_back
(
inPoint
);
//adding initial offset
waypoints
.
push_back
(
compute_offset
(
inPoint
,
lift_normal
,
lift_offset
,
lift_offset_duration
));
//inserting all waypoints but last
waypoints
.
insert
(
waypoints
.
end
(),
wayPointsBegin
+
1
,
wayPointsEnd
-
1
);
//inserting waypoint to start landing
const
Waypoint
&
landWaypoint
=
compute_offset
(
endPoint
,
land_normal
,
land_offset
,
-
land_offset_duration
);
waypoints
.
push_back
(
landWaypoint
);
//specifying end velocity constraint such that landing will be in straight line
spline_t
end_spline
=
make_end_spline
(
land_normal
,
landWaypoint
.
second
,
land_offset
,
landWaypoint
.
first
,
land_offset_duration
);
spline_constraints_t
constraints
=
compute_required_offset_velocity_acceleration
(
end_spline
,
land_offset_duration
);
spline_deriv_constraint_t
all_but_end
(
waypoints
.
begin
(),
waypoints
.
end
(),
constraints
);
t_spline_t
splines
=
all_but_end
.
subSplines_
;
splines
.
push_back
(
end_spline
);
return
new
exact_cubic_t
(
splines
);
}
}
}
#endif //_CLASS_EFFECTORSPLINE
/**
* \file exact_cubic.h
* \brief class allowing to create an Exact cubic spline.
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*
* This file contains definitions for the ExactCubic class.
* Given a set of waypoints (x_i*) and timestep (t_i), it provides the unique set of
* cubic splines fulfulling those 4 restrictions :
* - x_i(t_i) = x_i* ; this means that the curve passes trough each waypoint
* - x_i(t_i+1) = x_i+1* ;
* - its derivative is continous at t_i+1
* - its acceleration is continous at t_i+1
* more details in paper "Task-Space Trajectories via Cubic Spline Optimization"
* By J. Zico Kolter and Andrew Y.ng (ICRA 2009)
*/
#ifndef _CLASS_EFFECTORSPLINE
#define _CLASS_EFFECTORSPLINE
#include
"hpp/spline/spline_deriv_constraint.h"
namespace
spline
{
namespace
helpers
{
typedef
double
Numeric
;
typedef
double
Time
;
typedef
Eigen
::
Matrix
<
Numeric
,
3
,
1
>
Point
;
typedef
std
::
vector
<
Point
,
Eigen
::
aligned_allocator
<
Point
>
>
T_Point
;
typedef
std
::
pair
<
double
,
Point
>
Waypoint
;
typedef
std
::
vector
<
Waypoint
>
T_Waypoint
;
typedef
spline_deriv_constraint
<
Time
,
Numeric
,
3
,
true
,
Point
,
T_Point
>
spline_deriv_constraint_t
;
typedef
spline_deriv_constraint_t
::
spline_constraints
spline_constraints_t
;
typedef
spline_deriv_constraint_t
::
exact_cubic_t
exact_cubic_t
;
typedef
spline_deriv_constraint_t
::
t_spline_t
t_spline_t
;
typedef
spline_deriv_constraint_t
::
spline_t
spline_t
;
Waypoint
compute_offset
(
const
Waypoint
&
source
,
const
Point
&
normal
,
const
Numeric
offset
,
const
Time
time_offset
)
{
//compute time such that the equation from source to offsetpoint is necessarily a line
Numeric
norm
=
normal
.
norm
();
assert
(
norm
>
0.
);
return
std
::
make_pair
(
source
.
first
+
time_offset
,(
source
.
second
+
normal
/
norm
*
offset
));
}
spline_t
make_end_spline
(
const
Point
&
normal
,
const
Point
&
from
,
const
Numeric
offset
,
const
Time
init_time
,
const
Time
time_offset
)
{
// compute spline from land way point to end point
// constraints are null velocity and acceleration
Numeric
norm
=
normal
.
norm
();
assert
(
norm
>
0.
);
Point
n
=
normal
/
norm
;
Point
d
=
offset
/
(
time_offset
*
time_offset
*
time_offset
)
*
-
n
;
Point
c
=
-
3
*
d
*
time_offset
;
Point
b
=
-
c
*
time_offset
;
T_Point
points
;
points
.
push_back
(
from
);
points
.
push_back
(
b
);
points
.
push_back
(
c
);
points
.
push_back
(
d
);
return
spline_t
(
points
.
begin
(),
points
.
end
(),
init_time
,
init_time
+
time_offset
);
}
spline_constraints_t
compute_required_offset_velocity_acceleration
(
const
spline_t
&
end_spline
,
const
Time
/*time_offset*/
)
{
//computing end velocity: along landing normal and respecting time
spline_constraints_t
constraints
;
constraints
.
end_acc
=
end_spline
.
derivate
(
end_spline
.
min
(),
2
);
constraints
.
end_vel
=
end_spline
.
derivate
(
end_spline
.
min
(),
1
);
return
constraints
;
}
/// \brief Helper method to create a spline typically used to
/// guide the 3d trajectory of a robot end effector.
/// Given a set of waypoints, and the normal vector of the start and
/// ending positions, automatically create the spline such that:
/// + init and end velocities / accelerations are 0.
/// + the effector lifts and lands exactly in the direction of the specified normals
/// \param wayPointsBegin : an iterator pointing to the first element of a waypoint container
/// \param wayPointsEnd : an iterator pointing to the end of a waypoint container
/// \param lift_normal : normal to be followed by end effector at take-off
/// \param land_normal : normal to be followed by end effector at landing
/// \param lift_offset : length of the straight line along normal at take-off
/// \param land_offset : length of the straight line along normal at landing
/// \param lift_offset_duration : time travelled along straight line at take-off
/// \param land_offset_duration : time travelled along straight line at landing
///
template
<
typename
In
>
exact_cubic_t
*
effector_spline
(
In
wayPointsBegin
,
In
wayPointsEnd
,
const
Point
&
lift_normal
=
Eigen
::
Vector3d
::
UnitZ
(),
const
Point
&
land_normal
=
Eigen
::
Vector3d
::
UnitZ
(),
const
Numeric
lift_offset
=
0.02
,
const
Numeric
land_offset
=
0.02
,
const
Time
lift_offset_duration
=
0.02
,
const
Time
land_offset_duration
=
0.02
)
{
T_Waypoint
waypoints
;
const
Waypoint
&
inPoint
=*
wayPointsBegin
,
endPoint
=*
(
wayPointsEnd
-
1
);
waypoints
.
push_back
(
inPoint
);
//adding initial offset
waypoints
.
push_back
(
compute_offset
(
inPoint
,
lift_normal
,
lift_offset
,
lift_offset_duration
));
//inserting all waypoints but last
waypoints
.
insert
(
waypoints
.
end
(),
wayPointsBegin
+
1
,
wayPointsEnd
-
1
);
//inserting waypoint to start landing
const
Waypoint
&
landWaypoint
=
compute_offset
(
endPoint
,
land_normal
,
land_offset
,
-
land_offset_duration
);
waypoints
.
push_back
(
landWaypoint
);
//specifying end velocity constraint such that landing will be in straight line
spline_t
end_spline
=
make_end_spline
(
land_normal
,
landWaypoint
.
second
,
land_offset
,
landWaypoint
.
first
,
land_offset_duration
);
spline_constraints_t
constraints
=
compute_required_offset_velocity_acceleration
(
end_spline
,
land_offset_duration
);
spline_deriv_constraint_t
all_but_end
(
waypoints
.
begin
(),
waypoints
.
end
(),
constraints
);
t_spline_t
splines
=
all_but_end
.
subSplines_
;
splines
.
push_back
(
end_spline
);
return
new
exact_cubic_t
(
splines
);
}
}
}
#endif //_CLASS_EFFECTORSPLINE
include/spline/helpers/effector_spline_rotation.h
→
include/
hpp/
spline/helpers/effector_spline_rotation.h
View file @
c8fd823c
This diff is collapsed.
Click to expand it.
include/spline/optimization/OptimizeSpline.h
→
include/
hpp/
spline/optimization/OptimizeSpline.h
View file @
c8fd823c
File moved
include/spline/polynom.h
→
include/
hpp/
spline/polynom.h
View file @
c8fd823c
File moved
include/spline/quintic_spline.h
→
include/
hpp/
spline/quintic_spline.h
View file @
c8fd823c
File moved
include/spline/spline_deriv_constraint.h
→
include/
hpp/
spline/spline_deriv_constraint.h
View file @
c8fd823c
File moved
python/spline_python.cpp
View file @
c8fd823c
#include
"spline/bezier_curve.h"
#include
"spline/polynom.h"
#include
"spline/exact_cubic.h"
#include
"spline/spline_deriv_constraint.h"
#include
"spline/curve_constraint.h"
#include
"spline/bezier_polynom_conversion.h"
#include
"spline/bernstein.h"
#include
"
hpp/
spline/bezier_curve.h"
#include
"
hpp/
spline/polynom.h"
#include
"
hpp/
spline/exact_cubic.h"
#include
"
hpp/
spline/spline_deriv_constraint.h"
#include
"
hpp/
spline/curve_constraint.h"
#include
"
hpp/
spline/bezier_polynom_conversion.h"
#include
"
hpp/
spline/bernstein.h"
#include
<vector>
...
...
tests/Main.cpp
View file @
c8fd823c
This diff is collapsed.
Click to expand it.
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