quadrature.h
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement
4  *
5  * Copyright (c) 2020,
6  * TU Dortmund - Institute of Control Theory and Systems Engineering.
7  * All rights reserved.
8  *
9  * This program is free software: you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation, either version 3 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU General Public License for more details.
18  *
19  * You should have received a copy of the GNU General Public License
20  * along with this program. If not, see <https://www.gnu.org/licenses/>.
21  *
22  * Authors: Christoph Rösmann
23  *********************************************************************/
24 
25 #ifndef SRC_NUMERICS_INCLUDE_CORBO_NUMERICS_QUADRATURE_H_
26 #define SRC_NUMERICS_INCLUDE_CORBO_NUMERICS_QUADRATURE_H_
27 
28 #include <corbo-numerics/quadrature_interface.h>
29 
30 #include <cmath>
31 #include <memory>
32 
33 namespace corbo {
34 
48 class QuadratureRectangleRule : public QuadratureCollocationInterface
49 {
50  public:
51  // Implements interface method
52  DynamicsEvalInterface::Ptr getInstance() const override { return std::make_shared<QuadratureRectangleRule>(); }
53 
54  // Implements interface method
55  bool isSupportingCompressedStatesMode() const override { return true; }
56  bool isIntermediateControlSubjectToOptim() const override { return false; }
57  int getNumIntermediatePoints() const override { return 0; }
58 
59  int getNumIntermediateControls() const override { return 0; }
60  int getNumIntermediateStates() const override { return 0; }
61 
62  // Implements interface method
63  void quadrature(const std::vector<const Eigen::VectorXd*>& x1_points, const std::vector<const Eigen::VectorXd*>& u1_points,
64  const Eigen::VectorXd& u2, const Eigen::VectorXd& x2, double dt, const SystemDynamicsInterface& system,
65  Eigen::Ref<Eigen::VectorXd> integral_value) override
66  {
67  assert(x1_points.size() == 1);
68  assert(u1_points.size() == 1);
69 
70  system.dynamics(0.5 * (*x1_points.front() + x2), 0.5 * (*u1_points.front() + u2), integral_value);
71  integral_value *= dt;
72  }
73 
74  void quadrature(const std::vector<const Eigen::VectorXd*>& x1_points, const std::vector<const Eigen::VectorXd*>& u1_points,
75  const Eigen::VectorXd& u2, const Eigen::VectorXd& x2, double dt,
76  const BinaryFunction& fun, Eigen::Ref<Eigen::VectorXd> integral_value) override
77  {
78  assert(x1_points.size() == 1);
79  assert(u1_points.size() == 1);
80 
81  fun(0.5 * (*x1_points.front() + x2), 0.5 * (*u1_points.front() + u2), integral_value);
82  integral_value *= dt;
83  }
84 
85  bool interpolate(const std::vector<const Eigen::VectorXd*>& x1_points, const std::vector<const Eigen::VectorXd*>& u1_points,
86  const Eigen::VectorXd& u2, const Eigen::VectorXd& x2, double t1, double dt, double dt_interp,
87  const SystemDynamicsInterface& system, bool skip_u2_x2, TimeSeries& ts_x, TimeSeries& ts_u) override
88  {
89  assert(x1_points.size() == 1);
90  assert(u1_points.size() == 1);
91 
92  Eigen::VectorXd xc = 0.5 * (*x1_points.front() + x2);
93  Eigen::VectorXd uc = 0.5 * (*u1_points.front() + u2);
94 
95  if (dt_interp <= 0 || dt_interp >= dt)
96  {
97  ts_x.add(t1, xc);
98  ts_u.add(t1, uc);
99  }
100  else
101  {
102  double t = t1;
103  int n = std::floor(dt / dt_interp);
104  for (int i = 0; i < n; ++i)
105  {
106  ts_x.add(t, xc);
107  ts_u.add(t, uc);
108  t += dt_interp;
109  }
110  }
111 
112  if (!skip_u2_x2)
113  {
114  ts_x.add(t1 + dt, xc);
115  ts_u.add(t1 + dt, uc);
116  }
117  return true;
118  }
119 
120  // Implements interface method
121  void quadrature(const StateVector& x1, const InputVector& u1, const StateVector& x2, double dt, const SystemDynamicsInterface& system,
122  Eigen::Ref<Eigen::VectorXd> integral_value) override
123  {
124  assert(integral_value.size() == x1.size());
125  assert(dt > 0 && "dt must be greater then zero!");
126 
127  system.dynamics(0.5 * (x1 + x2), u1, integral_value);
128  integral_value *= dt;
129  }
130 
131  // Implements interface method
132  void quadrature(const StateVector& x1, const InputVector& u1, const StateVector& x2, double dt, const SystemDynamicsInterface& system,
133  Eigen::Ref<Eigen::VectorXd> integral_value, std::vector<Eigen::VectorXd*>& intermediate_x) override
134  {
135  assert(integral_value.size() == x1.size());
136  assert(dt > 0 && "dt must be greater then zero!");
137 
138  system.dynamics(0.5 * (x1 + x2), u1, integral_value);
139  integral_value *= dt;
140  }
141 
142  // Implements interface method
143  void quadrature(const StateVector& x1, const InputVector& u1, const StateVector& x2, const InputVector& u2, double dt,
144  const SystemDynamicsInterface& system, Eigen::Ref<Eigen::VectorXd> integral_value) override
145  {
146  assert(integral_value.size() == x1.size());
147  assert(dt > 0 && "dt must be greater then zero!");
148 
149  system.dynamics(0.5 * (x1 + x2), 0.5 * (u1 + u2), integral_value);
150  integral_value *= dt;
151  }
152 
153  void quadrature(const StateVector& x1, const InputVector& u1, const StateVector& x2, const InputVector& u2, double dt,
154  const SystemDynamicsInterface& system, const std::vector<Eigen::VectorXd*>& intermediate_x,
155  const std::vector<Eigen::VectorXd*>& intermediate_u, Eigen::Ref<Eigen::VectorXd> integral_value,
156  Eigen::Ref<Eigen::VectorXd> interm_x_error) override
157  {
158  system.dynamics(0.5 * (x1 + x2), 0.5 * (u1 + u2), integral_value);
159  integral_value *= dt;
160  }
161 
162  void quadrature(const StateVector& x1, const InputVector& u1, const StateVector& x2, const InputVector& u2, double dt,
163  const SystemDynamicsInterface& system, const std::vector<Eigen::VectorXd*>& intermediate_u,
164  Eigen::Ref<Eigen::VectorXd> integral_value, std::vector<Eigen::VectorXd*>& intermediate_x) override
165  {
166  assert(integral_value.size() == x1.size());
167  assert(dt > 0 && "dt must be greater then zero!");
168 
169  system.dynamics(0.5 * (x1 + x2), 0.5 * (u1 + u2), integral_value);
170  integral_value *= dt;
171  }
172 
173  // Implements interface method
174  void quadrature(const StateVector& x1, const InputVector& u1, const StateVector& x2, const InputVector& u2, double dt,
175  const SystemDynamicsInterface& system, Eigen::Ref<Eigen::VectorXd> integral_value, std::vector<Eigen::VectorXd*>& intermediate_x,
176  std::vector<Eigen::VectorXd*>& intermediate_u) override
177  {
178  assert(integral_value.size() == x1.size());
179  assert(dt > 0 && "dt must be greater then zero!");
180 
181  system.dynamics(0.5 * (x1 + x2), 0.5 * (u1 + u2), integral_value);
182  integral_value *= dt;
183  }
184 
185  // Implements interface method
186  void quadrature(const Eigen::Ref<const Eigen::VectorXd>& x1, const Eigen::Ref<const Eigen::VectorXd>& x2, double dt, const UnaryFunction& fun,
187  Eigen::Ref<Eigen::VectorXd> integral_value) override
188  {
189  assert(dt > 0 && "dt must be greater then zero!");
190 
191  fun(0.5 * (x1 + x2), integral_value);
192  integral_value *= dt;
193  }
194 
195  // Implements interface method
196  void quadrature(const Eigen::Ref<const Eigen::VectorXd>& x1, const Eigen::Ref<const Eigen::VectorXd>& x2, double dt,
197  const std::vector<Eigen::VectorXd*>& intermediate_x, const UnaryFunction& fun,
198  Eigen::Ref<Eigen::VectorXd> integral_value) override
199  {
200  fun(0.5 * (x1 + x2), integral_value);
201  integral_value *= dt;
202  }
203 
204  // Implements interface method
205  void quadrature(const Eigen::Ref<const Eigen::VectorXd>& x1, const Eigen::Ref<const Eigen::VectorXd>& x2, double dt, const UnaryFunction& fun,
206  Eigen::Ref<Eigen::VectorXd> integral_value, std::vector<Eigen::VectorXd*>& intermediate_x) override
207  {
208  assert(intermediate_x.size() == 1);
209  assert(intermediate_x[0]->size() == x1.size());
210  intermediate_x[0]->noalias() = 0.5 * (x1 + x2);
211  fun(*intermediate_x[0], integral_value);
212  integral_value *= dt;
213  }
214 
215  // Implements interface method
216  void quadrature(const Eigen::Ref<const Eigen::VectorXd>& x1, const Eigen::Ref<const Eigen::VectorXd>& u1,
217  const Eigen::Ref<const Eigen::VectorXd>& x2, const Eigen::Ref<const Eigen::VectorXd>& u2, double dt, const BinaryFunction& fun,
218  Eigen::Ref<Eigen::VectorXd> integral_value) override
219  {
220  assert(dt > 0 && "dt must be greater then zero!");
221 
222  fun(0.5 * (x1 + x2), 0.5 * (u1 + u2), integral_value);
223  integral_value *= dt;
224  }
225 
226  // Implements interface method
227  void quadrature(const Eigen::Ref<const Eigen::VectorXd>& x1, const Eigen::Ref<const Eigen::VectorXd>& u1,
228  const Eigen::Ref<const Eigen::VectorXd>& x2, const Eigen::Ref<const Eigen::VectorXd>& u2, double dt,
229  const std::vector<Eigen::VectorXd*>& intermediate_x, const std::vector<Eigen::VectorXd*>& intermediate_u,
230  const BinaryFunction& fun, Eigen::Ref<Eigen::VectorXd> integral_value) override
231  {
232  fun(0.5 * (x1 + x2), 0.5 * (u1 + u2), integral_value);
233  integral_value *= dt;
234  }
235 
236  // Implements interface method
237  void quadrature(const Eigen::Ref<const Eigen::VectorXd>& x1, const Eigen::Ref<const Eigen::VectorXd>& u1,
238  const Eigen::Ref<const Eigen::VectorXd>& x2, const Eigen::Ref<const Eigen::VectorXd>& u2, double dt, const BinaryFunction& fun,
239  Eigen::Ref<Eigen::VectorXd> integral_value, std::vector<Eigen::VectorXd*>& intermediate_x,
240  std::vector<Eigen::VectorXd*>& intermediate_u) override
241  {
242  fun(0.5 * (x1 + x2), 0.5 * (u1 + u2), integral_value);
243  integral_value *= dt;
244  }
245 
246  bool interpolate(const Eigen::Ref<const Eigen::VectorXd>& x1, const Eigen::Ref<const Eigen::VectorXd>& u1,
247  const Eigen::Ref<const Eigen::VectorXd>& x2, const Eigen::Ref<const Eigen::VectorXd>& u2, double dt,
248  const SystemDynamicsInterface& /*system*/, const Range& range, std::vector<Eigen::VectorXd>& states,
249  std::vector<Eigen::VectorXd>& controls) override
250  {
251  if (range.getStart() < 0 || range.getEnd() > dt)
252  {
253  PRINT_ERROR_NAMED("range must be in the interval [0, dt]");
254  return false;
255  }
256 
257  Eigen::VectorXd xcenter = 0.5 * (x1 + x2);
258  Eigen::VectorXd ucenter = 0.5 * (u1 + u2);
259 
260  if (dt < 1e-15)
261  {
262  states.push_back(x1);
263  controls.push_back(ucenter);
264  return true;
265  }
266 
267  int n = range.getNumInRange();
268 
269  double t = range.getStart();
270  for (int i = 0; i < n; ++i, t += range.getStep())
271  {
272  double tau = (t - range.getStart()) / dt;
273  states.push_back(x1 + tau * (x2 - x1));
274  controls.push_back(ucenter);
275  }
276  if (range.includeEnd())
277  {
278  states.push_back(x2);
279  controls.push_back(ucenter);
280  }
281  return true;
282  }
283 
284  bool computeIntermediateControls(const Eigen::Ref<const Eigen::VectorXd>& u1, const Eigen::Ref<const Eigen::VectorXd>& u2,
285  std::vector<Eigen::VectorXd*>& intermediate_u) override
286  {
287  return true;
288  }
289 
290 #ifdef MESSAGE_SUPPORT
291  // Implements interface method
292  void toMessage(messages::Quadrature& message) const override
293  {
294  QuadratureCollocationInterface::toMessage(message);
295  message.mutable_rectangle_rule();
296  }
297 #endif
298 };
299 
300 FACTORY_REGISTER_QUADRATURE_COLLOCATION(QuadratureRectangleRule)
301 
302 
315 class QuadratureTrapezoidalRule : public QuadratureCollocationInterface
316 {
317  public:
318  // Implements interface method
319  DynamicsEvalInterface::Ptr getInstance() const override { return std::make_shared<QuadratureTrapezoidalRule>(); }
320 
321  // Implements interface method
322  bool isSupportingCompressedStatesMode() const override { return true; }
323  bool isIntermediateControlSubjectToOptim() const override { return false; }
324  int getNumIntermediatePoints() const override { return 0; }
325 
326  int getNumIntermediateControls() const override { return 0; }
327  int getNumIntermediateStates() const override { return 0; }
328 
329  // Implements interface method
330  void quadrature(const std::vector<const Eigen::VectorXd*>& x1_points, const std::vector<const Eigen::VectorXd*>& u1_points,
331  const Eigen::VectorXd& u2, const Eigen::VectorXd& x2, double dt, const SystemDynamicsInterface& system,
332  Eigen::Ref<Eigen::VectorXd> integral_value) override
333  {
334  assert(x1_points.size() == 1);
335  assert(u1_points.size() == 1);
336 
337  Eigen::VectorXd f1(x2.size());
338  system.dynamics(*x1_points.front(), *u1_points.front(), f1);
339  system.dynamics(x2, u2, integral_value);
340  integral_value = dt * 0.5 * (f1 + integral_value);
341  }
342 
343  void quadrature(const std::vector<const Eigen::VectorXd*>& x1_points, const std::vector<const Eigen::VectorXd*>& u1_points,
344  const Eigen::VectorXd& u2, const Eigen::VectorXd& x2, double dt,
345  const BinaryFunction& fun, Eigen::Ref<Eigen::VectorXd> integral_value) override
346  {
347  assert(x1_points.size() == 1);
348  assert(u1_points.size() == 1);
349 
350  Eigen::VectorXd f1(x2.size());
351  fun(*x1_points.front(), *u1_points.front(), f1);
352  fun(x2, u2, integral_value);
353  integral_value = dt * 0.5 * (f1 + integral_value);
354  }
355 
356  bool interpolate(const std::vector<const Eigen::VectorXd*>& x1_points, const std::vector<const Eigen::VectorXd*>& u1_points,
357  const Eigen::VectorXd& u2, const Eigen::VectorXd& x2, double t1, double dt, double dt_interp,
358  const SystemDynamicsInterface& system, bool skip_u2_x2, TimeSeries& ts_x, TimeSeries& ts_u) override
359  {
360  assert(x1_points.size() == 1);
361  assert(u1_points.size() == 1);
362 
363  if (dt_interp <= 0 || dt_interp >= dt)
364  {
365  ts_x.add(t1, *x1_points[0]);
366  ts_u.add(t1, *u1_points[0]);
367  }
368  else if (dt > 0)
369  {
370  Eigen::VectorXd xvel = (x2 - *x1_points[0]) / dt;
371  Eigen::VectorXd uvel = (u2 - *u1_points[0]) / dt;
372  int n = std::floor(dt / dt_interp);
373  int t = t1;
374  for (int i = 0; i < n; ++i)
375  {
376  ts_x.add(t, *x1_points[0] + (double)i * dt_interp * xvel);
377  ts_u.add(t, *u1_points[0] + (double)i * dt_interp * uvel);
378  t += dt_interp;
379  }
380  }
381 
382  if (!skip_u2_x2)
383  {
384  ts_x.add(t1 + dt, x2);
385  ts_u.add(t1 + dt, u2);
386  }
387  return true;
388  }
389 
390  // Implements interface method
391  void quadrature(const StateVector& x1, const InputVector& u1, const StateVector& x2, double dt, const SystemDynamicsInterface& system,
392  Eigen::Ref<Eigen::VectorXd> integral_value) override
393  {
394  assert(integral_value.size() == x1.size());
395  assert(dt > 0 && "dt must be greater then zero!");
396 
397  Eigen::VectorXd f1(x1.size());
398  system.dynamics(x1, u1, f1);
399  system.dynamics(x2, u1, integral_value);
400  integral_value = dt * 0.5 * (f1 + integral_value);
401  }
402 
403  // Implements interface method
404  void quadrature(const StateVector& x1, const InputVector& u1, const StateVector& x2, double dt, const SystemDynamicsInterface& system,
405  Eigen::Ref<Eigen::VectorXd> integral_value, std::vector<Eigen::VectorXd*>& /*intermediate_x*/) override
406  {
407  assert(integral_value.size() == x1.size());
408  assert(dt > 0 && "dt must be greater then zero!");
409 
410  Eigen::VectorXd f1(x1.size());
411  system.dynamics(x1, u1, f1);
412  system.dynamics(x2, u1, integral_value);
413  integral_value = dt * 0.5 * (f1 + integral_value);
414  }
415 
416  // Implements interface method
417  void quadrature(const StateVector& x1, const InputVector& u1, const StateVector& x2, const InputVector& u2, double dt,
418  const SystemDynamicsInterface& system, Eigen::Ref<Eigen::VectorXd> integral_value) override
419  {
420  assert(integral_value.size() == x1.size());
421  assert(dt > 0 && "dt must be greater then zero!");
422 
423  Eigen::VectorXd f1(x1.size());
424  system.dynamics(x1, u1, f1);
425  system.dynamics(x2, u2, integral_value);
426  integral_value = dt * 0.5 * (f1 + integral_value);
427  }
428 
429  void quadrature(const StateVector& x1, const InputVector& u1, const StateVector& x2, const InputVector& u2, double dt,
430  const SystemDynamicsInterface& system, const std::vector<Eigen::VectorXd*>& /*intermediate_x*/,
431  const std::vector<Eigen::VectorXd*>& /*intermediate_u*/, Eigen::Ref<Eigen::VectorXd> integral_value,
432  Eigen::Ref<Eigen::VectorXd> /*interm_x_error*/) override
433  {
434  assert(dt > 0 && "dt must be greater then zero!");
435 
436  Eigen::VectorXd f1(integral_value.size());
437  system.dynamics(x1, u1, f1);
438  system.dynamics(x2, u2, integral_value);
439  integral_value = dt * 0.5 * (f1 + integral_value);
440  }
441 
442  void quadrature(const StateVector& x1, const InputVector& u1, const StateVector& x2, const InputVector& u2, double dt,
443  const SystemDynamicsInterface& system, const std::vector<Eigen::VectorXd*>& /*intermediate_u*/,
444  Eigen::Ref<Eigen::VectorXd> integral_value, std::vector<Eigen::VectorXd*>& /*intermediate_x*/) override
445  {
446  assert(integral_value.size() == x1.size());
447  assert(dt > 0 && "dt must be greater then zero!");
448 
449  Eigen::VectorXd f1(x1.size());
450  system.dynamics(x1, u1, f1);
451  system.dynamics(x2, u2, integral_value);
452  integral_value = dt * 0.5 * (f1 + integral_value);
453  }
454 
455  // Implements interface method
456  void quadrature(const StateVector& x1, const InputVector& u1, const StateVector& x2, const InputVector& u2, double dt,
457  const SystemDynamicsInterface& system, Eigen::Ref<Eigen::VectorXd> integral_value,
458  std::vector<Eigen::VectorXd*>& /*intermediate_x*/, std::vector<Eigen::VectorXd*>& /*intermediate_u*/) override
459  {
460  assert(integral_value.size() == x1.size());
461  assert(dt > 0 && "dt must be greater then zero!");
462 
463  Eigen::VectorXd f1(x1.size());
464  system.dynamics(x1, u1, f1);
465  system.dynamics(x2, u2, integral_value);
466  integral_value = dt * 0.5 * (f1 + integral_value);
467  }
468  // Implements interface method
469  void quadrature(const Eigen::Ref<const Eigen::VectorXd>& x1, const Eigen::Ref<const Eigen::VectorXd>& x2, double dt, const UnaryFunction& fun,
470  Eigen::Ref<Eigen::VectorXd> integral_value) override
471  {
472  assert(dt > 0 && "dt must be greater then zero!");
473 
474  Eigen::VectorXd f1(integral_value.size());
475  fun(x1, f1);
476  fun(x2, integral_value);
477  integral_value = dt * 0.5 * (f1 + integral_value);
478  }
479 
480  // Implements interface method
481  void quadrature(const Eigen::Ref<const Eigen::VectorXd>& x1, const Eigen::Ref<const Eigen::VectorXd>& x2, double dt,
482  const std::vector<Eigen::VectorXd*>& /*intermediate_x*/, const UnaryFunction& fun,
483  Eigen::Ref<Eigen::VectorXd> integral_value) override
484  {
485  assert(dt > 0 && "dt must be greater then zero!");
486 
487  Eigen::VectorXd f1(integral_value.size());
488  fun(x1, f1);
489  fun(x2, integral_value);
490  integral_value = dt * 0.5 * (f1 + integral_value);
491  }
492 
493  // Implements interface method
494  void quadrature(const Eigen::Ref<const Eigen::VectorXd>& x1, const Eigen::Ref<const Eigen::VectorXd>& x2, double dt, const UnaryFunction& fun,
495  Eigen::Ref<Eigen::VectorXd> integral_value, std::vector<Eigen::VectorXd*>& /*intermediate_x*/) override
496  {
497  assert(dt > 0 && "dt must be greater then zero!");
498 
499  Eigen::VectorXd f1(integral_value.size());
500  fun(x1, f1);
501  fun(x2, integral_value);
502  integral_value = dt * 0.5 * (f1 + integral_value);
503  }
504 
505  // Implements interface method
506  void quadrature(const Eigen::Ref<const Eigen::VectorXd>& x1, const Eigen::Ref<const Eigen::VectorXd>& u1,
507  const Eigen::Ref<const Eigen::VectorXd>& x2, const Eigen::Ref<const Eigen::VectorXd>& u2, double dt, const BinaryFunction& fun,
508  Eigen::Ref<Eigen::VectorXd> integral_value) override
509  {
510  assert(dt > 0 && "dt must be greater then zero!");
511 
512  Eigen::VectorXd f1(integral_value.size());
513  fun(x1, u1, f1);
514  fun(x2, u2, integral_value);
515  integral_value = dt * 0.5 * (f1 + integral_value);
516  }
517 
518  // Implements interface method
519  void quadrature(const Eigen::Ref<const Eigen::VectorXd>& x1, const Eigen::Ref<const Eigen::VectorXd>& u1,
520  const Eigen::Ref<const Eigen::VectorXd>& x2, const Eigen::Ref<const Eigen::VectorXd>& u2, double dt,
521  const std::vector<Eigen::VectorXd*>& /*intermediate_x*/, const std::vector<Eigen::VectorXd*>& /*intermediate_u*/,
522  const BinaryFunction& fun, Eigen::Ref<Eigen::VectorXd> integral_value) override
523  {
524  assert(dt > 0 && "dt must be greater then zero!");
525 
526  Eigen::VectorXd f1(integral_value.size());
527  fun(x1, u1, f1);
528  fun(x2, u2, integral_value);
529  integral_value = dt * 0.5 * (f1 + integral_value);
530  }
531 
532  // Implements interface method
533  void quadrature(const Eigen::Ref<const Eigen::VectorXd>& x1, const Eigen::Ref<const Eigen::VectorXd>& u1,
534  const Eigen::Ref<const Eigen::VectorXd>& x2, const Eigen::Ref<const Eigen::VectorXd>& u2, double dt, const BinaryFunction& fun,
535  Eigen::Ref<Eigen::VectorXd> integral_value, std::vector<Eigen::VectorXd*>& /*intermediate_x*/,
536  std::vector<Eigen::VectorXd*>& /*intermediate_u*/) override
537  {
538  assert(dt > 0 && "dt must be greater then zero!");
539 
540  Eigen::VectorXd f1(integral_value.size());
541  fun(x1, u1, f1);
542  fun(x2, u2, integral_value);
543  integral_value = dt * 0.5 * (f1 + integral_value);
544  }
545 
546  bool interpolate(const Eigen::Ref<const Eigen::VectorXd>& x1, const Eigen::Ref<const Eigen::VectorXd>& u1,
547  const Eigen::Ref<const Eigen::VectorXd>& x2, const Eigen::Ref<const Eigen::VectorXd>& u2, double dt,
548  const SystemDynamicsInterface& system, const Range& range, std::vector<Eigen::VectorXd>& states,
549  std::vector<Eigen::VectorXd>& controls) override
550  {
551  if (range.getStart() < 0 || range.getEnd() > dt)
552  {
553  PRINT_ERROR_NAMED("range must be in the interval [0, dt]");
554  return false;
555  }
556 
557  states.push_back(x1);
558  controls.push_back(u1);
559 
560  if (dt < 1e-15) return true;
561 
562  Eigen::VectorXd f1(system.getStateDimension());
563  Eigen::VectorXd f2(system.getStateDimension());
564  system.dynamics(x1, u1, f1);
565  system.dynamics(x2, u2, f2);
566 
567  int n = range.getNumInRange();
568  double t = range.getStart() + range.getStep(); // we already added x1 and u1
569  for (int i = 1; i < n; ++i, t += range.getStep())
570  {
571  double tau = t - range.getStart();
572  states.push_back(x1 + tau * f1 + tau * tau / (2.0 * dt) * (f2 - f1)); // quadratic state trajectoy
573  controls.push_back(u1 + tau / dt * (u2 - u1)); // linear controls
574  }
575  if (range.includeEnd())
576  {
577  states.push_back(x2);
578  controls.push_back(u2);
579  }
580  return true;
581  }
582 
583  bool computeIntermediateControls(const Eigen::Ref<const Eigen::VectorXd>& u1, const Eigen::Ref<const Eigen::VectorXd>& u2,
584  std::vector<Eigen::VectorXd*>& intermediate_u) override
585  {
586  return true;
587  }
588 
589 #ifdef MESSAGE_SUPPORT
590  // Implements interface method
591  void toMessage(messages::Quadrature& message) const override
592  {
593  QuadratureCollocationInterface::toMessage(message);
594  message.mutable_trapezoidal_rule();
595  }
596 #endif
597 };
598 
599 FACTORY_REGISTER_QUADRATURE_COLLOCATION(QuadratureTrapezoidalRule)
600 
601 
622 class QuadratureHermiteSimpsonLinearControl : public QuadratureCollocationInterface
623 {
624  public:
625  // Implements interface method
626  DynamicsEvalInterface::Ptr getInstance() const override { return std::make_shared<QuadratureHermiteSimpsonLinearControl>(); }
627 
628  // Implements interface method
629  bool isSupportingCompressedStatesMode() const override { return true; }
630  bool isIntermediateControlSubjectToOptim() const override { return false; }
631  int getNumIntermediatePoints() const override { return 1; }
632 
633  int getNumIntermediateControls() const override { return 0; }
634  int getNumIntermediateStates() const override { return 0; }
635 
636  // Implements interface method
637  void quadrature(const std::vector<const Eigen::VectorXd*>& x1_points, const std::vector<const Eigen::VectorXd*>& u1_points,
638  const Eigen::VectorXd& u2, const Eigen::VectorXd& x2, double dt, const SystemDynamicsInterface& system,
639  Eigen::Ref<Eigen::VectorXd> integral_value) override
640  {
641  assert(x1_points.size() == 1);
642  assert(u1_points.size() == 1);
643 
644  Eigen::VectorXd f1(x2.size());
645  system.dynamics(*x1_points.front(), *u1_points.front(), f1);
646 
647  Eigen::VectorXd f2(x2.size());
648  system.dynamics(x2, u2, f2);
649 
650  Eigen::VectorXd x_center = 0.5 * (*x1_points.front() + x2) + dt / 8.0 * (f1 - f2);
651 
652  system.dynamics(x_center, 0.5 * (*u1_points.front() + u2), integral_value);
653 
654  integral_value = dt / 6.0 * (f1 + 4.0 * integral_value + f2);
655  }
656 
657  void quadrature(const std::vector<const Eigen::VectorXd*>& x1_points, const std::vector<const Eigen::VectorXd*>& u1_points,
658  const Eigen::VectorXd& u2, const Eigen::VectorXd& x2, double dt,
659  const BinaryFunction& fun, Eigen::Ref<Eigen::VectorXd> integral_value) override
660  {
661  assert(x1_points.size() == 1);
662  assert(u1_points.size() == 1);
663 
664  Eigen::VectorXd f1(x2.size());
665  fun(*x1_points.front(), *u1_points.front(), f1);
666 
667  Eigen::VectorXd f2(x2.size());
668  fun(x2, u2, f2);
669 
670  Eigen::VectorXd x_center = 0.5 * (*x1_points.front() + x2) + dt / 8.0 * (f1 - f2);
671 
672  fun(x_center, 0.5 * (*u1_points.front() + u2), integral_value);
673 
674  integral_value = dt / 6.0 * (f1 + 4.0 * integral_value + f2);
675  }
676 
677  bool interpolate(const std::vector<const Eigen::VectorXd*>& x1_points, const std::vector<const Eigen::VectorXd*>& u1_points,
678  const Eigen::VectorXd& u2, const Eigen::VectorXd& x2, double t1, double dt, double dt_interp,
679  const SystemDynamicsInterface& system, bool skip_u2_x2, TimeSeries& ts_x, TimeSeries& ts_u) override
680  {
681  assert(x1_points.size() == 1);
682  assert(u1_points.size() == 1);
683 
684  if (dt_interp <= 0 || dt_interp >= dt)
685  {
686  ts_x.add(t1, *x1_points[0]);
687  ts_u.add(t1, *u1_points[0]);
688  }
689  else if (dt > 0)
690  {
691  Eigen::VectorXd f1(x2.size());
692  system.dynamics(*x1_points.front(), *u1_points.front(), f1);
693 
694  Eigen::VectorXd f2(x2.size());
695  system.dynamics(x2, u2, f2);
696 
697  Eigen::VectorXd fc(x2.size());
698  Eigen::VectorXd xc = 0.5 * (*x1_points.front() + x2) + dt / 8.0 * (f1 - f2);
699  system.dynamics(xc, 0.5 * (*u1_points.front() + u2), fc);
700 
701  Eigen::VectorXd uvel = (u2 - *u1_points[0]) / dt;
702  int n = std::floor(dt / dt_interp);
703  double t = t1;
704  for (int i = 0; i < n; ++i)
705  {
706  double tau = (t - t1);
707 
708  // cubic interpolation for states (since quadratic interplation for xdot)
709  Eigen::VectorXd x_interp = *x1_points.front() + f1 * tau + 0.5 / dt * tau * tau * (-3.0 * f1 + 4.0 * fc - f2) +
710  1.0 / 3.0 / (dt * dt) * std::pow(tau, 3) * (2.0 * f1 - 4.0 * fc + 2.0 * f2);
711 
712  ts_x.add(t1 + tau, x_interp);
713  ts_u.add(t1 + tau, *u1_points[0] + (double)i * dt_interp * uvel);
714 
715  t += dt_interp;
716  }
717  }
718 
719  if (!skip_u2_x2)
720  {
721  ts_x.add(t1 + dt, x2);
722  ts_u.add(t1 + dt, u2);
723  }
724  return true;
725  }
726 
727  void quadrature(const StateVector& x1, const InputVector& u1, const StateVector& x2, double dt, const SystemDynamicsInterface& system,
728  Eigen::Ref<Eigen::VectorXd> integral_value) override
729  {
730  assert(integral_value.size() == x1.size());
731  assert(dt > 0 && "dt must be greater then zero!");
732 
733  Eigen::VectorXd f1(x1.size());
734  system.dynamics(x1, u1, f1);
735 
736  Eigen::VectorXd f2(x1.size());
737  system.dynamics(x2, u1, f2);
738 
739  Eigen::VectorXd x_center = 0.5 * (x1 + x2) + dt / 8.0 * (f1 - f2);
740 
741  system.dynamics(x_center, u1, integral_value);
742 
743  integral_value = dt / 6.0 * (f1 + 4.0 * integral_value + f2);
744  }
745 
746  void quadrature(const StateVector& x1, const InputVector& u1, const StateVector& x2, double dt, const SystemDynamicsInterface& system,
747  Eigen::Ref<Eigen::VectorXd> integral_value, std::vector<Eigen::VectorXd*>& intermediate_x) override
748  {
749  assert(integral_value.size() == x1.size());
750  assert(dt > 0 && "dt must be greater then zero!");
751  assert(intermediate_x.size() == 1);
752  assert(intermediate_x[0]->size() == x1.size());
753 
754  Eigen::VectorXd f1(x1.size());
755  system.dynamics(x1, u1, f1);
756 
757  Eigen::VectorXd f2(x1.size());
758  system.dynamics(x2, u1, f2);
759 
760  intermediate_x[0]->noalias() = 0.5 * (x1 + x2) + dt / 8.0 * (f1 - f2);
761 
762  system.dynamics(*intermediate_x[0], u1, integral_value);
763 
764  integral_value = dt / 6.0 * (f1 + 4.0 * integral_value + f2);
765  }
766 
767  // Implements interface method
768  void quadrature(const StateVector& x1, const InputVector& u1, const StateVector& x2, const InputVector& u2, double dt,
769  const SystemDynamicsInterface& system, Eigen::Ref<Eigen::VectorXd> integral_value) override
770  {
771  assert(integral_value.size() == x1.size());
772  assert(dt > 0 && "dt must be greater then zero!");
773 
774  Eigen::VectorXd f1(x1.size());
775  system.dynamics(x1, u1, f1);
776 
777  Eigen::VectorXd f2(x1.size());
778  system.dynamics(x2, u2, f2);
779 
780  Eigen::VectorXd x_center = 0.5 * (x1 + x2) + dt / 8.0 * (f1 - f2);
781 
782  system.dynamics(x_center, 0.5 * (u1 + u2), integral_value);
783 
784  integral_value = dt / 6.0 * (f1 + 4.0 * integral_value + f2);
785  }
786 
787  void quadrature(const StateVector& x1, const InputVector& u1, const StateVector& x2, const InputVector& u2, double dt,
788  const SystemDynamicsInterface& system, const std::vector<Eigen::VectorXd*>& intermediate_x,
789  const std::vector<Eigen::VectorXd*>& intermediate_u, Eigen::Ref<Eigen::VectorXd> integral_value,
790  Eigen::Ref<Eigen::VectorXd> interm_x_error) override
791  {
792  assert(intermediate_x.size() == 1);
793  assert(intermediate_x[0]->size() == x1.size());
794  assert(intermediate_u.size() == 1);
795  assert(intermediate_u[0]->size() == u1.size());
796  assert(dt > 0 && "dt must be greater then zero!");
797 
798  Eigen::VectorXd f1(integral_value.size());
799  system.dynamics(x1, u1, f1);
800 
801  Eigen::VectorXd f2(integral_value.size());
802  system.dynamics(x2, u2, f2);
803 
804  system.dynamics(*intermediate_x[0], *intermediate_u[0], integral_value);
805 
806  integral_value = dt / 6.0 * (f1 + 4.0 * integral_value + f2);
807 
808  assert(interm_x_error.size() == x1.size());
809  interm_x_error = *intermediate_x[0] - (0.5 * (x1 + x2) + dt / 8.0 * (f1 - f2));
810  }
811 
812  void quadrature(const StateVector& x1, const InputVector& u1, const StateVector& x2, const InputVector& u2, double dt,
813  const SystemDynamicsInterface& system, const std::vector<Eigen::VectorXd*>& intermediate_u,
814  Eigen::Ref<Eigen::VectorXd> integral_value, std::vector<Eigen::VectorXd*>& intermediate_x) override
815  {
816  assert(integral_value.size() == x1.size());
817  assert(dt > 0 && "dt must be greater then zero!");
818  assert(intermediate_x.size() == 1);
819  assert(intermediate_x[0]->size() == x1.size());
820  assert(intermediate_u.size() == 1);
821  assert(intermediate_u[0]->size() == u1.size());
822 
823  Eigen::VectorXd f1(x1.size());
824  system.dynamics(x1, u1, f1);
825 
826  Eigen::VectorXd f2(x1.size());
827  system.dynamics(x2, u2, f2);
828 
829  intermediate_x[0]->noalias() = 0.5 * (x1 + x2) + dt / 8.0 * (f1 - f2);
830 
831  system.dynamics(*intermediate_x[0], *intermediate_u[0], integral_value);
832 
833  integral_value = dt / 6.0 * (f1 + 4.0 * integral_value + f2);
834  }
835 
836  void quadrature(const StateVector& x1, const InputVector& u1, const StateVector& x2, const InputVector& u2, double dt,
837  const SystemDynamicsInterface& system, Eigen::Ref<Eigen::VectorXd> integral_value, std::vector<Eigen::VectorXd*>& intermediate_x,
838  std::vector<Eigen::VectorXd*>& intermediate_u) override
839  {
840  assert(integral_value.size() == x1.size());
841  assert(dt > 0 && "dt must be greater then zero!");
842  assert(intermediate_x.size() == 1);
843  assert(intermediate_x[0]->size() == x1.size());
844  assert(intermediate_u.size() == 1);
845  assert(intermediate_u[0]->size() == u1.size());
846 
847  Eigen::VectorXd f1(x1.size());
848  system.dynamics(x1, u1, f1);
849 
850  Eigen::VectorXd f2(x1.size());
851  system.dynamics(x2, u2, f2);
852 
853  intermediate_x[0]->noalias() = 0.5 * (x1 + x2) + dt / 8.0 * (f1 - f2);
854  intermediate_u[0]->noalias() = 0.5 * (u1 + u2);
855 
856  system.dynamics(*intermediate_x[0], *intermediate_u[0], integral_value);
857 
858  integral_value = dt / 6.0 * (f1 + 4.0 * integral_value + f2);
859  }
860 
861  // Implements interface method
862  void quadrature(const Eigen::Ref<const Eigen::VectorXd>& x1, const Eigen::Ref<const Eigen::VectorXd>& x2, double dt, const UnaryFunction& fun,
863  Eigen::Ref<Eigen::VectorXd> integral_value) override
864  {
865  assert(dt > 0 && "dt must be greater then zero!");
866 
867  Eigen::VectorXd f1(integral_value.size());
868  fun(x1, f1);
869 
870  Eigen::VectorXd f2(integral_value.size());
871  fun(x2, f2);
872 
873  Eigen::VectorXd x_center = 0.5 * (x1 + x2) + dt / 8.0 * (f1 - f2);
874 
875  fun(x_center, integral_value);
876 
877  integral_value = dt / 6.0 * (f1 + 4.0 * integral_value + f2);
878  }
879 
880  // Implements interface method
881  void quadrature(const Eigen::Ref<const Eigen::VectorXd>& x1, const Eigen::Ref<const Eigen::VectorXd>& x2, double dt,
882  const std::vector<Eigen::VectorXd*>& intermediate_x, const UnaryFunction& fun,
883  Eigen::Ref<Eigen::VectorXd> integral_value) override
884  {
885  assert(intermediate_x.size() == 1);
886  assert(intermediate_x[0]->size() == x1.size());
887  assert(dt > 0 && "dt must be greater then zero!");
888 
889  Eigen::VectorXd f1(integral_value.size());
890  fun(x1, f1);
891 
892  Eigen::VectorXd f2(integral_value.size());
893  fun(x2, f2);
894 
895  fun(*intermediate_x[0], integral_value);
896 
897  integral_value = dt / 6.0 * (f1 + 4.0 * integral_value + f2);
898  }
899 
900  // Implements interface method
901  void quadrature(const Eigen::Ref<const Eigen::VectorXd>& x1, const Eigen::Ref<const Eigen::VectorXd>& x2, double dt, const UnaryFunction& fun,
902  Eigen::Ref<Eigen::VectorXd> integral_value, std::vector<Eigen::VectorXd*>& intermediate_x) override
903  {
904  assert(intermediate_x.size() == 1);
905  assert(intermediate_x[0]->size() == x1.size());
906  assert(dt > 0 && "dt must be greater then zero!");
907 
908  Eigen::VectorXd f1(integral_value.size());
909  fun(x1, f1);
910 
911  Eigen::VectorXd f2(integral_value.size());
912  fun(x2, f2);
913 
914  *intermediate_x[0] = 0.5 * (x1 + x2) + dt / 8.0 * (f1 - f2);
915 
916  fun(*intermediate_x[0], integral_value);
917 
918  integral_value = dt / 6.0 * (f1 + 4.0 * integral_value + f2);
919  }
920 
921  // Implements interface method
922  void quadrature(const Eigen::Ref<const Eigen::VectorXd>& x1, const Eigen::Ref<const Eigen::VectorXd>& u1,
923  const Eigen::Ref<const Eigen::VectorXd>& x2, const Eigen::Ref<const Eigen::VectorXd>& u2, double dt, const BinaryFunction& fun,
924  Eigen::Ref<Eigen::VectorXd> integral_value) override
925  {
926  assert(dt > 0 && "dt must be greater then zero!");
927 
928  Eigen::VectorXd f1(integral_value.size());
929  fun(x1, u1, f1);
930 
931  Eigen::VectorXd f2(integral_value.size());
932  fun(x2, u2, f2);
933 
934  Eigen::VectorXd x_center = 0.5 * (x1 + x2) + dt / 8.0 * (f1 - f2);
935 
936  fun(x_center, 0.5 * (u1 + u2), integral_value);
937 
938  integral_value = dt / 6.0 * (f1 + 4.0 * integral_value + f2);
939  }
940 
941  // Implements interface method
942  void quadrature(const Eigen::Ref<const Eigen::VectorXd>& x1, const Eigen::Ref<const Eigen::VectorXd>& u1,
943  const Eigen::Ref<const Eigen::VectorXd>& x2, const Eigen::Ref<const Eigen::VectorXd>& u2, double dt,
944  const std::vector<Eigen::VectorXd*>& intermediate_x, const std::vector<Eigen::VectorXd*>& intermediate_u,
945  const BinaryFunction& fun, Eigen::Ref<Eigen::VectorXd> integral_value) override
946  {
947  assert(intermediate_x.size() == 1);
948  assert(intermediate_x[0]->size() == x1.size());
949  assert(intermediate_u.size() == 1);
950  assert(intermediate_u[0]->size() == u1.size());
951  assert(dt > 0 && "dt must be greater then zero!");
952 
953  Eigen::VectorXd f1(integral_value.size());
954  fun(x1, u1, f1);
955 
956  Eigen::VectorXd f2(integral_value.size());
957  fun(x2, u2, f2);
958 
959  fun(*intermediate_x[0], *intermediate_u[0], integral_value);
960 
961  integral_value = dt / 6.0 * (f1 + 4.0 * integral_value + f2);
962  }
963 
964  // Implements interface method
965  void quadrature(const Eigen::Ref<const Eigen::VectorXd>& x1, const Eigen::Ref<const Eigen::VectorXd>& u1,
966  const Eigen::Ref<const Eigen::VectorXd>& x2, const Eigen::Ref<const Eigen::VectorXd>& u2, double dt, const BinaryFunction& fun,
967  Eigen::Ref<Eigen::VectorXd> integral_value, std::vector<Eigen::VectorXd*>& intermediate_x,
968  std::vector<Eigen::VectorXd*>& intermediate_u) override
969  {
970  assert(intermediate_x.size() == 1);
971  assert(intermediate_x[0]->size() == x1.size());
972  assert(intermediate_u.size() == 1);
973  assert(intermediate_u[0]->size() == u1.size());
974  assert(dt > 0 && "dt must be greater then zero!");
975 
976  Eigen::VectorXd f1(integral_value.size());
977  fun(x1, u1, f1);
978 
979  Eigen::VectorXd f2(integral_value.size());
980  fun(x2, u2, f2);
981 
982  *intermediate_x[0] = 0.5 * (x1 + x2) + dt / 8.0 * (f1 - f2);
983  *intermediate_u[0] = 0.5 * (u1 + u2);
984 
985  fun(*intermediate_x[0], *intermediate_u[0], integral_value);
986 
987  integral_value = dt / 6.0 * (f1 + 4.0 * integral_value + f2);
988  }
989 
990  bool interpolate(const Eigen::Ref<const Eigen::VectorXd>& x1, const Eigen::Ref<const Eigen::VectorXd>& u1,
991  const Eigen::Ref<const Eigen::VectorXd>& x2, const Eigen::Ref<const Eigen::VectorXd>& u2, double dt,
992  const SystemDynamicsInterface& system, const Range& range, std::vector<Eigen::VectorXd>& states,
993  std::vector<Eigen::VectorXd>& controls) override
994  {
995  if (range.getStart() < 0 || range.getEnd() > dt)
996  {
997  PRINT_ERROR_NAMED("range must be in the interval [0, dt]");
998  return false;
999  }
1000 
1001  states.push_back(x1);
1002  controls.push_back(u1);
1003 
1004  if (dt < 1e-15) return true;
1005 
1006  Eigen::VectorXd f1(system.getStateDimension());
1007  system.dynamics(x1, u1, f1);
1008 
1009  Eigen::VectorXd f2(system.getStateDimension());
1010  system.dynamics(x2, u2, f2);
1011 
1012  Eigen::VectorXd xc = 0.5 * (x1 + x2) + dt / 8.0 * (f1 - f2);
1013  Eigen::VectorXd uc = 0.5 * (u1 + u2);
1014 
1015  Eigen::VectorXd fc(system.getStateDimension());
1016  system.dynamics(xc, uc, fc);
1017 
1018  int n = range.getNumInRange();
1019  double t = range.getStart() + range.getStep(); // x1 and u1 are already added
1020  for (int i = 1; i < n; ++i, t += range.getStep())
1021  {
1022  double tau = (t - range.getStart());
1023 
1024  // quadratic interpolation
1025  // Eigen::VectorXd u_interp =
1026  // 2.0 / dt_square * ((tau - dt / 2.0) * (tau - dt) * u1 - 2.0 * tau * (tau - dt) * u_center + tau * (tau - dt / 2.0) *
1027  // u2);
1028 
1029  // linear interpolation for controls
1030  controls.push_back(u1 + tau / dt * (u2 - u1));
1031 
1032  // cubic interpolation for states (since quadratic interplation for xdot)
1033  Eigen::VectorXd x_interp = x1 + f1 * tau + 0.5 / dt * tau * tau * (-3.0 * f1 + 4.0 * fc - f2) +
1034  1.0 / 3.0 / (dt * dt) * std::pow(tau, 3) * (2.0 * f1 - 4.0 * fc + 2.0 * f2);
1035  states.push_back(x_interp);
1036  }
1037 
1038  if (range.includeEnd())
1039  {
1040  states.push_back(x2);
1041  controls.push_back(u2);
1042  }
1043  return true;
1044  }
1045 
1046  bool computeIntermediateControls(const Eigen::Ref<const Eigen::VectorXd>& u1, const Eigen::Ref<const Eigen::VectorXd>& u2,
1047  std::vector<Eigen::VectorXd*>& intermediate_u) override
1048  {
1049  assert(intermediate_u.size() == 1);
1050  assert(intermediate_u[0]->size() == u1.size());
1051  *intermediate_u[0] = 0.5 * (u1 + u2);
1052  return true;
1053  }
1054 
1055 #ifdef MESSAGE_SUPPORT
1056  // Implements interface method
1057  void toMessage(messages::Quadrature& message) const override
1058  {
1059  QuadratureCollocationInterface::toMessage(message);
1060  message.mutable_hermite_simpson_linear_u();
1061  }
1062 #endif
1063 };
1064 
1065 FACTORY_REGISTER_QUADRATURE_COLLOCATION(QuadratureHermiteSimpsonLinearControl)
1066 
1067 
1087 class QuadratureHermiteSimpson : public QuadratureCollocationInterface
1088 {
1089  public:
1090  // Implements interface method
1091  DynamicsEvalInterface::Ptr getInstance() const override { return std::make_shared<QuadratureHermiteSimpson>(); }
1092 
1093  // Implements interface method
1094  bool isSupportingCompressedStatesMode() const override { return true; }
1095  bool isIntermediateControlSubjectToOptim() const override { return true; }
1096  int getNumIntermediatePoints() const override { return 1; }
1097 
1098  int getNumIntermediateControls() const override { return 1; }
1099  int getNumIntermediateStates() const override { return 0; }
1100 
1101  // Implements interface method
1102  void quadrature(const std::vector<const Eigen::VectorXd*>& x1_points, const std::vector<const Eigen::VectorXd*>& u1_points,
1103  const Eigen::VectorXd& u2, const Eigen::VectorXd& x2, double dt, const SystemDynamicsInterface& system,
1104  Eigen::Ref<Eigen::VectorXd> integral_value) override
1105  {
1106  assert(x1_points.size() == 1);
1107  assert(u1_points.size() == 2);
1108 
1109  Eigen::VectorXd f1(integral_value.size());
1110  system.dynamics(*x1_points.front(), *u1_points[0], f1);
1111 
1112  Eigen::VectorXd f2(integral_value.size());
1113  system.dynamics(x2, u2, f2);
1114 
1115  Eigen::VectorXd x_mid = 0.5 * (*x1_points.front() + x2) + dt / 8.0 * (f1 - f2);
1116  system.dynamics(x_mid, *u1_points[1], integral_value);
1117 
1118  integral_value = dt / 6.0 * (f1 + 4.0 * integral_value + f2);
1119  }
1120 
1121  void quadrature(const std::vector<const Eigen::VectorXd*>& x1_points, const std::vector<const Eigen::VectorXd*>& u1_points,
1122  const Eigen::VectorXd& u2, const Eigen::VectorXd& x2, double dt,
1123  const BinaryFunction& fun, Eigen::Ref<Eigen::VectorXd> integral_value) override
1124  {
1125  assert(x1_points.size() == 1);
1126  assert(u1_points.size() == 2);
1127 
1128  Eigen::VectorXd f1(integral_value.size());
1129  fun(*x1_points.front(), *u1_points[0], f1);
1130 
1131  Eigen::VectorXd f2(integral_value.size());
1132  fun(x2, u2, f2);
1133 
1134  Eigen::VectorXd x_mid = 0.5 * (*x1_points.front() + x2) + dt / 8.0 * (f1 - f2);
1135  fun(x_mid, *u1_points[1], integral_value);
1136 
1137  integral_value = dt / 6.0 * (f1 + 4.0 * integral_value + f2);
1138  }
1139 
1140  bool interpolate(const std::vector<const Eigen::VectorXd*>& x1_points, const std::vector<const Eigen::VectorXd*>& u1_points,
1141  const Eigen::VectorXd& u2, const Eigen::VectorXd& x2, double t1, double dt, double dt_interp,
1142  const SystemDynamicsInterface& system, bool skip_u2_x2, TimeSeries& ts_x, TimeSeries& ts_u) override
1143  {
1144  assert(x1_points.size() == 1);
1145  assert(u1_points.size() == 2);
1146 
1147  if (dt_interp <= 0 || dt_interp >= dt)
1148  {
1149  ts_x.add(t1, *x1_points[0]);
1150  ts_u.add(t1, *u1_points[0]);
1151  }
1152  else if (dt > 0)
1153  {
1154  Eigen::VectorXd f1(x2.size());
1155  system.dynamics(*x1_points.front(), *u1_points.front(), f1);
1156 
1157  Eigen::VectorXd f2(x2.size());
1158  system.dynamics(x2, u2, f2);
1159 
1160  Eigen::VectorXd fc(x2.size());
1161  Eigen::VectorXd xc = 0.5 * (*x1_points.front() + x2) + dt / 8.0 * (f1 - f2);
1162  system.dynamics(xc, *u1_points[1], fc);
1163 
1164  int n = std::floor(dt / dt_interp);
1165  double t = t1;
1166  for (int i = 0; i < n; ++i)
1167  {
1168  double tau = (t - t1);
1169 
1170  // cubic interpolation for states (since quadratic interplation for xdot)
1171  Eigen::VectorXd x_interp = *x1_points.front() + f1 * tau + 0.5 / dt * tau * tau * (-3.0 * f1 + 4.0 * fc - f2) +
1172  1.0 / 3.0 / (dt * dt) * std::pow(tau, 3) * (2.0 * f1 - 4.0 * fc + 2.0 * f2);
1173 
1174  Eigen::VectorXd u_interp = *u1_points[0] + tau / dt * (-3.0 * *u1_points[0] + 4.0 * *u1_points[1] - u2) +
1175  2.0 * tau * tau / (dt * dt) * (*u1_points[0] - 2.0 * *u1_points[1] + u2);
1176 
1177  ts_x.add(t1 + tau, x_interp);
1178  ts_u.add(t1 + tau, u_interp);
1179 
1180  t += dt_interp;
1181  }
1182  }
1183 
1184  if (!skip_u2_x2)
1185  {
1186  ts_x.add(t1 + dt, x2);
1187  ts_u.add(t1 + dt, u2);
1188  }
1189  return true;
1190  }
1191 
1192  void quadrature(const StateVector& x1, const InputVector& u1, const StateVector& x2, double dt, const SystemDynamicsInterface& system,
1193  Eigen::Ref<Eigen::VectorXd> integral_value) override
1194  {
1195  assert(integral_value.size() == x1.size());
1196  assert(dt > 0 && "dt must be greater then zero!");
1197 
1198  Eigen::VectorXd f1(x1.size());
1199  system.dynamics(x1, u1, f1);
1200 
1201  Eigen::VectorXd f2(x1.size());
1202  system.dynamics(x2, u1, f2);
1203 
1204  Eigen::VectorXd x_center = 0.5 * (x1 + x2) + dt / 8.0 * (f1 - f2);
1205 
1206  system.dynamics(x_center, u1, integral_value);
1207 
1208  integral_value = dt / 6.0 * (f1 + 4.0 * integral_value + f2);
1209  }
1210 
1211  void quadrature(const StateVector& x1, const InputVector& u1, const StateVector& x2, double dt, const SystemDynamicsInterface& system,
1212  Eigen::Ref<Eigen::VectorXd> integral_value, std::vector<Eigen::VectorXd*>& intermediate_x) override
1213  {
1214  assert(integral_value.size() == x1.size());
1215  assert(dt > 0 && "dt must be greater then zero!");
1216  assert(intermediate_x.size() == 1);
1217  assert(intermediate_x[0]->size() == x1.size());
1218 
1219  Eigen::VectorXd f1(x1.size());
1220  system.dynamics(x1, u1, f1);
1221 
1222  Eigen::VectorXd f2(x1.size());
1223  system.dynamics(x2, u1, f2);
1224 
1225  intermediate_x[0]->noalias() = 0.5 * (x1 + x2) + dt / 8.0 * (f1 - f2);
1226 
1227  system.dynamics(*intermediate_x[0], u1, integral_value);
1228 
1229  integral_value = dt / 6.0 * (f1 + 4.0 * integral_value + f2);
1230  }
1231 
1232  // Implements interface method
1233  void quadrature(const StateVector& x1, const InputVector& u1, const StateVector& x2, const InputVector& u2, double dt,
1234  const SystemDynamicsInterface& system, Eigen::Ref<Eigen::VectorXd> integral_value) override
1235  {
1236  PRINT_WARNING_NAMED(
1237  "This function overload is not supported by HermiteSimpson-Collocation since we require the midpoint between u1 and u2 as extra "
1238  "argument.");
1239  }
1240 
1241  void quadrature(const StateVector& x1, const InputVector& u1, const StateVector& x2, const InputVector& u2, double dt,
1242  const SystemDynamicsInterface& system, const std::vector<Eigen::VectorXd*>& intermediate_x,
1243  const std::vector<Eigen::VectorXd*>& intermediate_u, Eigen::Ref<Eigen::VectorXd> integral_value,
1244  Eigen::Ref<Eigen::VectorXd> interm_x_error) override
1245  {
1246  assert(intermediate_x.size() == 1);
1247  assert(intermediate_x[0]->size() == x1.size());
1248  assert(intermediate_u.size() == 1);
1249  assert(intermediate_u[0]->size() == u1.size());
1250  assert(dt > 0 && "dt must be greater then zero!");
1251 
1252  Eigen::VectorXd f1(integral_value.size());
1253  system.dynamics(x1, u1, f1);
1254 
1255  Eigen::VectorXd f2(integral_value.size());
1256  system.dynamics(x2, u2, f2);
1257 
1258  system.dynamics(*intermediate_x[0], *intermediate_u[0], integral_value);
1259 
1260  integral_value = dt / 6.0 * (f1 + 4.0 * integral_value + f2);
1261 
1262  assert(interm_x_error.size() == x1.size());
1263  interm_x_error = *intermediate_x[0] - (0.5 * (x1 + x2) + dt / 8.0 * (f1 - f2));
1264  }
1265 
1266  void quadrature(const StateVector& x1, const InputVector& u1, const StateVector& x2, const InputVector& u2, double dt,
1267  const SystemDynamicsInterface& system, const std::vector<Eigen::VectorXd*>& intermediate_u,
1268  Eigen::Ref<Eigen::VectorXd> integral_value, std::vector<Eigen::VectorXd*>& intermediate_x) override
1269  {
1270  assert(intermediate_x.size() == 1);
1271  assert(intermediate_x[0]->size() == x1.size());
1272  assert(intermediate_u.size() == 1);
1273  assert(intermediate_u[0]->size() == u1.size());
1274  assert(dt > 0 && "dt must be greater then zero!");
1275 
1276  Eigen::VectorXd f1(integral_value.size());
1277  system.dynamics(x1, u1, f1);
1278 
1279  Eigen::VectorXd f2(integral_value.size());
1280  system.dynamics(x2, u2, f2);
1281 
1282  intermediate_x[0]->noalias() = 0.5 * (x1 + x2) + dt / 8.0 * (f1 - f2);
1283  system.dynamics(*intermediate_x[0], *intermediate_u[0], integral_value);
1284 
1285  integral_value = dt / 6.0 * (f1 + 4.0 * integral_value + f2);
1286  }
1287 
1288  void quadrature(const StateVector& x1, const InputVector& u1, const StateVector& x2, const InputVector& u2, double dt,
1289  const SystemDynamicsInterface& system, Eigen::Ref<Eigen::VectorXd> integral_value, std::vector<Eigen::VectorXd*>& intermediate_x,
1290  std::vector<Eigen::VectorXd*>& intermediate_u) override
1291  {
1292  PRINT_WARNING_NAMED(
1293  "This function overload is not supported by HermiteSimpson-Collocation since we require the midpoint between u1 and u2 as extra "
1294  "argument.");
1295  }
1296 
1297  // Implements interface method
1298  void quadrature(const Eigen::Ref<const Eigen::VectorXd>& x1, const Eigen::Ref<const Eigen::VectorXd>& x2, double dt, const UnaryFunction& fun,
1299  Eigen::Ref<Eigen::VectorXd> integral_value) override
1300  {
1301  assert(dt > 0 && "dt must be greater then zero!");
1302 
1303  Eigen::VectorXd f1(integral_value.size());
1304  fun(x1, f1);
1305 
1306  Eigen::VectorXd f2(integral_value.size());
1307  fun(x2, f2);
1308 
1309  Eigen::VectorXd x_center = 0.5 * (x1 + x2) + dt / 8.0 * (f1 - f2);
1310 
1311  fun(x_center, integral_value);
1312 
1313  integral_value = dt / 6.0 * (f1 + 4.0 * integral_value + f2);
1314  }
1315 
1316  // Implements interface method
1317  void quadrature(const Eigen::Ref<const Eigen::VectorXd>& x1, const Eigen::Ref<const Eigen::VectorXd>& x2, double dt,
1318  const std::vector<Eigen::VectorXd*>& intermediate_x, const UnaryFunction& fun,
1319  Eigen::Ref<Eigen::VectorXd> integral_value) override
1320  {
1321  assert(intermediate_x.size() == 1);
1322  assert(intermediate_x[0]->size() == x1.size());
1323  assert(dt > 0 && "dt must be greater then zero!");
1324 
1325  Eigen::VectorXd f1(integral_value.size());
1326  fun(x1, f1);
1327 
1328  Eigen::VectorXd f2(integral_value.size());
1329  fun(x2, f2);
1330 
1331  fun(*intermediate_x[0], integral_value);
1332 
1333  integral_value = dt / 6.0 * (f1 + 4.0 * integral_value + f2);
1334  }
1335 
1336  // Implements interface method
1337  void quadrature(const Eigen::Ref<const Eigen::VectorXd>& x1, const Eigen::Ref<const Eigen::VectorXd>& x2, double dt, const UnaryFunction& fun,
1338  Eigen::Ref<Eigen::VectorXd> integral_value, std::vector<Eigen::VectorXd*>& intermediate_x) override
1339  {
1340  assert(intermediate_x.size() == 1);
1341  assert(intermediate_x[0]->size() == x1.size());
1342  assert(dt > 0 && "dt must be greater then zero!");
1343 
1344  Eigen::VectorXd f1(integral_value.size());
1345  fun(x1, f1);
1346 
1347  Eigen::VectorXd f2(integral_value.size());
1348  fun(x2, f2);
1349 
1350  *intermediate_x[0] = 0.5 * (x1 + x2) + dt / 8.0 * (f1 - f2);
1351 
1352  fun(*intermediate_x[0], integral_value);
1353 
1354  integral_value = dt / 6.0 * (f1 + 4.0 * integral_value + f2);
1355  }
1356 
1357  // Implements interface method
1358  void quadrature(const Eigen::Ref<const Eigen::VectorXd>& x1, const Eigen::Ref<const Eigen::VectorXd>& u1,
1359  const Eigen::Ref<const Eigen::VectorXd>& x2, const Eigen::Ref<const Eigen::VectorXd>& u2, double dt, const BinaryFunction& fun,
1360  Eigen::Ref<Eigen::VectorXd> integral_value) override
1361  {
1362  PRINT_WARNING_NAMED(
1363  "This function overload is not supported by HermiteSimpson-Collocation since we require the midpoint between u1 and u2 as extra "
1364  "argument.");
1365  }
1366 
1367  // Implements interface method
1368  void quadrature(const Eigen::Ref<const Eigen::VectorXd>& x1, const Eigen::Ref<const Eigen::VectorXd>& u1,
1369  const Eigen::Ref<const Eigen::VectorXd>& x2, const Eigen::Ref<const Eigen::VectorXd>& u2, double dt,
1370  const std::vector<Eigen::VectorXd*>& intermediate_x, const std::vector<Eigen::VectorXd*>& intermediate_u,
1371  const BinaryFunction& fun, Eigen::Ref<Eigen::VectorXd> integral_value) override
1372  {
1373  assert(intermediate_x.size() == 1);
1374  assert(intermediate_x[0]->size() == x1.size());
1375  assert(intermediate_u.size() == 1);
1376  assert(intermediate_u[0]->size() == u1.size());
1377  assert(dt > 0 && "dt must be greater then zero!");
1378 
1379  Eigen::VectorXd f1(integral_value.size());
1380  fun(x1, u1, f1);
1381 
1382  Eigen::VectorXd f2(integral_value.size());
1383  fun(x2, u2, f2);
1384 
1385  fun(*intermediate_x[0], *intermediate_u[0], integral_value);
1386 
1387  integral_value = dt / 6.0 * (f1 + 4.0 * integral_value + f2);
1388  }
1389 
1390  // Implements interface method
1391  void quadrature(const Eigen::Ref<const Eigen::VectorXd>& x1, const Eigen::Ref<const Eigen::VectorXd>& u1,
1392  const Eigen::Ref<const Eigen::VectorXd>& x2, const Eigen::Ref<const Eigen::VectorXd>& u2, double dt, const BinaryFunction& fun,
1393  Eigen::Ref<Eigen::VectorXd> integral_value, std::vector<Eigen::VectorXd*>& intermediate_x,
1394  std::vector<Eigen::VectorXd*>& intermediate_u) override
1395  {
1396  PRINT_WARNING_NAMED(
1397  "This function overload is not supported by HermiteSimpson-Collocation since we require the midpoint between u1 and u2 as extra "
1398  "argument.");
1399  }
1400 
1401  bool interpolate(const Eigen::Ref<const Eigen::VectorXd>& x1, const Eigen::Ref<const Eigen::VectorXd>& u1,
1402  const Eigen::Ref<const Eigen::VectorXd>& x2, const Eigen::Ref<const Eigen::VectorXd>& u2, double dt,
1403  const SystemDynamicsInterface& system, const Range& range, std::vector<Eigen::VectorXd>& states,
1404  std::vector<Eigen::VectorXd>& controls) override
1405  {
1406  return false;
1407  }
1408 
1409  bool interpolate(const std::vector<const Eigen::VectorXd*>& x, const std::vector<const Eigen::VectorXd*>& u, double dt,
1410  const SystemDynamicsInterface& system, const Range& range, std::vector<Eigen::VectorXd>& states,
1411  std::vector<Eigen::VectorXd>& controls) override
1412  {
1413  if ((x.size() != 2 && x.size() != 3) || u.size() != 3)
1414  {
1415  PRINT_DEBUG_NAMED("x-dim must be 2 in compressed mode and 3 in uncompressed mode. U-dim must be 3");
1416  return false;
1417  }
1418 
1419  if (range.getStart() < 0 || range.getEnd() > dt)
1420  {
1421  PRINT_ERROR_NAMED("range must be in the interval [0, dt]");
1422  return false;
1423  }
1424 
1425  states.push_back(*x[0]);
1426  controls.push_back(*u[0]);
1427 
1428  if (dt < 1e-15) return true;
1429 
1430  Eigen::VectorXd f1(system.getStateDimension());
1431  system.dynamics(*x[0], *u[0], f1);
1432 
1433  Eigen::VectorXd f2(system.getStateDimension());
1434  system.dynamics(*x.back(), *u[2], f2);
1435 
1436  Eigen::VectorXd fc(system.getStateDimension());
1437  if (x.size() == 2)
1438  {
1439  Eigen::VectorXd xc = 0.5 * (*x[0] + *x[1]) + dt / 8.0 * (f1 - f2);
1440  system.dynamics(xc, *u[1], fc);
1441  }
1442  else
1443  {
1444  assert(x.size() == 3);
1445  system.dynamics(*x[1], *u[1], fc);
1446  }
1447 
1448  int n = range.getNumInRange();
1449  double t = range.getStart() + range.getStep(); // x1 and u1 are already added
1450  for (int i = 1; i < n; ++i, t += range.getStep())
1451  {
1452  double tau = (t - range.getStart());
1453 
1454  // quadratic interpolation
1455  // Eigen::VectorXd u_interp =
1456  // 2.0 / (dt * dt) * ((tau - dt / 2.0) * (tau - dt) * *u[0] - 2.0 * tau * (tau - dt) * *u[1] + tau * (tau - dt / 2.0) * *u[2]);
1457  Eigen::VectorXd u_interp = *u[0] + tau / dt * (-3 * *u[0] + 4 * *u[1] - *u[2]) + 2 * tau * tau / (dt * dt) * (*u[0] - 2 * *u[1] + *u[2]);
1458 
1459  // linear interpolation for controls
1460  controls.push_back(u_interp);
1461 
1462  // cubic interpolation for states (since quadratic interplation for xdot)
1463  Eigen::VectorXd x_interp = *x[0] + f1 * tau + 0.5 / dt * tau * tau * (-3.0 * f1 + 4.0 * fc - f2) +
1464  1.0 / 3.0 / (dt * dt) * std::pow(tau, 3) * (2.0 * f1 - 4.0 * fc + 2.0 * f2);
1465  states.push_back(x_interp);
1466  }
1467 
1468  if (range.includeEnd())
1469  {
1470  states.push_back(*x.back());
1471  controls.push_back(*u[2]);
1472  }
1473  return true;
1474  }
1475 
1476  bool computeIntermediateControls(const Eigen::Ref<const Eigen::VectorXd>& u1, const Eigen::Ref<const Eigen::VectorXd>& u2,
1477  std::vector<Eigen::VectorXd*>& intermediate_u) override
1478  {
1479  return false;
1480  }
1481 
1482 #ifdef MESSAGE_SUPPORT
1483  // Implements interface method
1484  void toMessage(messages::Quadrature& message) const override
1485  {
1486  QuadratureCollocationInterface::toMessage(message);
1487  message.mutable_hermite_simpson();
1488  }
1489 #endif
1490 };
1491 
1492 FACTORY_REGISTER_QUADRATURE_COLLOCATION(QuadratureHermiteSimpson)
1493 
1494 } // namespace corbo
1495 
1496 #endif // SRC_NUMERICS_INCLUDE_CORBO_NUMERICS_QUADRATURE_H_
PRINT_WARNING_NAMED
#define PRINT_WARNING_NAMED(msg)
Definition: console.h:255
corbo::QuadratureHermiteSimpsonLinearControl::quadrature
void quadrature(const StateVector &x1, const InputVector &u1, const StateVector &x2, double dt, const SystemDynamicsInterface &system, Eigen::Ref< Eigen::VectorXd > integral_value, std::vector< Eigen::VectorXd * > &intermediate_x) override
Definition: quadrature.h:768
corbo::QuadratureHermiteSimpson::quadrature
void quadrature(const StateVector &x1, const InputVector &u1, const StateVector &x2, double dt, const SystemDynamicsInterface &system, Eigen::Ref< Eigen::VectorXd > integral_value, std::vector< Eigen::VectorXd * > &intermediate_x) override
Definition: quadrature.h:1233
corbo::SystemDynamicsInterface::dynamics
virtual void dynamics(const Eigen::Ref< const StateVector > &x, const Eigen::Ref< const ControlVector > &u, Eigen::Ref< StateVector > f) const =0
Evaluate the system dynamics equation.
corbo::QuadratureRectangleRule::quadrature
void quadrature(const std::vector< const Eigen::VectorXd * > &x1_points, const std::vector< const Eigen::VectorXd * > &u1_points, const Eigen::VectorXd &u2, const Eigen::VectorXd &x2, double dt, const SystemDynamicsInterface &system, Eigen::Ref< Eigen::VectorXd > integral_value) override
Allocate memory for a given state dimension.
Definition: quadrature.h:107
corbo::QuadratureHermiteSimpson::quadrature
void quadrature(const StateVector &x1, const InputVector &u1, const StateVector &x2, const InputVector &u2, double dt, const SystemDynamicsInterface &system, const std::vector< Eigen::VectorXd * > &intermediate_u, Eigen::Ref< Eigen::VectorXd > integral_value, std::vector< Eigen::VectorXd * > &intermediate_x) override
Definition: quadrature.h:1288
corbo::SystemDynamicsInterface::getStateDimension
virtual int getStateDimension() const =0
Return state dimension (x)
corbo::QuadratureTrapezoidalRule::getNumIntermediateControls
int getNumIntermediateControls() const override
Definition: quadrature.h:348
PRINT_ERROR_NAMED
#define PRINT_ERROR_NAMED(msg)
Definition: console.h:260
corbo::SystemDynamicsInterface
Interface class for system dynamic equations.
Definition: system_dynamics_interface.h:88
corbo::QuadratureHermiteSimpsonLinearControl::computeIntermediateControls
bool computeIntermediateControls(const Eigen::Ref< const Eigen::VectorXd > &u1, const Eigen::Ref< const Eigen::VectorXd > &u2, std::vector< Eigen::VectorXd * > &intermediate_u) override
Definition: quadrature.h:1068
corbo::QuadratureRectangleRule::getInstance
DynamicsEvalInterface::Ptr getInstance() const override
Return a newly created shared instance of the implemented class.
Definition: quadrature.h:96
corbo::CollocationInterface::InputVector
Eigen::VectorXd InputVector
Definition: collocation_interface.h:114
Eigen::half_impl::pow
EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half pow(const half &a, const half &b)
Definition: Half.h:477
corbo
Definition: communication/include/corbo-communication/utilities.h:37
corbo::QuadratureRectangleRule::computeIntermediateControls
bool computeIntermediateControls(const Eigen::Ref< const Eigen::VectorXd > &u1, const Eigen::Ref< const Eigen::VectorXd > &u2, std::vector< Eigen::VectorXd * > &intermediate_u) override
Definition: quadrature.h:328
corbo::QuadratureHermiteSimpson::getNumIntermediateControls
int getNumIntermediateControls() const override
Definition: quadrature.h:1120
corbo::Range::getStart
double getStart() const
Definition: range.h:106
corbo::QuadratureTrapezoidalRule::getInstance
DynamicsEvalInterface::Ptr getInstance() const override
Return a newly created shared instance of the implemented class.
Definition: quadrature.h:341
corbo::QuadratureCollocationInterface::UnaryFunction
const std::function< void(const Eigen::VectorXd &, Eigen::Ref< Eigen::VectorXd >)> UnaryFunction
Definition: quadrature_interface.h:93
corbo::QuadratureHermiteSimpsonLinearControl::isSupportingCompressedStatesMode
bool isSupportingCompressedStatesMode() const override
Definition: quadrature.h:651
corbo::Range::getStep
double getStep() const
Definition: range.h:107
corbo::TimeSeries::add
bool add(double time, const std::vector< double > &values)
Add time and value vector pair (STL version)
Definition: time_series.cpp:70
corbo::QuadratureHermiteSimpsonLinearControl::getNumIntermediateControls
int getNumIntermediateControls() const override
Definition: quadrature.h:655
corbo::QuadratureHermiteSimpsonLinearControl::getNumIntermediateStates
int getNumIntermediateStates() const override
Definition: quadrature.h:656
corbo::Range
Definition: range.h:59
corbo::QuadratureTrapezoidalRule::getNumIntermediateStates
int getNumIntermediateStates() const override
Definition: quadrature.h:349
corbo::QuadratureTrapezoidalRule::getNumIntermediatePoints
int getNumIntermediatePoints() const override
Definition: quadrature.h:346
corbo::QuadratureRectangleRule::isSupportingCompressedStatesMode
bool isSupportingCompressedStatesMode() const override
Definition: quadrature.h:99
corbo::QuadratureHermiteSimpson::interpolate
bool interpolate(const std::vector< const Eigen::VectorXd * > &x1_points, const std::vector< const Eigen::VectorXd * > &u1_points, const Eigen::VectorXd &u2, const Eigen::VectorXd &x2, double t1, double dt, double dt_interp, const SystemDynamicsInterface &system, bool skip_u2_x2, TimeSeries &ts_x, TimeSeries &ts_u) override
Definition: quadrature.h:1162
corbo::QuadratureHermiteSimpsonLinearControl::isIntermediateControlSubjectToOptim
bool isIntermediateControlSubjectToOptim() const override
Definition: quadrature.h:652
corbo::QuadratureTrapezoidalRule::computeIntermediateControls
bool computeIntermediateControls(const Eigen::Ref< const Eigen::VectorXd > &u1, const Eigen::Ref< const Eigen::VectorXd > &u2, std::vector< Eigen::VectorXd * > &intermediate_u) override
Definition: quadrature.h:605
corbo::QuadratureHermiteSimpson::isSupportingCompressedStatesMode
bool isSupportingCompressedStatesMode() const override
Definition: quadrature.h:1116
corbo::QuadratureTrapezoidalRule::isIntermediateControlSubjectToOptim
bool isIntermediateControlSubjectToOptim() const override
Definition: quadrature.h:345
PRINT_DEBUG_NAMED
#define PRINT_DEBUG_NAMED(msg)
Definition: console.h:245
corbo::QuadratureHermiteSimpson::computeIntermediateControls
bool computeIntermediateControls(const Eigen::Ref< const Eigen::VectorXd > &u1, const Eigen::Ref< const Eigen::VectorXd > &u2, std::vector< Eigen::VectorXd * > &intermediate_u) override
Definition: quadrature.h:1498
corbo::QuadratureTrapezoidalRule::interpolate
bool interpolate(const std::vector< const Eigen::VectorXd * > &x1_points, const std::vector< const Eigen::VectorXd * > &u1_points, const Eigen::VectorXd &u2, const Eigen::VectorXd &x2, double t1, double dt, double dt_interp, const SystemDynamicsInterface &system, bool skip_u2_x2, TimeSeries &ts_x, TimeSeries &ts_u) override
Definition: quadrature.h:378
corbo::Range::includeEnd
bool includeEnd() const
Definition: range.h:110
corbo::QuadratureCollocationInterface::BinaryFunction
const std::function< void(const Eigen::VectorXd &, const Eigen::VectorXd &, Eigen::Ref< Eigen::VectorXd >)> BinaryFunction
Definition: quadrature_interface.h:94
corbo::QuadratureRectangleRule::interpolate
bool interpolate(const std::vector< const Eigen::VectorXd * > &x1_points, const std::vector< const Eigen::VectorXd * > &u1_points, const Eigen::VectorXd &u2, const Eigen::VectorXd &x2, double t1, double dt, double dt_interp, const SystemDynamicsInterface &system, bool skip_u2_x2, TimeSeries &ts_x, TimeSeries &ts_u) override
Definition: quadrature.h:129
x
Scalar * x
Definition: level1_cplx_impl.h:89
corbo::QuadratureHermiteSimpsonLinearControl::getNumIntermediatePoints
int getNumIntermediatePoints() const override
Definition: quadrature.h:653
corbo::QuadratureRectangleRule::getNumIntermediateStates
int getNumIntermediateStates() const override
Definition: quadrature.h:104
corbo::QuadratureHermiteSimpsonLinearControl::getInstance
DynamicsEvalInterface::Ptr getInstance() const override
Return a newly created shared instance of the implemented class.
Definition: quadrature.h:648
corbo::QuadratureRectangleRule::getNumIntermediatePoints
int getNumIntermediatePoints() const override
Definition: quadrature.h:101
corbo::TimeSeries
Time Series (trajectory resp. sequence of values w.r.t. time)
Definition: time_series.h:76
corbo::QuadratureTrapezoidalRule::isSupportingCompressedStatesMode
bool isSupportingCompressedStatesMode() const override
Definition: quadrature.h:344
corbo::CollocationInterface::StateVector
Eigen::VectorXd StateVector
Definition: collocation_interface.h:113
Eigen::Ref
A matrix or vector expression mapping an existing expression.
Definition: Ref.h:192
corbo::QuadratureTrapezoidalRule::quadrature
void quadrature(const std::vector< const Eigen::VectorXd * > &x1_points, const std::vector< const Eigen::VectorXd * > &u1_points, const Eigen::VectorXd &u2, const Eigen::VectorXd &x2, double dt, const SystemDynamicsInterface &system, Eigen::Ref< Eigen::VectorXd > integral_value) override
Allocate memory for a given state dimension.
Definition: quadrature.h:352
corbo::QuadratureRectangleRule::getNumIntermediateControls
int getNumIntermediateControls() const override
Definition: quadrature.h:103
corbo::QuadratureHermiteSimpson::getNumIntermediatePoints
int getNumIntermediatePoints() const override
Definition: quadrature.h:1118
corbo::QuadratureHermiteSimpsonLinearControl::interpolate
bool interpolate(const std::vector< const Eigen::VectorXd * > &x1_points, const std::vector< const Eigen::VectorXd * > &u1_points, const Eigen::VectorXd &u2, const Eigen::VectorXd &x2, double t1, double dt, double dt_interp, const SystemDynamicsInterface &system, bool skip_u2_x2, TimeSeries &ts_x, TimeSeries &ts_u) override
Definition: quadrature.h:699
utility::tuple::size
static constexpr size_t size(Tuple< Args... > &)
Provides access to the number of elements in a tuple as a compile-time constant expression.
Definition: TensorSyclTuple.h:143
corbo::QuadratureRectangleRule::isIntermediateControlSubjectToOptim
bool isIntermediateControlSubjectToOptim() const override
Definition: quadrature.h:100
quadrature_interface.h
corbo::Range::getNumInRange
int getNumInRange() const
Definition: range.h:115
corbo::Range::getEnd
double getEnd() const
Definition: range.h:108
n
PlainMatrixType mat * n
Definition: eigenvalues.cpp:41
corbo::QuadratureHermiteSimpsonLinearControl::quadrature
void quadrature(const std::vector< const Eigen::VectorXd * > &x1_points, const std::vector< const Eigen::VectorXd * > &u1_points, const Eigen::VectorXd &u2, const Eigen::VectorXd &x2, double dt, const SystemDynamicsInterface &system, Eigen::Ref< Eigen::VectorXd > integral_value) override
Allocate memory for a given state dimension.
Definition: quadrature.h:659
FACTORY_REGISTER_QUADRATURE_COLLOCATION
#define FACTORY_REGISTER_QUADRATURE_COLLOCATION(type)
Definition: quadrature_interface.h:214
corbo::QuadratureHermiteSimpson::quadrature
void quadrature(const std::vector< const Eigen::VectorXd * > &x1_points, const std::vector< const Eigen::VectorXd * > &u1_points, const Eigen::VectorXd &u2, const Eigen::VectorXd &x2, double dt, const SystemDynamicsInterface &system, Eigen::Ref< Eigen::VectorXd > integral_value) override
Allocate memory for a given state dimension.
Definition: quadrature.h:1124
corbo::DynamicsEvalInterface::Ptr
std::shared_ptr< DynamicsEvalInterface > Ptr
Definition: dynamics_eval_interface.h:101
corbo::QuadratureHermiteSimpson::isIntermediateControlSubjectToOptim
bool isIntermediateControlSubjectToOptim() const override
Definition: quadrature.h:1117
corbo::QuadratureHermiteSimpson::getNumIntermediateStates
int getNumIntermediateStates() const override
Definition: quadrature.h:1121
corbo::QuadratureHermiteSimpson::getInstance
DynamicsEvalInterface::Ptr getInstance() const override
Return a newly created shared instance of the implemented class.
Definition: quadrature.h:1113
floor
const EIGEN_DEVICE_FUNC FloorReturnType floor() const
Definition: ArrayCwiseUnaryOps.h:388

control_box_rst
Author(s): Christoph Rösmann
autogenerated on Wed Mar 2 2022 00:06:08