-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
trajectory.h
113 lines (91 loc) · 3.38 KB
/
trajectory.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
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
#pragma once
#include <memory>
#include <vector>
#include <Eigen/Core>
#include "drake/common/default_scalars.h"
#include "drake/common/drake_copyable.h"
#include "drake/common/eigen_types.h"
namespace drake {
namespace trajectories {
/**
* A Trajectory represents a time-varying matrix, indexed by a single scalar
* time.
*
* @tparam_default_scalar
*/
template <typename T>
class Trajectory {
public:
virtual ~Trajectory() = default;
/**
* @return A deep copy of this Trajectory.
*/
virtual std::unique_ptr<Trajectory<T>> Clone() const = 0;
/**
* Evaluates the trajectory at the given time \p t.
* @param t The time at which to evaluate the trajectory.
* @return The matrix of evaluated values.
*/
virtual MatrixX<T> value(const T& t) const = 0;
/**
* If cols()==1, then evaluates the trajectory at each time @p t, and returns
* the results as a Matrix with the ith column corresponding to the ith time.
* Otherwise, if rows()==1, then evaluates the trajectory at each time @p t,
* and returns the results as a Matrix with the ith row corresponding to
* the ith time.
* @throws std::exception if both cols and rows are not equal to 1.
*/
MatrixX<T> vector_values(const std::vector<T>& t) const;
/**
* If cols()==1, then evaluates the trajectory at each time @p t, and returns
* the results as a Matrix with the ith column corresponding to the ith time.
* Otherwise, if rows()==1, then evaluates the trajectory at each time @p t,
* and returns the results as a Matrix with the ith row corresponding to
* the ith time.
* @throws std::exception if both cols and rows are not equal to 1.
*/
MatrixX<T> vector_values(const Eigen::Ref<const VectorX<T>>& t) const;
/**
* Returns true iff the Trajectory provides and implementation for
* EvalDerivative() and MakeDerivative(). The derivative need not be
* continuous, but should return a result for all t for which value(t) returns
* a result.
*/
bool has_derivative() const;
/**
* Evaluates the derivative of `this` at the given time @p t.
* Returns the nth derivative, where `n` is the value of @p derivative_order.
*
* @pre derivative_order must be non-negative.
*/
MatrixX<T> EvalDerivative(const T& t, int derivative_order = 1) const;
/**
* Takes the derivative of this Trajectory.
* @param derivative_order The number of times to take the derivative before
* returning.
* @return The nth derivative of this object.
*/
std::unique_ptr<Trajectory<T>> MakeDerivative(int derivative_order = 1) const;
/**
* @return The number of rows in the matrix returned by value().
*/
virtual Eigen::Index rows() const = 0;
/**
* @return The number of columns in the matrix returned by value().
*/
virtual Eigen::Index cols() const = 0;
virtual T start_time() const = 0;
virtual T end_time() const = 0;
protected:
// Final subclasses are allowed to make copy/move/assign public.
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Trajectory)
Trajectory() = default;
virtual bool do_has_derivative() const;
virtual MatrixX<T> DoEvalDerivative(const T& t, int derivative_order) const;
virtual std::unique_ptr<Trajectory<T>> DoMakeDerivative(
int derivative_order) const;
};
} // namespace trajectories
} // namespace drake
DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
class drake::trajectories::Trajectory)