-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
sine.cc
163 lines (140 loc) · 5.44 KB
/
sine.cc
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
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
#include "drake/systems/primitives/sine.h"
#include "drake/common/drake_throw.h"
#include "drake/common/fmt_eigen.h"
namespace drake {
namespace systems {
template <typename T>
Sine<T>::Sine(double amplitude, double frequency, double phase, int size,
bool is_time_based)
: Sine(Eigen::VectorXd::Ones(size) * amplitude,
Eigen::VectorXd::Ones(size) * frequency,
Eigen::VectorXd::Ones(size) * phase, is_time_based) {}
template <typename T>
Sine<T>::Sine(const Eigen::VectorXd& amplitudes,
const Eigen::VectorXd& frequencies, const Eigen::VectorXd& phases,
bool is_time_based)
: LeafSystem<T>(SystemTypeTag<Sine>{}),
amplitude_(amplitudes),
frequency_(frequencies),
phase_(phases),
is_time_based_(is_time_based) {
// Ensure the incoming vectors are all the same size
DRAKE_THROW_UNLESS(amplitudes.size() == frequencies.size());
DRAKE_THROW_UNLESS(amplitudes.size() == phases.size());
// Check each of the incoming vectors. For each vector, set a flag if every
// element in that vector is the same.
is_const_amplitude_ = amplitude_.isConstant(amplitude_[0]);
is_const_frequency_ = frequency_.isConstant(frequency_[0]);
is_const_phase_ = phase_.isConstant(phase_[0]);
// If the Sine system is system time based, do not create an input port.
// System time is used as the time variable in this case. If the Sine system
// is not system time based, create an input port that contains the signal to
// be used as the time variable.
if (!is_time_based) {
this->DeclareInputPort(kUseDefaultName, kVectorValued, amplitudes.size());
}
value_output_port_index_ =
this->DeclareVectorOutputPort(kUseDefaultName, amplitudes.size(),
&Sine::CalcValueOutput)
.get_index();
first_derivative_output_port_index_ =
this->DeclareVectorOutputPort(kUseDefaultName, amplitudes.size(),
&Sine::CalcFirstDerivativeOutput)
.get_index();
second_derivative_output_port_index_ =
this->DeclareVectorOutputPort(kUseDefaultName, amplitudes.size(),
&Sine::CalcSecondDerivativeOutput)
.get_index();
}
template <typename T>
template <typename U>
Sine<T>::Sine(const Sine<U>& other)
: Sine<T>(other.amplitude_vector(), other.frequency_vector(),
other.phase_vector(), other.is_time_based()) {}
template <typename T>
double Sine<T>::amplitude() const {
if (!is_const_amplitude_) {
throw std::logic_error(fmt::format(
"The amplitude vector, [{}], cannot be represented as a scalar value. "
"Please use drake::systems::Sine::amplitude_vector() instead.",
fmt_eigen(amplitude_)));
}
return amplitude_[0];
}
template <typename T>
double Sine<T>::frequency() const {
if (!is_const_frequency_) {
throw std::logic_error(fmt::format(
"The frequency vector, [{}], cannot be represented as a scalar value. "
"Please use drake::systems::Sine::frequency_vector() instead.",
fmt_eigen(frequency_)));
}
return frequency_[0];
}
template <typename T>
double Sine<T>::phase() const {
if (!is_const_phase_) {
throw std::logic_error(fmt::format(
"The phase vector, [{}], cannot be represented as a scalar value. "
"Please use drake::systems::Sine::phase_vector() instead.",
fmt_eigen(phase_)));
}
return phase_[0];
}
template <typename T>
bool Sine<T>::is_time_based() const {
return is_time_based_;
}
template <typename T>
const Eigen::VectorXd& Sine<T>::amplitude_vector() const {
return amplitude_;
}
template <typename T>
const Eigen::VectorXd& Sine<T>::frequency_vector() const {
return frequency_;
}
template <typename T>
const Eigen::VectorXd& Sine<T>::phase_vector() const {
return phase_;
}
template <typename T>
void Sine<T>::CalcValueOutput(const Context<T>& context,
BasicVector<T>* output) const {
VectorX<T> sine_arg;
Sine::CalcArg(context, &sine_arg);
Eigen::VectorBlock<VectorX<T>> output_block = output->get_mutable_value();
output_block = amplitude_.array() * sine_arg.array().sin();
}
template <typename T>
void Sine<T>::CalcFirstDerivativeOutput(const Context<T>& context,
BasicVector<T>* output) const {
VectorX<T> cos_arg;
Sine::CalcArg(context, &cos_arg);
Eigen::VectorBlock<VectorX<T>> output_block = output->get_mutable_value();
output_block =
amplitude_.array() * frequency_.array() * cos_arg.array().cos();
}
template <typename T>
void Sine<T>::CalcSecondDerivativeOutput(const Context<T>& context,
BasicVector<T>* output) const {
VectorX<T> sine_arg;
Sine::CalcArg(context, &sine_arg);
Eigen::VectorBlock<VectorX<T>> output_block = output->get_mutable_value();
output_block =
-amplitude_.array() * frequency_.array().pow(2) * sine_arg.array().sin();
}
template <typename T>
void Sine<T>::CalcArg(const Context<T>& context, VectorX<T>* arg) const {
if (is_time_based_) {
VectorX<T> time_vec(amplitude_.size());
time_vec.fill(context.get_time());
*arg = frequency_.array() * time_vec.array() + phase_.array();
} else {
const VectorX<T>& input = this->get_input_port(0).Eval(context);
*arg = frequency_.array() * input.array() + phase_.array();
}
}
} // namespace systems
} // namespace drake
DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
class ::drake::systems::Sine)