-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
gain.cc
51 lines (41 loc) · 1.47 KB
/
gain.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
#include "drake/systems/primitives/gain.h"
#include "drake/common/fmt_eigen.h"
#include "drake/common/unused.h"
namespace drake {
namespace systems {
// TODO(amcastro-tri): remove the size parameter from the constructor once
// #3109 supporting automatic sizes is resolved.
template <typename T>
Gain<T>::Gain(double k, int size) : Gain(Eigen::VectorXd::Ones(size) * k) {}
template <typename T>
Gain<T>::Gain(const Eigen::VectorXd& k)
: VectorSystem<T>(SystemTypeTag<Gain>{}, k.size(), k.size()), k_(k) {}
template <typename T>
template <typename U>
Gain<T>::Gain(const Gain<U>& other) : Gain<T>(other.get_gain_vector()) {}
template <typename T>
double Gain<T>::get_gain() const {
if (!k_.isConstant(k_[0])) {
throw std::runtime_error(fmt::format(
"The gain vector [{}] cannot be represented as a scalar value. "
"Please use drake::systems::Gain::get_gain_vector() instead.",
fmt_eigen(k_.transpose())));
}
return k_[0];
}
template <typename T>
const Eigen::VectorXd& Gain<T>::get_gain_vector() const {
return k_;
}
template <typename T>
void Gain<T>::DoCalcVectorOutput(
const Context<T>&, const Eigen::VectorBlock<const VectorX<T>>& input,
const Eigen::VectorBlock<const VectorX<T>>& state,
Eigen::VectorBlock<VectorX<T>>* output) const {
unused(state);
*output = k_.array() * input.array();
}
} // namespace systems
} // namespace drake
DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
class ::drake::systems::Gain)