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
c0022298
Commit
c0022298
authored
Jan 26, 2017
by
Steve Tonneau
Browse files
effector spline helper with rotation. tests not all written
parent
5ead2d60
Changes
7
Hide whitespace changes
Inline
Side-by-side
include/spline/bezier_curve.h
View file @
c0022298
...
...
@@ -67,7 +67,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, 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
virtual
point_t
operator
()(
const
time_t
t
)
const
{
num_t
nT
=
(
t
-
minBound_
)
/
(
maxBound_
-
minBound_
);
if
(
Safe
&!
(
0
<=
nT
&&
nT
<=
1
))
...
...
include/spline/curve_abc.h
View file @
c0022298
...
...
@@ -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
()(
const
time_t
t
)
const
=
0
;
/*Operations*/
/*Helpers*/
...
...
include/spline/exact_cubic.h
View file @
c0022298
...
...
@@ -154,16 +154,16 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \brief Evaluation of the cubic spline at time t.
/// \param t : the time when to evaluate the spline
/// \param return : the value x(t)
virtual
point_t
operator
()(
time_t
t
)
const
virtual
point_t
operator
()(
const
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
);
}
}
}
}
/// \brief Evaluation of the derivative spline at time t.
...
...
include/spline/helpers/effector_spline.h
View file @
c0022298
...
...
@@ -79,7 +79,7 @@ spline_constraints_t compute_required_offset_velocity_acceleration(const spline_
/// \brief Helper method to create a spline typically used to
/// guide the
2d or
3d trajectory of a robot end effector.
/// 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.
...
...
@@ -90,28 +90,28 @@ spline_constraints_t compute_required_offset_velocity_acceleration(const spline_
/// \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
time_
lift_offset : time travelled along straight line at take-off
/// \param
time_
land_offset : time travelled along straight line 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
time_
lift_offset
=
0.02
,
const
Time
time_
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
,
time_
lift_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
,
-
time_
land_offset
);
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
,
time_
land_offset
);
spline_constraints_t
constraints
=
compute_required_offset_velocity_acceleration
(
end_spline
,
time_
land_offset
);
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
);
...
...
include/spline/helpers/effector_spline_rotation.h
0 → 100644
View file @
c0022298
/**
* \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_EFFECTOR_SPLINE_ROTATION
#define _CLASS_EFFECTOR_SPLINE_ROTATION
#include
"spline/helpers/effector_spline.h"
#include
<Eigen/Geometry>
namespace
spline
{
namespace
helpers
{
typedef
Eigen
::
Matrix
<
Numeric
,
4
,
1
>
quat_t
;
typedef
Eigen
::
Ref
<
quat_t
>
quat_ref_t
;
typedef
const
Eigen
::
Ref
<
const
quat_t
>
quat_ref_const_t
;
typedef
Eigen
::
Matrix
<
Numeric
,
7
,
1
>
config_t
;
/// \class effector_spline_rotation
/// \brief Represents a trajectory for and end effector
/// uses the method effector_spline to create a spline trajectory.
/// Additionally, handles the rotation of the effector as follows:
/// does not rotate during the take off and landing phase,
/// then uses a SLERP algorithm to interpolate the rotation in the quaternion
/// space.
class
effector_spline_rotation
{
/* Constructors - destructors */
public:
/// \brief Constructor.
/// 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 to_quat : 4D vector, quaternion indicating rotation at take off(x, y, z, w)
/// \param land_quat : 4D vector, quaternion indicating rotation at landing (x, y, z, w)
/// \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
>
effector_spline_rotation
(
In
wayPointsBegin
,
In
wayPointsEnd
,
quat_ref_const_t
&
to_quat
=
quat_t
(
0
,
0
,
0
,
1
),
quat_ref_const_t
&
land_quat
=
quat_t
(
0
,
0
,
0
,
1
),
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
)
:
spline_
(
effector_spline
(
wayPointsBegin
,
wayPointsEnd
,
lift_normal
,
land_normal
,
lift_offset
,
land_offset
,
lift_offset_duration
,
land_offset_duration
))
,
to_quat_
(
to_quat
.
data
()),
land_quat_
(
land_quat
.
data
())
,
time_lift_offset_
(
spline_
->
min
()
+
lift_offset_duration
)
,
time_land_offset_
(
spline_
->
max
()
-
land_offset_duration
)
{
// NOTHING
}
~
effector_spline_rotation
()
{
delete
spline_
;}
/* Constructors - destructors */
/*Helpers*/
public:
Numeric
min
()
const
{
return
spline_
->
min
();}
Numeric
max
()
const
{
return
spline_
->
max
();}
/*Helpers*/
/*Operations*/
public:
/// \brief Evaluation of the effector position and rotation at time t.
/// \param t : the time when to evaluate the spline
/// \param return : a 7D vector; The 3 first values are the 3D position, the 4 last the
/// quaternion describing the rotation
///
config_t
operator
()(
const
Numeric
t
)
const
{
config_t
res
;
res
.
head
<
3
>
()
=
(
*
spline_
)(
t
);
res
.
tail
<
4
>
()
=
interpolate_quat
(
t
);
return
res
;
}
/// \brief Interpolates between two quaternions
/// \param t : the time when to evaluate the spline
/// \param quat : quaternion updated as the interpolation result
///
quat_t
interpolate_quat
(
time_t
t
)
const
{
if
(
t
<=
time_lift_offset_
)
return
quat_t
(
to_quat_
.
coeffs
().
data
());
if
(
t
>=
time_land_offset_
)
return
quat_t
(
land_quat_
.
coeffs
().
data
());
//normalize u
Numeric
u
=
(
t
-
time_lift_offset_
)
/
(
time_land_offset_
-
time_lift_offset_
);
return
quat_t
(
to_quat_
.
slerp
(
u
,
land_quat_
).
coeffs
().
data
());
}
/*Operations*/
/*Attributes*/
public:
const
exact_cubic_t
*
spline_
;
const
Eigen
::
Quaterniond
to_quat_
;
const
Eigen
::
Quaterniond
land_quat_
;
const
double
time_lift_offset_
;
const
double
time_land_offset_
;
/*Attributes*/
};
}
// namespace helpers
}
// namespace spline
#endif //_CLASS_EFFECTOR_SPLINE_ROTATION
include/spline/spline_curve.h
View file @
c0022298
...
...
@@ -115,7 +115,7 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, 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
virtual
point_t
operator
()(
const
time_t
t
)
const
{
if
((
t
<
t_min_
||
t
>
t_max_
)
&&
Safe
){
throw
std
::
out_of_range
(
"TODO"
);}
time_t
const
dt
(
t
-
t_min_
);
...
...
src/tests/spline_test/Main.cpp
View file @
c0022298
...
...
@@ -4,6 +4,7 @@
#include
"spline/spline_curve.h"
#include
"spline/spline_deriv_constraint.h"
#include
"spline/helpers/effector_spline.h"
#include
"spline/helpers/effector_spline_rotation.h"
#include
<string>
#include
<iostream>
...
...
@@ -42,12 +43,7 @@ bool QuasiEqual(const double a, const double b, const float margin)
}
}
const
float
margin
=
0.01
f
;
bool
operator
==
(
const
point_t
&
a
,
const
point_t
&
b
)
{
return
QuasiEqual
(
a
.
x
(),
b
.
x
(),
margin
)
&&
QuasiEqual
(
a
.
y
(),
b
.
y
(),
margin
)
&&
QuasiEqual
(
a
.
z
(),
b
.
z
(),
margin
);
}
const
double
margin
=
0.001
;
}
// namespace spline
...
...
@@ -59,24 +55,15 @@ ostream& operator<<(ostream& os, const point_t& pt)
return
os
;
}
void
ComparePoints
(
const
point_t
&
pt1
,
const
point_t
&
pt2
,
const
std
::
string
&
errmsg
,
bool
&
error
)
void
ComparePoints
(
const
Eigen
::
VectorXd
&
pt1
,
const
Eigen
::
VectorXd
&
pt2
,
const
std
::
string
&
errmsg
,
bool
&
error
)
{
if
(
!
(
pt1
==
pt2
)
)
if
((
pt1
-
pt2
).
norm
()
>
margin
)
{
error
=
true
;
std
::
cout
<<
errmsg
<<
pt1
<<
" ; "
<<
pt2
<<
std
::
endl
;
}
}
void
ComparePoints
(
const
point_one
&
pt1
,
const
point_one
&
pt2
,
const
std
::
string
&
errmsg
,
bool
&
error
)
{
if
(
!
(
pt1
==
pt2
))
{
error
=
true
;
std
::
cout
<<
errmsg
<<
pt1
<<
" ; "
<<
pt2
<<
std
::
endl
;
}
}
/*Cubic Function tests*/
void
CubicFunctionTest
(
bool
&
error
)
...
...
@@ -402,10 +389,10 @@ void EffectorTrajectoryTest(bool& error)
}
helpers
::
exact_cubic_t
*
eff_traj
=
helpers
::
effector_spline
(
waypoints
.
begin
(),
waypoints
.
end
(),
Eigen
::
Vector3d
::
UnitZ
(),
Eigen
::
Vector3d
(
0
,
0
,
2
),
1
,
1
,
1
,
0.5
);
1
,
0.02
,
1
,
0.5
);
point_t
zero
(
0
,
0
,
0
);
point_t
off1
(
0
,
0
,
1
);
point_t
off2
(
10
,
10
,
1
1
);
point_t
off2
(
10
,
10
,
1
0.02
);
point_t
end
(
10
,
10
,
10
);
std
::
string
errmsg
(
"Error in EffectorTrajectoryTest; while checking waypoints (expected / obtained)"
);
std
::
string
errmsg2
(
"Error in EffectorTrajectoryTest; while checking derivative (expected / obtained)"
);
...
...
@@ -429,13 +416,40 @@ void EffectorTrajectoryTest(bool& error)
CheckPointOnline
(
errmsg3
,(
*
eff_traj
)(
0
),(
*
eff_traj
)(
1
),
i
,
eff_traj
,
error
);
}
for
(
double
i
=
9.
6
;
i
<
10
;
i
+=
0.
1
)
for
(
double
i
=
9.
981
;
i
<
10
;
i
+=
0.
002
)
{
CheckPointOnline
(
errmsg3
,(
*
eff_traj
)(
9.5
),(
*
eff_traj
)(
10
),
i
,
eff_traj
,
error
);
}
delete
eff_traj
;
}
helpers
::
quat_t
GetXRotQuat
(
const
double
theta
)
{
Eigen
::
AngleAxisd
m
(
theta
,
Eigen
::
Vector3d
::
UnitX
());
return
helpers
::
quat_t
(
Eigen
::
Quaterniond
(
m
).
coeffs
().
data
());
}
void
EffectorSplineRotationNoRotationTest
(
bool
&
error
)
{
// create arbitrary trajectory
spline
::
T_Waypoint
waypoints
;
for
(
double
i
=
0
;
i
<=
10
;
i
=
i
+
2
)
{
waypoints
.
push_back
(
std
::
make_pair
(
i
,
point_t
(
i
,
i
,
i
)));
}
helpers
::
effector_spline_rotation
eff_traj
(
waypoints
.
begin
(),
waypoints
.
end
());
helpers
::
config_t
q_init
;
q_init
<<
0.
,
0.
,
0.
,
0.
,
0.
,
0.
,
1.
;
helpers
::
config_t
q_end
;
q_end
<<
10.
,
10.
,
10.
,
0.
,
0.
,
0.
,
1.
;
helpers
::
config_t
q_to
;
q_to
<<
0.
,
0
,
0.02
,
0.
,
0.
,
0.
,
1.
;
helpers
::
config_t
q_land
;
q_land
<<
10
,
10
,
10.02
,
0
,
0.
,
0.
,
1.
;
helpers
::
config_t
q_mod
;
q_mod
<<
6.
,
6.
,
6.
,
0.
,
0.
,
0.
,
1.
;
std
::
string
errmsg
(
"Error in EffectorSplineRotationNoRotationTest; while checking waypoints (expected / obtained)"
);
ComparePoints
(
q_init
,
eff_traj
(
0
),
errmsg
,
error
);
ComparePoints
(
q_to
,
eff_traj
(
0.02
),
errmsg
,
error
);
ComparePoints
(
q_land
,
eff_traj
(
9.98
),
errmsg
,
error
);
ComparePoints
(
q_mod
,
eff_traj
(
6
),
errmsg
,
error
);
ComparePoints
(
q_end
,
eff_traj
(
10
),
errmsg
,
error
);
}
int
main
(
int
/*argc*/
,
char
**
/*argv[]*/
)
{
...
...
@@ -448,6 +462,7 @@ int main(int /*argc*/, char** /*argv[]*/)
ExactCubicOneDimTest
(
error
);
ExactCubicVelocityConstraintsTest
(
error
);
EffectorTrajectoryTest
(
error
);
EffectorSplineRotationNoRotationTest
(
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