Drake
exponential_plus_piecewise_polynomial.h
Go to the documentation of this file.
1 #pragma once
2
3 #include <memory>
4 #include <vector>
5
6 #include <Eigen/Core>
7
11
12
13 namespace drake {
14 namespace trajectories {
15
16 /**
17  * y(t) = K * exp(A * (t - t_j)) * alpha.col(j) + piecewise_polynomial_part(t)
18  */
19 template <typename T>
21  : public PiecewiseTrajectory<T> {
22  public:
23  // We are final, so this is okay.
25
27
28  template <typename DerivedK, typename DerivedA, typename DerivedAlpha>
30  const Eigen::MatrixBase<DerivedK>& K,
31  const Eigen::MatrixBase<DerivedA>& A,
32  const Eigen::MatrixBase<DerivedAlpha>& alpha,
33  const PiecewisePolynomial<T>& piecewise_polynomial_part)
34  : PiecewiseTrajectory<T>(piecewise_polynomial_part),
35  K_(K),
36  A_(A),
37  alpha_(alpha),
38  piecewise_polynomial_part_(piecewise_polynomial_part) {
39  DRAKE_ASSERT(K.rows() == rows());
40  DRAKE_ASSERT(K.cols() == A.rows());
41  DRAKE_ASSERT(A.rows() == A.cols());
42  DRAKE_ASSERT(alpha.rows() == A.cols());
43  DRAKE_ASSERT(alpha.cols() ==
44  piecewise_polynomial_part.get_number_of_segments());
45  DRAKE_ASSERT(piecewise_polynomial_part.rows() == rows());
46  DRAKE_ASSERT(piecewise_polynomial_part.cols() == 1);
47  }
48
49  // from PiecewisePolynomial
51  const PiecewisePolynomial<T>& piecewise_polynomial_part);
52
53  ~ExponentialPlusPiecewisePolynomial() override = default;
54
55  std::unique_ptr<Trajectory<T>> Clone() const override;
56
57  MatrixX<T> value(double t) const override;
58
59  ExponentialPlusPiecewisePolynomial derivative(int derivative_order = 1) const;
60
61  std::unique_ptr<Trajectory<T>> MakeDerivative(
62  int derivative_order = 1) const override {
63  return derivative(derivative_order).Clone();
64  };
65
66  Eigen::Index rows() const override;
67
68  Eigen::Index cols() const override;
69
70  void shiftRight(double offset);
71
72  private:
73  MatrixX<T> K_;
74  MatrixX<T> A_;
75  MatrixX<T> alpha_;
76  PiecewisePolynomial<T> piecewise_polynomial_part_;
77 };
78
79 } // namespace trajectories
80 } // namespace drake
ExponentialPlusPiecewisePolynomial(const Eigen::MatrixBase< DerivedK > &K, const Eigen::MatrixBase< DerivedA > &A, const Eigen::MatrixBase< DerivedAlpha > &alpha, const PiecewisePolynomial< T > &piecewise_polynomial_part)
Definition: exponential_plus_piecewise_polynomial.h:29
This file contains abbreviated definitions for certain specializations of Eigen::Matrix that are comm...
Definition: automotive_demo.cc:105
Eigen::Index cols() const override
Definition: exponential_plus_piecewise_polynomial.cc:61
Eigen::Index rows() const override
Returns the row count of each and every PolynomialMatrix segment.
Definition: piecewise_polynomial.cc:349
ExponentialPlusPiecewisePolynomial derivative(int derivative_order=1) const
Definition: exponential_plus_piecewise_polynomial.cc:42
std::vector< snopt::doublereal > A
Definition: snopt_solver.cc:93
#define DRAKE_ASSERT(condition)
DRAKE_ASSERT(condition) is similar to the built-in assert(condition) from the C++ system header <cas...
Definition: drake_assert.h:37
#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
A scalar multi-variate piecewise polynomial.
Definition: piecewise_polynomial.h:47
Provides Drake&#39;s assertion implementation.
MatrixX< T > value(double t) const override
Evaluates the trajectory at the given time t.
Definition: exponential_plus_piecewise_polynomial.cc:31
y(t) = K * exp(A * (t - t_j)) * alpha.col(j) + piecewise_polynomial_part(t)
Definition: exponential_plus_piecewise_polynomial.h:20
int get_number_of_segments() const
Definition: piecewise_trajectory.cc:29
std::unique_ptr< Trajectory< T > > MakeDerivative(int derivative_order=1) const override
Takes the derivative of this Trajectory.
Definition: exponential_plus_piecewise_polynomial.h:61
std::unique_ptr< Trajectory< T > > Clone() const override
Definition: exponential_plus_piecewise_polynomial.cc:25
Eigen::Index rows() const override
Definition: exponential_plus_piecewise_polynomial.cc:56
Abstract class that implements the basic logic of maintaining consequent segments of time (delimited ...
Definition: piecewise_trajectory.h:21
Provides careful macros to selectively enable or disable the special member functions for copy-constr...
Eigen::Index cols() const override
Returns the column count of each and every PolynomialMatrix segment.
Definition: piecewise_polynomial.cc:358
void shiftRight(double offset)
Definition: exponential_plus_piecewise_polynomial.cc:66