Drake
trajectory.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <vector>
4 
5 #include <Eigen/Dense>
6 #include <Eigen/Geometry>
7 
11 #include "drake/math/quaternion.h"
14 
15 namespace drake {
16 namespace automotive {
17 
18 /// Wraps the raw data contained in a trajectory expressed in terms of pose and
19 /// velocity values. Represents a translational and rotational transformation
20 /// of a reference frame A with respect to world frame W, expressed in x-y-z
21 /// coordinates, and translational and rotational velocities of frame A with
22 /// respect to W.
23 class PoseVelocity final {
24  public:
26 
27  /// Default constructor. Sets rotation to an identity transform and all
28  /// translation and velocity components to zero.
29  PoseVelocity();
30 
31  /// Fully-parameterized constructor.
32  ///
33  /// @param rotation the orientation R_WA of frame A with respect to the world
34  /// frame W, and can be either normalized or unnormalized.
35  /// @param translation the x, y, z position p_WA of frame A measured from W's
36  /// origin.
37  /// @param velocity the (rotational/translational) spatial velocity Xdot_WA of
38  /// the frame A with respect to frame W.
39  PoseVelocity(const Eigen::Quaternion<double>& rotation,
40  const Eigen::Vector3d& translation,
42 
43  /// Accesses p_WA, the translation component of the pose of A, expressed in
44  /// world-frame coordinates W.
45  const Eigen::Vector3d& translation() const { return translation_; }
46 
47  /// Accesses R_WA, the rotation component of the pose of A, expressed in
48  /// world-frame coordinates W.
49  const Eigen::Quaternion<double>& rotation() const { return rotation_; }
50 
51  /// Accesses the projection of the pose of frame A to the x-y components of
52  /// translation and the z-component of rotation.
53  Eigen::Vector3d pose3() const {
54  const math::RollPitchYaw<double> rpy(rotation_);
55  const double w_z = rpy.yaw_angle();
56  return Eigen::Vector3d{translation_.x(), translation_.y(), w_z};
57  }
58 
59  /// Accesses Xdot_WA, a SpatialVelocity of frame A, expressed in world-frame
60  /// coordinates W.
62  return velocity_;
63  }
64 
65  /// Gets the speed ‖pdot_WA‖₂, the 2-norm of the world-frame translational
66  /// velocities.
67  double speed() const { return velocity_.translational().norm(); }
68 
69  private:
70  Eigen::Quaternion<double> rotation_;
71  Eigen::Vector3d translation_;
73 };
74 
75 /// A class that wraps a piecewise trajectory instantiated from pose data. The
76 /// underlying pose data is represented internally as a configurable
77 /// PiecewisePolynomial for translation and a PiecewiseQuaternionSlerp for the
78 /// rotation. The rotational/translational velocity of the pose is then
79 /// computed from time-differentiation of the PiecewisePolynomial and an angular
80 /// velocity conversion within PiecewiseQuaternionSlerp (see
81 /// piecewise_polynomial.h and piecewise_quaternion.h for details). A
82 /// PoseVelocity instance is therefore well-defined when evaluated at a given
83 /// time and, additionally, the translaton and rotation components of PoseVector
84 /// match the input pose data exactly.
85 class Trajectory final {
86  public:
88 
89  /// An identifier for the type of valid types of interpolation used in
90  /// evaluating the translational component of a Trajectory. These types
91  /// mirror the associated constructors for PiecewisePolynomial (see
92  /// common/trajectories/piecewise_polynomial.h for further details).
93  enum class InterpolationType { kFirstOrderHold, kCubic, kPchip };
94 
95  /// Makes a Trajectory from a discrete set of time-indexed pose data
96  /// under the specified interpolation scheme.
97  ///
98  /// @param times a vector of time indices representing the break points of the
99  /// trajectory.
100  /// @param knots_rotation a vector of time-indexed rotation data, whose length
101  /// must match that of @p times.
102  /// @param knots_translation a vector of time-indexed translation data, whose
103  /// length must match that of @p times.
104  /// @param interp_type an InterpolationType with the interpolation scheme used
105  /// for building a piecewise polynomial trajectory for the translational
106  /// component.
107  /// @default InterpolationType::kFirstOrderHold.
108  /// @throws std::logic_error if @p interp_type is not supported.
109  /// @throws std::runtime_error if `times` and `knots` have different lengths,
110  /// `times` is not strictly increasing, and the inputs are otherwise
111  /// inconsistent with the given `interp_type` (see piecewise_polynomial.h).
112  static Trajectory Make(
113  const std::vector<double>& times,
114  const std::vector<Eigen::Quaternion<double>>& knots_rotation,
115  const std::vector<Eigen::Vector3d>& knots_translation,
116  const InterpolationType& interp_type =
117  InterpolationType::kFirstOrderHold);
118 
119  /// Makes a Trajectory from a discrete set of (time-independent)
120  /// waypoints and a vector of speeds. The resulting trajectory is assumed to
121  /// start at time time t = 0 and follow a cubic-spline profile
122  /// (InterpolationType::kCubic) for the translation elements, rendering speed
123  /// as a piecewise-quadratic function. For now, we determine the break points
124  /// (time vector) from the time required to travel a Euclidean distance
125  /// between consecutive waypoints at the average speed between each. We apply
126  /// this "average-speed" approach in order not to impose assumptions about the
127  /// accelerations at the knot points. Velocities at each break point are
128  /// computed by taking into account both the translation and rotation of A
129  /// with respect to W.
130  ///
131  /// @param waypoints_rotation a vector of rotations, whose length must match
132  /// that of @p speeds.
133  /// @param waypoints_translation a vector of translations, whose length must
134  /// match that of @p speeds.
135  /// @param speeds a vector of speeds to be applied at knot points and linearly
136  /// interpolated between waypoints. All entries of @p speeds must be
137  /// non-negative, with some zero entries allowed, so long as they are not
138  /// consecutive.
139  /// @throws std::exception if `waypoints_*` and `speeds` have different
140  /// lengths, any are empty, or if any element of `speeds` violates any of the
141  /// conditions enumerated above.
142  //
143  // TODO(jadecastro) Compute the break points as the solution to an
144  // optimization problem or from additional user inputs.
145  static Trajectory MakeCubicFromWaypoints(
146  const std::vector<Eigen::Quaternion<double>>& waypoints_rotation,
147  const std::vector<Eigen::Vector3d>& waypoints_translation,
148  const std::vector<double>& speeds);
149 
150  /// Makes a Trajectory from a discrete set of (time-independent)
151  /// waypoints, based on a constant speed, using cubic-polynomial
152  /// interpolation.
153  ///
154  /// @param waypoints_rotation a vector of rotations, whose length must match
155  /// that of @p speeds.
156  /// @param waypoints_translation a vector of translations, whose length must
157  /// match that of @p speeds.
158  /// @param speed a positive speed to be applied over the entirety of the
159  /// trajectory.
160  /// @throws std::exception if `speed` is non-positive or if either of the
161  /// input vectors is empty.
162  static Trajectory MakeCubicFromWaypoints(
163  const std::vector<Eigen::Quaternion<double>>& waypoints_rotation,
164  const std::vector<Eigen::Vector3d>& waypoints_translation, double speed);
165 
166  /// Evaluates the Trajectory at a given @p time, returning a packed
167  /// PoseVelocity.
168  PoseVelocity value(double time) const;
169 
170  private:
171  // Constructs a Trajectory from a translation PiecewisePolynomial, @p
172  // translation, and a rotation PiecewiseQuaternionSlerp, @p rotation.
173  explicit Trajectory(
176 
180 };
181 
182 } // namespace automotive
183 } // namespace drake
This class represents the orientation between two arbitrary frames A and D associated with a Space-fi...
Definition: roll_pitch_yaw.h:65
const multibody::SpatialVelocity< double > & velocity() const
Accesses Xdot_WA, a SpatialVelocity of frame A, expressed in world-frame coordinates W...
Definition: trajectory.h:61
double value
Definition: wrap_test_util_py.cc:12
Definition: automotive_demo.cc:90
PoseVelocity()
Default constructor.
Definition: trajectory.cc:27
std::vector< double > times
Definition: pick_and_place_state_machine.cc:36
std::vector< double > vector
Definition: translator_test.cc:20
const T & yaw_angle() const
Returns the yaw-angle underlying this RollPitchYaw.
Definition: roll_pitch_yaw.h:127
InterpolationType
An identifier for the type of valid types of interpolation used in evaluating the translational compo...
Definition: trajectory.h:93
#define DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Classname)
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN defaults the special member functions for copy-construction, copy-assignment, move-construction, and move-assignment.
Definition: drake_copyable.h:57
Eigen::Vector3d rpy
Definition: rgbd_camera_publish_lcm_example.cc:79
const Eigen::Vector3d & translation() const
Accesses p_WA, the translation component of the pose of A, expressed in world-frame coordinates W...
Definition: trajectory.h:45
const double time
Definition: robot_plan_interpolator_test.cc:64
Utilities for arithmetic on quaternions.
A class that wraps a piecewise trajectory instantiated from pose data.
Definition: trajectory.h:85
Wraps the raw data contained in a trajectory expressed in terms of pose and velocity values...
Definition: trajectory.h:23
const Vector3< T > & translational() const
Const access to the translational component of this spatial vector.
Definition: spatial_vector.h:106
Eigen::Vector3d pose3() const
Accesses the projection of the pose of frame A to the x-y components of translation and the z-compone...
Definition: trajectory.h:53
double speed() const
Gets the speed ‖pdot_WA‖₂, the 2-norm of the world-frame translational velocities.
Definition: trajectory.h:67
Provides careful macros to selectively enable or disable the special member functions for copy-constr...
const Eigen::Quaternion< double > & rotation() const
Accesses R_WA, the rotation component of the pose of A, expressed in world-frame coordinates W...
Definition: trajectory.h:49