Drake
piecewise_quaternion.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <memory>
4 #include <vector>
5 
9 
10 namespace drake {
11 namespace trajectories {
12 
13 /**
14  * A class representing a trajectory for quaternions that are interpolated
15  * using piecewise slerp (spherical linear interpolation).
16  * All the orientation knots are expected to be with respect to the same
17  * parent reference frame, i.e. q_i represents the rotation R_PBi for the
18  * orientation of frame B at the ith knot in a fixed parent frame P.
19  * The world frame is a common choice for the parent frame.
20  * The angular velocity and acceleration are also relative to the parent frame
21  * and expressed in the parent frame.
22  * Since there is a sign ambiguity when using quaternions to represent
23  * orientation, namely q and -q represent the same orientation, the internal
24  * quaternion representations ensure that q_n.dot(q_{n+1}) >= 0.
25  * Another intuitive way to think about this is that consecutive quaternions
26  * have the shortest geodesic distance on the unit sphere.
27  *
28  * @tparam T, double.
29  *
30  */
31 // TODO(siyuan.feng): check if this works for AutoDiffScalar.
32 template<typename T>
34  public:
36 
37  /**
38  * Builds an empty PiecewiseQuaternionSlerp.
39  */
40  PiecewiseQuaternionSlerp() = default;
41 
42  /**
43  * Builds a PiecewiseQuaternionSlerp.
44  * @throws std::logic_error if breaks and quaternions have different length,
45  * or breaks have length < 2.
46  */
48  const std::vector<double>& breaks,
49  const std::vector<Quaternion<T>>& quaternions);
50 
51  /**
52  * Builds a PiecewiseQuaternionSlerp.
53  * @throws std::logic_error if breaks and rot_matrices have different length,
54  * or breaks have length < 2.
55  */
57  const std::vector<double>& breaks,
58  const std::vector<Matrix3<T>>& rot_matrices);
59 
60  /**
61  * Builds a PiecewiseQuaternionSlerp.
62  * @throws std::logic_error if breaks and ang_axes have different length,
63  * or breaks have length < 2.
64  */
66  const std::vector<double>& breaks,
67  const std::vector<AngleAxis<T>>& ang_axes);
68 
69  ~PiecewiseQuaternionSlerp() override = default;
70 
71  std::unique_ptr<Trajectory<T>> Clone() const override;
72 
73  Eigen::Index rows() const override { return 4; }
74 
75  Eigen::Index cols() const override { return 1; }
76 
77  /**
78  * Interpolates orientation.
79  * @param t Time for interpolation.
80  * @return The interpolated quaternion at `t`.
81  */
82  Quaternion<T> orientation(double t) const;
83 
84  MatrixX<T> value(double t) const override { return orientation(t).matrix(); }
85 
86  /**
87  * Interpolates angular velocity.
88  * @param t Time for interpolation.
89  * @return The interpolated angular velocity at `t`,
90  * which is constant per segment.
91  */
92  Vector3<T> angular_velocity(double t) const;
93 
94  /**
95  * @throws std::runtime_error (always) because it is not implemented yet.
96  */
97  // TODO(russt): Implement this if/when someone needs it!
98  std::unique_ptr<Trajectory<T>> MakeDerivative(
99  int derivative_order = 1) const override;
100 
101  /**
102  * Interpolates angular acceleration.
103  * @param t Time for interpolation.
104  * @return The interpolated angular acceleration at `t`,
105  * which is always zero for slerp.
106  */
107  Vector3<T> angular_acceleration(double t) const;
108 
109  /**
110  * Getter for the internal quaternion knots.
111  * Note: the returned quaternions might be different from the ones used for
112  * construction because the internal representations are set to always be
113  * the "closest" w.r.t to the previous one.
114  *
115  * @return the internal knot points.
116  */
117  const std::vector<Quaternion<T>>& get_quaternion_knots() const {
118  return quaternions_;
119  }
120 
121  /**
122  * Returns true if all the corresponding segment times are within
123  * @p tol seconds, and the angle difference between the corresponding
124  * quaternion knot points are within @p tol.
125  */
126  bool is_approx(const PiecewiseQuaternionSlerp<T>& other,
127  const T& tol) const;
128 
129  private:
130  // Initialize quaternions_ and computes angular velocity for each segment.
131  void Initialize(
132  const std::vector<double>& breaks,
133  const std::vector<Quaternion<T>>& quaternions);
134 
135  // Computes angular velocity for each segment.
136  void ComputeAngularVelocities();
137 
138  // Computes the interpolation time within each segment. Result is in [0, 1].
139  double ComputeInterpTime(int segment_index, double time) const;
140 
141  std::vector<Quaternion<T>> quaternions_;
142  std::vector<Vector3<T>> angular_velocities_;
143 };
144 
145 } // namespace trajectories
146 } // namespace drake
PiecewiseQuaternionSlerp()=default
Builds an empty PiecewiseQuaternionSlerp.
Eigen::Quaternion< Scalar > Quaternion
A quaternion templated on scalar type.
Definition: eigen_types.h:119
Eigen::AngleAxis< Scalar > AngleAxis
An AngleAxis templated on scalar type.
Definition: eigen_types.h:123
This file contains abbreviated definitions for certain specializations of Eigen::Matrix that are comm...
Definition: automotive_demo.cc:105
bool is_approx(const PiecewiseQuaternionSlerp< T > &other, const T &tol) const
Returns true if all the corresponding segment times are within tol seconds, and the angle difference ...
Definition: piecewise_quaternion.cc:12
Eigen::Index rows() const override
Definition: piecewise_quaternion.h:73
std::unique_ptr< Trajectory< T > > MakeDerivative(int derivative_order=1) const override
Definition: piecewise_quaternion.cc:145
Eigen::Index cols() const override
Definition: piecewise_quaternion.h:75
std::vector< double > vector
Definition: translator_test.cc:20
Eigen::Matrix< Scalar, 3, 1 > Vector3
A column vector of size 3, templated on scalar type.
Definition: eigen_types.h:34
#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::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > MatrixX
A matrix of dynamic size, templated on scalar type.
Definition: eigen_types.h:108
Vector3< T > angular_velocity(double t) const
Interpolates angular velocity.
Definition: piecewise_quaternion.cc:138
MatrixX< T > value(double t) const override
Evaluates the trajectory at the given time t.
Definition: piecewise_quaternion.h:84
Quaternion< T > orientation(double t) const
Interpolates orientation.
Definition: piecewise_quaternion.cc:125
std::unique_ptr< Trajectory< T > > Clone() const override
Definition: piecewise_quaternion.cc:110
const double time
Definition: robot_plan_interpolator_test.cc:64
Vector3< T > angular_acceleration(double t) const
Interpolates angular acceleration.
Definition: piecewise_quaternion.cc:152
Eigen::Matrix< Scalar, 3, 3 > Matrix3
A matrix of 3 rows and 3 columns, templated on scalar type.
Definition: eigen_types.h:80
const std::vector< Quaternion< T > > & get_quaternion_knots() const
Getter for the internal quaternion knots.
Definition: piecewise_quaternion.h:117
const std::vector< double > & breaks() const
Definition: piecewise_trajectory.h:66
Abstract class that implements the basic logic of maintaining consequent segments of time (delimited ...
Definition: piecewise_trajectory.h:21
A class representing a trajectory for quaternions that are interpolated using piecewise slerp (spheri...
Definition: piecewise_quaternion.h:33
Provides careful macros to selectively enable or disable the special member functions for copy-constr...