Drake
piecewise_quaternion.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <vector>
4 
9 
10 namespace drake {
11 
30 // TODO(siyuan.feng): check if this works for AutoDiffScalar.
31 template <typename Scalar = double>
33  public:
35 
37 
38  /*
39  * Builds a PiecewiseQuaternionSlerp.
40  * @throw if breaks and quaternions have different length,
41  * or breaks have length < 2.
42  */
44  const std::vector<double>& breaks,
45  const eigen_aligned_std_vector<Quaternion<Scalar>>& quaternions);
46 
47  /*
48  * Builds a PiecewiseQuaternionSlerp.
49  * @throw if breaks and rot_matrices have different length,
50  * or breaks have length < 2.
51  */
53  const std::vector<double>& breaks,
54  const eigen_aligned_std_vector<Matrix3<Scalar>>& rot_matrices);
55 
56  /*
57  * Builds a PiecewiseQuaternionSlerp.
58  * @throw if breaks and ang_axes have different length,
59  * or breaks have length < 2.
60  */
62  const std::vector<double>& breaks,
64 
65  Eigen::Index rows() const override { return 4; }
66  Eigen::Index cols() const override { return 1; }
67 
73  Quaternion<Scalar> orientation(double t) const;
74 
81  Vector3<Scalar> angular_velocity(double t) const;
82 
89  Vector3<Scalar> angular_acceleration(double t) const;
90 
100  const {
101  return quaternions_;
102  }
103 
110  const Scalar& tol) const;
111 
112  private:
113  // Initialize quaternions_ and computes angular velocity for each segment.
114  void Initialize(
115  const std::vector<double>& breaks,
116  const eigen_aligned_std_vector<Quaternion<Scalar>>& quaternions);
117 
118  // Computes angular velocity for each segment.
119  void ComputeAngularVelocities();
120 
121  // Computes the interpolation time within each segment. Result is in [0, 1].
122  double ComputeInterpTime(int segment_index, double time) const;
123 
125  eigen_aligned_std_vector<Vector3<Scalar>> angular_velocities_;
126 };
127 
128 } // namespace drake
Eigen::Index cols() const override
Definition: piecewise_quaternion.h:66
A class representing a trajectory for quaternions that are interpolated using piecewise slerp (spheri...
Definition: piecewise_quaternion.h:32
bool is_approx(const PiecewiseQuaternionSlerp< Scalar > &other, const Scalar &tol) const
Returns true if all the corresponding segment times are within tol seconds, and the angle difference ...
Definition: piecewise_quaternion.cc:10
Eigen::Quaternion< Scalar > Quaternion
A quaternion templated on scalar type.
Definition: eigen_types.h:98
Eigen::AngleAxis< Scalar > AngleAxis
An AngleAxis templated on scalar type.
Definition: eigen_types.h:102
This file contains abbreviated definitions for certain specializations of Eigen::Matrix that are comm...
Vector3< Scalar > angular_velocity(double t) const
Interpolates angular velocity.
Definition: piecewise_quaternion.cc:130
Definition: automotive_demo.cc:88
Definition: piecewise_function.h:8
Vector3< Scalar > angular_acceleration(double t) const
Interpolates angular acceleration.
Definition: piecewise_quaternion.cc:138
std::vector< T, Eigen::aligned_allocator< T >> eigen_aligned_std_vector
A std::vector that uses Eigen::aligned_allocator so that the contained types may be fixed-size Eigen ...
Definition: eigen_stl_types.h:38
This file contains definitions for using Eigen with the STL.
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:59
Quaternion< Scalar > orientation(double t) const
Interpolates orientation.
Definition: piecewise_quaternion.cc:116
std::vector< double > breaks
Definition: piecewise_function.h:10
const eigen_aligned_std_vector< Quaternion< Scalar > > & get_quaternion_knots() const
Getter for the internal quaternion knots.
Definition: piecewise_quaternion.h:99
PiecewiseQuaternionSlerp()
Definition: piecewise_quaternion.h:36
Eigen::Index rows() const override
Definition: piecewise_quaternion.h:65
const double time
Definition: robot_plan_interpolator_test.cc:63
Eigen::Matrix< Scalar, 3, 3 > Matrix3
A matrix of 3 rows and 3 columns, templated on scalar type.
Definition: eigen_types.h:59
Provides careful macros to selectively enable or disable the special member functions for copy-constr...