-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
pid_controller.cc
146 lines (126 loc) · 5.53 KB
/
pid_controller.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
#include "drake/systems/controllers/pid_controller.h"
#include <string>
#include "drake/common/default_scalars.h"
namespace drake {
namespace systems {
namespace controllers {
template <typename T>
PidController<T>::PidController(const Eigen::VectorXd& kp,
const Eigen::VectorXd& ki,
const Eigen::VectorXd& kd)
: PidController(MatrixX<double>::Identity(2 * kp.size(), 2 * kp.size()), kp,
ki, kd) {}
template <typename T>
PidController<T>::PidController(const MatrixX<double>& state_projection,
const Eigen::VectorXd& kp,
const Eigen::VectorXd& ki,
const Eigen::VectorXd& kd)
: PidController(state_projection,
MatrixX<double>::Identity(kp.size(), kp.size()), kp, ki,
kd) {}
template <typename T>
PidController<T>::PidController(const MatrixX<double>& state_projection,
const MatrixX<double>& output_projection,
const Eigen::VectorXd& kp,
const Eigen::VectorXd& ki,
const Eigen::VectorXd& kd)
: LeafSystem<T>(SystemTypeTag<PidController>{}),
kp_(kp),
ki_(ki),
kd_(kd),
num_controlled_q_(kp.size()),
num_full_state_(state_projection.cols()),
state_projection_(state_projection),
output_projection_(output_projection) {
if (kp_.size() != kd_.size() || kd_.size() != ki_.size()) {
throw std::logic_error(
"Gains must have equal length: |Kp| = " + std::to_string(kp_.size()) +
", |Ki| = " + std::to_string(ki_.size()) +
", |Kd| = " + std::to_string(kd_.size()));
}
if (state_projection_.rows() != 2 * num_controlled_q_) {
throw std::logic_error(
"State projection row dimension mismatch, expecting " +
std::to_string(2 * num_controlled_q_) + ", is " +
std::to_string(state_projection_.rows()));
}
if (output_projection_.cols() != kp_.size()) {
throw std::logic_error(
"Output projection column dimension mismatch, expecting " +
std::to_string(kp_.size()) + ", is " +
std::to_string(output_projection_.cols()));
}
this->DeclareContinuousState(num_controlled_q_);
output_index_control_ =
this->DeclareVectorOutputPort("control",
BasicVector<T>(output_projection_.rows()),
&PidController<T>::CalcControl)
.get_index();
input_index_state_ =
this->DeclareInputPort("estimated_state", kVectorValued, num_full_state_)
.get_index();
input_index_desired_state_ =
this->DeclareInputPort("desired_state", kVectorValued,
2 * num_controlled_q_)
.get_index();
}
template <typename T>
template <typename U>
PidController<T>::PidController(const PidController<U>& other)
: PidController(other.state_projection_, other.output_projection_,
other.kp_, other.ki_, other.kd_) {}
template <typename T>
void PidController<T>::DoCalcTimeDerivatives(
const Context<T>& context, ContinuousState<T>* derivatives) const {
const Eigen::VectorBlock<const VectorX<T>> state =
get_input_port_estimated_state().Eval(context);
const Eigen::VectorBlock<const VectorX<T>> state_d =
get_input_port_desired_state().Eval(context);
// The derivative of the continuous state is the instantaneous position error.
VectorBase<T>& derivatives_vector = derivatives->get_mutable_vector();
const VectorX<T> controlled_state_diff =
state_d - (state_projection_.cast<T>() * state);
derivatives_vector.SetFromVector(
controlled_state_diff.head(num_controlled_q_));
}
template <typename T>
void PidController<T>::CalcControl(const Context<T>& context,
BasicVector<T>* control) const {
const Eigen::VectorBlock<const VectorX<T>> state =
get_input_port_estimated_state().Eval(context);
const Eigen::VectorBlock<const VectorX<T>> state_d =
get_input_port_desired_state().Eval(context);
// State error.
const VectorX<T> controlled_state_diff =
state_d - (state_projection_.cast<T>() * state);
// Intergral error, which is stored in the continuous state.
const VectorBase<T>& state_vector = context.get_continuous_state_vector();
const Eigen::VectorBlock<const VectorX<T>> state_block =
dynamic_cast<const BasicVector<T>&>(state_vector).get_value();
// Sets output to the sum of all three terms.
control->SetFromVector(
output_projection_.cast<T>() *
((kp_.array() * controlled_state_diff.head(num_controlled_q_).array())
.matrix() +
(kd_.array() * controlled_state_diff.tail(num_controlled_q_).array())
.matrix() +
(ki_.array() * state_block.array()).matrix()));
}
// Adds a simple record-based representation of the PID controller to @p dot.
template <typename T>
void PidController<T>::GetGraphvizFragment(int max_depth,
std::stringstream* dot) const {
unused(max_depth);
std::string name = this->get_name();
if (name.empty()) {
name = "PID Controller";
}
*dot << this->GetGraphvizId() << " [shape=record, label=\"" << name;
*dot << " | { {<u0> x |<u1> x_d} |<y0> y}";
*dot << "\"];" << std::endl;
}
} // namespace controllers
} // namespace systems
} // namespace drake
DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
class ::drake::systems::controllers::PidController)