delphyne
|
A class that wraps a piecewise trajectory instantiated from pose data.
The underlying pose data is represented internally as a configurable PiecewisePolynomial for translation and a PiecewiseQuaternionSlerp for the rotation. The rotational/translational velocity of the pose is then computed from time-differentiation of the PiecewisePolynomial and an angular velocity conversion within PiecewiseQuaternionSlerp (see piecewise_polynomial.h and piecewise_quaternion.h for details). A PoseVelocity instance is therefore well-defined when evaluated at a given time and, additionally, the translaton and rotation components of PoseVector match the input pose data exactly.
#include <src/systems/trajectory.h>
Public Types | |
enum | InterpolationType { kFirstOrderHold, kCubic, kPchip } |
An identifier for the type of valid types of interpolation used in evaluating the translational component of a Trajectory. More... | |
Public Member Functions | |
PoseVelocity | value (double time) const |
Evaluates the Trajectory at a given time , returning a packed PoseVelocity. More... | |
Static Public Member Functions | |
static Trajectory | Make (const std::vector< double > ×, const std::vector< Eigen::Quaternion< double >> &knots_rotation, const std::vector< Eigen::Vector3d > &knots_translation, const InterpolationType &interp_type=InterpolationType::kFirstOrderHold) |
Makes a Trajectory from a discrete set of time-indexed pose data under the specified interpolation scheme. More... | |
static Trajectory | MakeCubicFromWaypoints (const std::vector< Eigen::Quaternion< double >> &waypoints_rotation, const std::vector< Eigen::Vector3d > &waypoints_translation, const std::vector< double > &speeds) |
Makes a Trajectory from a discrete set of (time-independent) waypoints and a vector of speeds. More... | |
static Trajectory | MakeCubicFromWaypoints (const std::vector< Eigen::Quaternion< double >> &waypoints_rotation, const std::vector< Eigen::Vector3d > &waypoints_translation, double speed) |
Makes a Trajectory from a discrete set of (time-independent) waypoints, based on a constant speed, using cubic-polynomial interpolation. More... | |
|
strong |
An identifier for the type of valid types of interpolation used in evaluating the translational component of a Trajectory.
These types mirror the associated constructors for PiecewisePolynomial (see common/trajectories/piecewise_polynomial.h for further details).
Enumerator | |
---|---|
kFirstOrderHold | |
kCubic | |
kPchip |
|
static |
Makes a Trajectory from a discrete set of time-indexed pose data under the specified interpolation scheme.
times | a vector of time indices representing the break points of the trajectory. |
knots_rotation | a vector of time-indexed rotation data, whose length must match that of times . |
knots_translation | a vector of time-indexed translation data, whose length must match that of times . |
interp_type | an InterpolationType with the interpolation scheme used for building a piecewise polynomial trajectory for the translational component. Default: InterpolationType::kFirstOrderHold. |
std::logic_error | if interp_type is not supported. |
std::runtime_error | if times and knots have different lengths, times is not strictly increasing, and the inputs are otherwise inconsistent with the given interp_type (see piecewise_polynomial.h). |
|
static |
Makes a Trajectory from a discrete set of (time-independent) waypoints and a vector of speeds.
The resulting trajectory is assumed to start at time time t = 0 and follow a cubic-spline profile (InterpolationType::kCubic) for the translation elements, rendering speed as a piecewise-quadratic function. For now, we determine the break points (time vector) from the time required to travel a Euclidean distance between consecutive waypoints at the average speed between each. We apply this "average-speed" approach in order not to impose assumptions about the accelerations at the knot points. Velocities at each break point are computed by taking into account both the translation and rotation of A with respect to W.
waypoints_rotation | a vector of rotations, whose length must match that of speeds . |
waypoints_translation | a vector of translations, whose length must match that of speeds . |
speeds | a vector of speeds to be applied at knot points and linearly interpolated between waypoints. All entries of speeds must be non-negative, with some zero entries allowed, so long as they are not consecutive. |
std::exception | if waypoints_* and speeds have different lengths, any are empty, or if any element of speeds violates any of the conditions enumerated above. |
|
static |
Makes a Trajectory from a discrete set of (time-independent) waypoints, based on a constant speed, using cubic-polynomial interpolation.
waypoints_rotation | a vector of rotations, whose length must match that of speeds . |
waypoints_translation | a vector of translations, whose length must match that of speeds . |
speed | a positive speed to be applied over the entirety of the trajectory. |
std::exception | if speed is non-positive or if either of the input vectors is empty. |
PoseVelocity value | ( | double | time | ) | const |
Evaluates the Trajectory at a given time
, returning a packed PoseVelocity.