ctrl-utils
control.h
1 
10 #ifndef CTRL_UTILS_CONTROL_H_
11 #define CTRL_UTILS_CONTROL_H_
12 
13 #include "ctrl_utils/eigen.h"
14 #include "ctrl_utils/euclidian.h"
15 
16 namespace ctrl_utils {
17 
42 template <typename Derived1, typename Derived2, typename Derived3,
43  typename Derived5>
44 inline typename Derived1::PlainObject PdControl(
45  const ::Eigen::MatrixBase<Derived1>& x,
46  const ::Eigen::MatrixBase<Derived2>& x_des,
47  const ::Eigen::MatrixBase<Derived3>& dx,
48  const ::Eigen::MatrixBase<Derived5>& kp_kv, double ddx_max = 0.,
49  typename Derived1::PlainObject* x_err_out = nullptr) {
50  static_assert(
51  Derived5::ColsAtCompileTime == 2 || (Derived5::ColsAtCompileTime == 1 &&
52  Derived5::RowsAtCompileTime == 2),
53  "kp_kv must be a vector of size 2 or a matrix of size x.rows() x 2.");
54 
55  // Output error.
56  typename Derived1::PlainObject x_err = x - x_des;
57  if (x_err_out != nullptr) {
58  *x_err_out = x_err;
59  }
60  typename Derived3::PlainObject dx_err = dx;
61 
62  // Apply gains.
63  if (Derived5::ColsAtCompileTime == 1) {
64  x_err *= -kp_kv(0);
65  dx_err *= -kp_kv(1);
66  } else {
67  x_err.array() *= -kp_kv.block(0, 0, x_err.size(), 1).array();
68  dx_err.array() *= -kp_kv.block(0, 1, dx_err.size(), 1).array();
69  }
70 
71  // Limit maximum error
72  if (ddx_max > 0.) {
73  const double x_err_sq_norm = x_err.squaredNorm();
74  const double ddx_max_sq = ddx_max * ddx_max;
75  if (x_err_sq_norm > ddx_max_sq) {
76  x_err *= ddx_max / std::sqrt(x_err_sq_norm);
77  }
78  }
79 
80  return x_err + dx_err;
81 }
82 
83 template <typename Derived1, typename Derived2, typename Derived3,
84  typename Derived4, typename Derived5>
85 inline typename Derived1::PlainObject PdControl(
86  const ::Eigen::MatrixBase<Derived1>& x,
87  const ::Eigen::MatrixBase<Derived2>& x_des,
88  const ::Eigen::MatrixBase<Derived3>& dx,
89  const ::Eigen::MatrixBase<Derived4>& dx_des,
90  const ::Eigen::MatrixBase<Derived5>& kp_kv, double ddx_max = 0.,
91  typename Derived1::PlainObject* x_err_out = nullptr,
92  typename Derived3::PlainObject* dx_err_out = nullptr) {
93  static_assert(
94  Derived5::ColsAtCompileTime == 2 || (Derived5::ColsAtCompileTime == 1 &&
95  Derived5::RowsAtCompileTime == 2),
96  "kp_kv must be a vector of size 2 or a matrix of size x.rows() x 2.");
97 
98  // Output error.
99  typename Derived1::PlainObject x_err = x - x_des;
100  if (x_err_out != nullptr) {
101  *x_err_out = x_err;
102  }
103  typename Derived3::PlainObject dx_err = dx - dx_des;
104  if (dx_err_out != nullptr) {
105  *dx_err_out = dx_err;
106  }
107 
108  // Apply gains.
109  if (Derived5::ColsAtCompileTime == 1) {
110  x_err = -kp_kv(0) * x_err;
111  dx_err = -kp_kv(1) * dx_err;
112  } else {
113  x_err = -kp_kv.block(0, 0, x_err.size(), 1).array() * x_err.array();
114  dx_err = -kp_kv.block(0, 1, dx_err.size(), 1).array() * dx_err.array();
115  }
116 
117  // Limit maximum error
118  if (ddx_max > 0.) {
119  const double x_err_sq_norm = x_err.squaredNorm();
120  const double ddx_max_sq = ddx_max * ddx_max;
121  if (x_err_sq_norm > ddx_max_sq) {
122  x_err *= ddx_max / std::sqrt(x_err_sq_norm);
123  }
124  }
125 
126  return x_err + dx_err;
127 
128  // With velocity clipping
129  // if (dx_max > 0.) {
130  // typename Derived1::PlainObject dx_des = -(kp_kv(0) / kp_kv(1)) * x_err;
131  // if (dx_des.norm() > 0.01) {
132  // typename Derived1::Scalar v = dx_max / dx_des.norm();
133  // if (v > 1.) v = 1.;
134  // return -kp_kv(1) * (dx - v * dx_des);
135  // }
136  // }
137 }
138 
164 template <typename Derived1, typename Derived3>
165 inline typename Derived1::PlainObject PdControl(
166  const ::Eigen::Quaterniond& quat, const ::Eigen::Quaterniond& quat_des,
167  const ::Eigen::MatrixBase<Derived1>& w,
168  const ::Eigen::MatrixBase<Derived3>& kp_kv, double dw_max = 0.,
169  typename Derived1::PlainObject* ori_err_out = nullptr) {
170  static_assert(
171  (Derived3::RowsAtCompileTime == 3 && Derived3::ColsAtCompileTime == 2) ||
172  (Derived3::RowsAtCompileTime == 2 &&
173  Derived3::ColsAtCompileTime == 1),
174  "kp_kv must be a vector of size 2 or a matrix of size x.rows() x 2.");
175 
176  // Output error.
177  typename Derived1::PlainObject ori_err = ctrl_utils::OrientationError(quat, quat_des);
178  if (ori_err_out != nullptr) {
179  *ori_err_out = ori_err;
180  }
181  typename Derived1::PlainObject w_err = w;
182 
183  // Apply kp .
184  ori_err *= -kp_kv(0);
185  w_err *= -kp_kv(1);
186 
187  // Limit maximum error
188  if (dw_max > 0.) {
189  const double ori_err_sq_norm = ori_err.squaredNorm();
190  const double dw_max_sq = dw_max * dw_max;
191  if (ori_err_sq_norm > dw_max_sq) {
192  ori_err *= dw_max / std::sqrt(ori_err_sq_norm);
193  }
194  }
195 
196  return ori_err + w_err;
197 }
198 
199 template <typename Derived1, typename Derived2, typename Derived3>
200 inline typename Derived1::PlainObject PdControl(
201  const ::Eigen::Quaterniond& quat, const ::Eigen::Quaterniond& quat_des,
202  const ::Eigen::MatrixBase<Derived1>& w,
203  const ::Eigen::MatrixBase<Derived2>& w_des,
204  const ::Eigen::MatrixBase<Derived3>& kp_kv, double dw_max = 0.,
205  typename Derived1::PlainObject* ori_err_out = nullptr,
206  typename Derived2::PlainObject* w_err_out = nullptr) {
207  static_assert(
208  (Derived3::RowsAtCompileTime == 3 && Derived3::ColsAtCompileTime == 2) ||
209  (Derived3::RowsAtCompileTime == 2 &&
210  Derived3::ColsAtCompileTime == 1),
211  "kp_kv must be a vector of size 2 or a matrix of size x.rows() x 2.");
212 
213  // Output error.
214  typename Derived1::PlainObject ori_err = ctrl_utils::OrientationError(quat, quat_des);
215  if (ori_err_out != nullptr) {
216  *ori_err_out = ori_err;
217  }
218  typename Derived1::PlainObject w_err = w - w_des;
219  if (w_err_out != nullptr) {
220  *w_err_out = w_err;
221  }
222 
223  // Apply kp .
224  ori_err = -kp_kv(0) * ori_err;
225  w_err = -kp_kv(1) * w_err;
226 
227  // Limit maximum error
228  if (dw_max > 0.) {
229  const double ori_err_sq_norm = ori_err.squaredNorm();
230  const double dw_max_sq = dw_max * dw_max;
231  if (ori_err_sq_norm > dw_max_sq) {
232  ori_err *= dw_max / std::sqrt(ori_err_sq_norm);
233  }
234  }
235 
236  return ori_err + w_err;
237 }
238 
239 template <typename Derived1, typename Derived2, typename Derived3,
240  typename Derived4>
241 inline typename Derived1::PlainObject LookatPdControl(
242  const ::Eigen::MatrixBase<Derived1>& vec,
243  const ::Eigen::MatrixBase<Derived2>& vec_des,
244  const ::Eigen::MatrixBase<Derived3>& w,
245  const ::Eigen::MatrixBase<Derived4>& kp_kv, double dw_max = 0.,
246  typename Derived1::PlainObject* ori_err_out = nullptr) {
247  static_assert(
248  (Derived4::RowsAtCompileTime == 3 && Derived4::ColsAtCompileTime == 2) ||
249  (Derived4::RowsAtCompileTime == 2 &&
250  Derived4::ColsAtCompileTime == 1),
251  "kp_kv must be a vector of size 2 or a matrix of size x.rows() x 2.");
252 
253  // Output orientation error
254  typename Derived1::PlainObject ori_err;
255  ori_err = ctrl_utils::LookatError(vec, vec_des);
256  if (ori_err_out != nullptr) {
257  *ori_err_out = ori_err;
258  }
259 
260  // Apply kp gains
261  ori_err = -kp_kv(0) * ori_err;
262 
263  // Limit maximum error
264  if (dw_max > 0. && ori_err.norm() > dw_max) {
265  ori_err = dw_max * ori_err.normalized();
266  }
267 
268  // Apply kv gains
269  return ori_err - kp_kv(1) * w;
270 }
271 
272 } // namespace ctrl_utils
273 
274 #endif // CTRL_UTILS_CONTROL_H_
ctrl_utils
Definition: ctrl_utils.cc:18
ctrl_utils::OrientationError
Eigen::Vector3d OrientationError(const Eigen::Quaterniond &quat, const Eigen::Quaterniond &quat_des)
Definition: euclidian.h:215
ctrl_utils::PdControl
Derived1::PlainObject PdControl(const ::Eigen::MatrixBase< Derived1 > &x, const ::Eigen::MatrixBase< Derived2 > &x_des, const ::Eigen::MatrixBase< Derived3 > &dx, const ::Eigen::MatrixBase< Derived5 > &kp_kv, double ddx_max=0., typename Derived1::PlainObject *x_err_out=nullptr)
Definition: control.h:44
ctrl_utils::LookatError
Eigen::Vector3d LookatError(const Eigen::Vector3d &vec, const Eigen::Vector3d &vec_des)
Definition: euclidian.h:224
Generated by doxygen 1.9.1