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
5ead2d60
Commit
5ead2d60
authored
Jan 25, 2017
by
Steve Tonneau
Browse files
helper method for effector splines
parent
0a87d54b
Changes
4
Hide whitespace changes
Inline
Side-by-side
include/spline/exact_cubic.h
View file @
5ead2d60
...
...
@@ -152,7 +152,7 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
/*Operations*/
public:
/// \brief Evaluation of the cubic spline at time t.
/// \param t : the time when to evaluate the spine
/// \param t : the time when to evaluate the sp
l
ine
/// \param return : the value x(t)
virtual
point_t
operator
()(
time_t
t
)
const
{
...
...
@@ -167,7 +167,7 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
}
/// \brief Evaluation of the derivative spline at time t.
/// \param t : the time when to evaluate the spine
/// \param t : the time when to evaluate the sp
l
ine
/// \param return : the value x(t)
virtual
point_t
derivate
(
time_t
t
,
std
::
size_t
order
)
const
{
...
...
include/spline/helpers/effector_spline.h
0 → 100644
View file @
5ead2d60
/**
* \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 2d or 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 time_lift_offset : time travelled along straight line at take-off
/// \param time_land_offset : 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
)
{
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
));
//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
);
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_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/spline_deriv_constraint.h
View file @
5ead2d60
...
...
@@ -70,7 +70,8 @@ struct spline_deriv_constraint : public exact_cubic<Time, Numeric, Dim, Safe, Po
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
///\param wayPointsEnd : an iterator pointing to the end of a waypoint container
///\param constraints : constraints on the init and end velocity / accelerations of the spline
template
<
typename
In
>
spline_deriv_constraint
(
In
wayPointsBegin
,
In
wayPointsEnd
,
const
spline_constraints
&
constraints
=
spline_constraints
())
:
exact_cubic_t
(
computeWayPoints
<
In
>
(
wayPointsBegin
,
wayPointsEnd
,
constraints
))
{}
...
...
@@ -143,10 +144,6 @@ struct spline_deriv_constraint : public exact_cubic<Time, Numeric, Dim, Safe, Po
spline_deriv_constraint
(
const
spline_deriv_constraint
&
);
spline_deriv_constraint
&
operator
=
(
const
spline_deriv_constraint
&
);
/* Constructors - destructors */
/*Attributes*/
public:
const
t_spline_t
subSplines_
;
/*Attributes*/
};
}
#endif //_CLASS_CUBICZEROVELACC
...
...
src/tests/spline_test/Main.cpp
View file @
5ead2d60
...
...
@@ -3,6 +3,7 @@
#include
"spline/bezier_curve.h"
#include
"spline/spline_curve.h"
#include
"spline/spline_deriv_constraint.h"
#include
"spline/helpers/effector_spline.h"
#include
<string>
#include
<iostream>
...
...
@@ -378,6 +379,63 @@ void ExactCubicVelocityConstraintsTest(bool& error)
CheckDerivative
(
errmsg4
,
1
,
2
,
constraints
.
end_acc
,
&
exactCubic2
,
error
);
}
void
CheckPointOnline
(
const
std
::
string
&
errmsg
,
const
point_t
&
A
,
const
point_t
&
B
,
const
double
target
,
const
exact_cubic_t
*
curve
,
bool
&
error
)
{
point_t
res1
=
curve
->
operator
()(
target
);
point_t
ar
=
(
res1
-
A
);
ar
.
normalize
();
point_t
rb
=
(
B
-
res1
);
rb
.
normalize
();
if
(
ar
.
dot
(
rb
)
<
0.99999
)
{
error
=
true
;
std
::
cout
<<
errmsg
<<
" ; "
<<
A
.
transpose
()
<<
"
\n
; "
<<
B
.
transpose
()
<<
"
\n
; "
<<
target
<<
" ; "
<<
res1
.
transpose
()
<<
std
::
endl
;
}
}
void
EffectorTrajectoryTest
(
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
::
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
);
point_t
zero
(
0
,
0
,
0
);
point_t
off1
(
0
,
0
,
1
);
point_t
off2
(
10
,
10
,
11
);
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)"
);
//first check start / goal positions
ComparePoints
(
zero
,
(
*
eff_traj
)(
0
),
errmsg
,
error
);
ComparePoints
(
off1
,
(
*
eff_traj
)(
1
),
errmsg
,
error
);
ComparePoints
(
off2
,
(
*
eff_traj
)(
9.5
),
errmsg
,
error
);
ComparePoints
(
end
,
(
*
eff_traj
)(
10
),
errmsg
,
error
);
//then check offset at start / goal positions
// now check derivatives
CheckDerivative
(
errmsg2
,
0
,
1
,
zero
,
eff_traj
,
error
);
CheckDerivative
(
errmsg2
,
10
,
1
,
zero
,
eff_traj
,
error
);
CheckDerivative
(
errmsg2
,
0
,
2
,
zero
,
eff_traj
,
error
);
CheckDerivative
(
errmsg2
,
10
,
2
,
zero
,
eff_traj
,
error
);
//check that end and init splines are line
std
::
string
errmsg3
(
"Error in EffectorTrajectoryTest; while checking that init/end splines are line (point A/ point B, time value / point obtained)
\n
"
);
for
(
double
i
=
0.1
;
i
<
1
;
i
+=
0.1
)
{
CheckPointOnline
(
errmsg3
,(
*
eff_traj
)(
0
),(
*
eff_traj
)(
1
),
i
,
eff_traj
,
error
);
}
for
(
double
i
=
9.6
;
i
<
10
;
i
+=
0.1
)
{
CheckPointOnline
(
errmsg3
,(
*
eff_traj
)(
9.5
),(
*
eff_traj
)(
10
),
i
,
eff_traj
,
error
);
}
}
int
main
(
int
/*argc*/
,
char
**
/*argv[]*/
)
{
...
...
@@ -389,10 +447,11 @@ int main(int /*argc*/, char** /*argv[]*/)
ExactCubicTwoPointsTest
(
error
);
ExactCubicOneDimTest
(
error
);
ExactCubicVelocityConstraintsTest
(
error
);
EffectorTrajectoryTest
(
error
);
//BezierCurveTest(error);
if
(
error
)
{
std
::
cout
<<
"There were some errors
\n
"
;
std
::
cout
<<
"There were some errors
\n
"
;
return
-
1
;
}
else
...
...
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