-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
exponential_plus_piecewise_polynomial.h
82 lines (64 loc) · 2.34 KB
/
exponential_plus_piecewise_polynomial.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
#pragma once
#include <memory>
#include <vector>
#include <Eigen/Core>
#include "drake/common/drake_assert.h"
#include "drake/common/drake_copyable.h"
#include "drake/common/eigen_types.h"
#include "drake/common/trajectories/piecewise_polynomial.h"
namespace drake {
namespace trajectories {
/**
* y(t) = K * exp(A * (t - t_j)) * alpha.col(j) + piecewise_polynomial_part(t)
*
* @tparam_double_only
*/
template <typename T>
class ExponentialPlusPiecewisePolynomial final
: public PiecewiseTrajectory<T> {
public:
// We are final, so this is okay.
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(ExponentialPlusPiecewisePolynomial)
ExponentialPlusPiecewisePolynomial() = default;
template <typename DerivedK, typename DerivedA, typename DerivedAlpha>
ExponentialPlusPiecewisePolynomial(
const Eigen::MatrixBase<DerivedK>& K,
const Eigen::MatrixBase<DerivedA>& A,
const Eigen::MatrixBase<DerivedAlpha>& alpha,
const PiecewisePolynomial<T>& piecewise_polynomial_part)
: PiecewiseTrajectory<T>(piecewise_polynomial_part),
K_(K),
A_(A),
alpha_(alpha),
piecewise_polynomial_part_(piecewise_polynomial_part) {
DRAKE_ASSERT(K.rows() == rows());
DRAKE_ASSERT(K.cols() == A.rows());
DRAKE_ASSERT(A.rows() == A.cols());
DRAKE_ASSERT(alpha.rows() == A.cols());
DRAKE_ASSERT(alpha.cols() ==
piecewise_polynomial_part.get_number_of_segments());
DRAKE_ASSERT(piecewise_polynomial_part.rows() == rows());
DRAKE_ASSERT(piecewise_polynomial_part.cols() == 1);
}
// from PiecewisePolynomial
ExponentialPlusPiecewisePolynomial(
const PiecewisePolynomial<T>& piecewise_polynomial_part);
~ExponentialPlusPiecewisePolynomial() override = default;
std::unique_ptr<Trajectory<T>> Clone() const override;
MatrixX<T> value(const T& t) const override;
ExponentialPlusPiecewisePolynomial derivative(int derivative_order = 1) const;
Eigen::Index rows() const override;
Eigen::Index cols() const override;
void shiftRight(double offset);
private:
std::unique_ptr<Trajectory<T>> DoMakeDerivative(
int derivative_order = 1) const override {
return derivative(derivative_order).Clone();
};
MatrixX<T> K_;
MatrixX<T> A_;
MatrixX<T> alpha_;
PiecewisePolynomial<T> piecewise_polynomial_part_;
};
} // namespace trajectories
} // namespace drake