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
c55a3f93
Commit
c55a3f93
authored
Aug 08, 2019
by
JasonChmn
Committed by
Pierre Fernbach
Sep 03, 2019
Browse files
Remove aligned_vector file / redo indentation in helpers
parent
882a795b
Changes
4
Expand all
Hide whitespace changes
Inline
Side-by-side
include/curves/helpers/effector_spline.h
View file @
c55a3f93
...
...
@@ -24,97 +24,96 @@
namespace
curves
{
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
exact_cubic
<
Time
,
Numeric
,
3
,
true
,
Point
,
T_Point
>
exact_cubic_t
;
typedef
exact_cubic_t
::
spline_constraints
spline_constraints_t
;
typedef
exact_cubic_t
::
t_spline_t
t_spline_t
;
typedef
exact_cubic_t
::
spline_t
spline_t
;
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
exact_cubic
<
Time
,
Numeric
,
3
,
true
,
Point
,
T_Point
>
exact_cubic_t
;
typedef
exact_cubic_t
::
spline_constraints
spline_constraints_t
;
typedef
exact_cubic_t
::
t_spline_t
t_spline_t
;
typedef
exact_cubic_t
::
spline_t
spline_t
;
/// \brief Compute time such that the equation from source to offsetpoint is necessarily a line.
Waypoint
compute_offset
(
const
Waypoint
&
source
,
const
Point
&
normal
,
const
Numeric
offset
,
const
Time
time_offset
)
{
Numeric
norm
=
normal
.
norm
();
assert
(
norm
>
0.
);
return
std
::
make_pair
(
source
.
first
+
time_offset
,(
source
.
second
+
normal
/
norm
*
offset
));
}
/// \brief Compute time such that the equation from source to offsetpoint is necessarily a line.
Waypoint
compute_offset
(
const
Waypoint
&
source
,
const
Point
&
normal
,
const
Numeric
offset
,
const
Time
time_offset
)
{
Numeric
norm
=
normal
.
norm
();
assert
(
norm
>
0.
);
return
std
::
make_pair
(
source
.
first
+
time_offset
,(
source
.
second
+
normal
/
norm
*
offset
));
}
/// \brief Compute spline from land way point to end point.
/// Constraints are null velocity and acceleration.
spline_t
make_end_spline
(
const
Point
&
normal
,
const
Point
&
from
,
const
Numeric
offset
,
const
Time
init_time
,
const
Time
time_offset
)
{
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
;
/// \brief Compute spline from land way point to end point.
/// Constraints are null velocity and acceleration.
spline_t
make_end_spline
(
const
Point
&
normal
,
const
Point
&
from
,
const
Numeric
offset
,
const
Time
init_time
,
const
Time
time_offset
)
{
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
);
}
T_Point
points
;
points
.
push_back
(
from
);
points
.
push_back
(
b
);
points
.
push_back
(
c
);
points
.
push_back
(
d
);
/// \brief Compute end velocity : along landing normal and respecting time.
spline_constraints_t
compute_required_offset_velocity_acceleration
(
const
spline_t
&
end_spline
,
const
Time
/*time_offset*/
)
{
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
;
}
return
spline_t
(
points
.
begin
(),
points
.
end
(),
init_time
,
init_time
+
time_offset
);
}
/// \brief Compute end velocity : along landing normal and respecting time.
spline_constraints_t
compute_required_offset_velocity_acceleration
(
const
spline_t
&
end_spline
,
const
Time
/*time_offset*/
)
{
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 last element 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
);
exact_cubic_t
all_but_end
(
waypoints
.
begin
(),
waypoints
.
end
(),
constraints
);
t_spline_t
splines
=
all_but_end
.
curves_
;
splines
.
push_back
(
end_spline
);
return
new
exact_cubic_t
(
splines
);
}
}
// namespace helpers
/// \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 last element 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
);
exact_cubic_t
all_but_end
(
waypoints
.
begin
(),
waypoints
.
end
(),
constraints
);
t_spline_t
splines
=
all_but_end
.
curves_
;
splines
.
push_back
(
end_spline
);
return
new
exact_cubic_t
(
splines
);
}
}
// namespace helpers
}
// namespace curves
#endif //_CLASS_EFFECTORSPLINE
include/curves/helpers/effector_spline_rotation.h
View file @
c55a3f93
This diff is collapsed.
Click to expand it.
include/curves/optimization/OptimizeSpline.h
View file @
c55a3f93
This diff is collapsed.
Click to expand it.
include/curves/serialization/CMakeLists.txt
View file @
c55a3f93
SET
(
${
PROJECT_NAME
}
_SERIALIZATION_HEADERS
aligned-vector.hpp
archive.hpp
eigen-matrix.hpp
fwd.hpp
...
...
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