test_full_discretization_grid.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement
4  *
5  * Copyright (c) 2018,
6  * TU Dortmund - Institute of Control Theory and Systems Engineering.
7  * All rights reserved.
8  *
9  * This software is currently not released.
10  * Redistribution and use in source and binary forms,
11  * with or without modification, are prohibited.
12  *
13  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
14  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
15  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
16  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
17  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
18  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
19  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
20  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
21  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
22  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
23  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
24  * POSSIBILITY OF SUCH DAMAGE.
25  *
26  * Authors: Christoph Rösmann
27  *********************************************************************/
28 
29 #include <corbo-optimal-control/structured_ocp/discretization_grids/full_discretization_grid.h>
30 
31 #include <corbo-core/reference_trajectory.h>
32 
33 #include <corbo-core/console.h>
34 #include <corbo-core/macros.h>
35 #include <corbo-core/value_comparison.h>
36 
37 #include "gtest/gtest.h"
38 
39 using corbo::FullDiscretizationGrid;
40 using corbo::StaticReference;
41 using corbo::VectorVertex;
42 using corbo::ZeroReference;
43 
44 class TestFullDiscretizationGrid : public testing::Test
45 {
46  protected:
47  // You can do set-up work for each test here.
48  TestFullDiscretizationGrid() {}
49  // You can do clean-up work that doesn't throw exceptions here.
50  virtual ~TestFullDiscretizationGrid() {}
51  // If the constructor and destructor are not enough for setting up
52  // and cleaning up each test, you can define the following methods:
53 
54  // Code here will be called immediately after the constructor (right
55  // before each test).
56  void SetUp() override {}
57  // Code here will be called immediately after each test (right
58  // before the destructor).
59  // virtual void TearDown();
60 
61  FullDiscretizationGrid grid;
62 };
63 
64 TEST_F(TestFullDiscretizationGrid, initialize_grid_start_state_only)
65 {
66  Eigen::Vector2d x0(0, 5);
67 
68  Eigen::Vector2d u0(0, 0);
69 
70  int num_states = 5;
71  double dt = 0.1;
72 
73  grid.setHorizon(num_states, dt);
74  grid.initialize(x0, x0, u0);
75 
76  corbo::TimeSeries::Ptr sn = std::make_shared<corbo::TimeSeries>();
77  grid.getShootingNodeTimeSeries(sn);
78  EXPECT_DOUBLE_EQ(sn->getValueDimension(), 2);
79  EXPECT_DOUBLE_EQ(sn->getTimeDimension(), 5);
80 
81  Eigen::MatrixXd s_seq = sn->getValuesMatrixView();
82 
83  EXPECT_DOUBLE_EQ(s_seq(0, 0), 0.0);
84  EXPECT_DOUBLE_EQ(s_seq(1, 0), 5.0);
85  EXPECT_DOUBLE_EQ(s_seq(0, 1), 0.0);
86  EXPECT_DOUBLE_EQ(s_seq(1, 1), 5.0);
87  EXPECT_DOUBLE_EQ(s_seq(0, 2), 0.0);
88  EXPECT_DOUBLE_EQ(s_seq(1, 2), 5.0);
89  EXPECT_DOUBLE_EQ(s_seq(0, 3), 0.0);
90  EXPECT_DOUBLE_EQ(s_seq(1, 3), 5.0);
91  EXPECT_DOUBLE_EQ(s_seq(0, 4), 0.0);
92  EXPECT_DOUBLE_EQ(s_seq(1, 4), 5.0);
93 
94  corbo::TimeSeries::Ptr un = std::make_shared<corbo::TimeSeries>();
95  grid.getControlInputTimeSeries(un);
96  EXPECT_DOUBLE_EQ(un->getValueDimension(), 2);
97  EXPECT_DOUBLE_EQ(un->getTimeDimension(), 5);
98 
99  Eigen::MatrixXd u_seq = un->getValuesMatrixView();
100  EXPECT_TRUE(u_seq.isZero()) << u_seq;
101 
102  // check fixed flags (first shooting node must be fixed, others not)
103  EXPECT_TRUE(grid.getShootingIntervals()[0].shooting_node->isFixed());
104  for (int i = 1; i < (int)grid.getShootingIntervals().size(); ++i)
105  {
106  EXPECT_FALSE(grid.getShootingIntervals()[i].shooting_node->isFixed());
107  }
108 }
109 
110 TEST_F(TestFullDiscretizationGrid, initialize_grid_start_state_end_state)
111 {
112  Eigen::Vector2d x0(0, 5);
113  Eigen::Vector2d xf(5, 0);
114 
115  Eigen::Vector2d u0(0, 0);
116 
117  int num_states = 5;
118  double dt = 0.1;
119 
120  grid.setHorizon(num_states, dt);
121  grid.initialize(x0, xf, u0);
122 
123  corbo::TimeSeries::Ptr sn = std::make_shared<corbo::TimeSeries>();
124  grid.getShootingNodeTimeSeries(sn);
125  EXPECT_DOUBLE_EQ(sn->getValueDimension(), 2);
126  EXPECT_DOUBLE_EQ(sn->getTimeDimension(), 5);
127 
128  Eigen::MatrixXd s_seq = sn->getValuesMatrixView();
129 
130  EXPECT_DOUBLE_EQ(s_seq(0, 0), 0.0);
131  EXPECT_DOUBLE_EQ(s_seq(1, 0), 5.0);
132  EXPECT_DOUBLE_EQ(s_seq(0, 1), 1.25);
133  EXPECT_DOUBLE_EQ(s_seq(1, 1), 3.75);
134  EXPECT_DOUBLE_EQ(s_seq(0, 2), 2.5);
135  EXPECT_DOUBLE_EQ(s_seq(1, 2), 2.5);
136  EXPECT_DOUBLE_EQ(s_seq(0, 3), 3.75);
137  EXPECT_DOUBLE_EQ(s_seq(1, 3), 1.25);
138  EXPECT_DOUBLE_EQ(s_seq(0, 4), 5.0);
139  EXPECT_DOUBLE_EQ(s_seq(1, 4), 0.0);
140 
141  corbo::TimeSeries::Ptr un = std::make_shared<corbo::TimeSeries>();
142  grid.getControlInputTimeSeries(un);
143  EXPECT_DOUBLE_EQ(un->getValueDimension(), 2);
144  EXPECT_DOUBLE_EQ(un->getTimeDimension(), 5);
145 
146  Eigen::MatrixXd u_seq = un->getValuesMatrixView();
147  EXPECT_TRUE(u_seq.isZero()) << u_seq;
148 
149  // check fixed flags (first shooting node must be fixed, others not)
150  EXPECT_TRUE(grid.getShootingIntervals()[0].shooting_node->isFixed());
151  for (int i = 1; i < (int)grid.getShootingIntervals().size(); ++i)
152  {
153  EXPECT_FALSE(grid.getShootingIntervals()[i].shooting_node->isFixed());
154  }
155 }
156 
157 TEST_F(TestFullDiscretizationGrid, grid_prune_trajectory)
158 {
159  Eigen::Vector2d x0(0, 5);
160  Eigen::Vector2d xf(5, 0);
161 
162  Eigen::Vector2d u0(0, 0);
163 
164  int num_states = 5;
165  double dt = 0.1;
166 
167  grid.setHorizon(num_states, dt);
168  grid.initialize(x0, xf, u0);
169 
170  grid.pruneTrajectory(Eigen::Vector2d(2.4, 2.4)); // erase the first two intervals
171 
172  corbo::TimeSeries::Ptr sn = std::make_shared<corbo::TimeSeries>();
173  grid.getShootingNodeTimeSeries(sn);
174  EXPECT_DOUBLE_EQ(sn->getValueDimension(), 2);
175  EXPECT_DOUBLE_EQ(sn->getTimeDimension(), 3);
176 
177  Eigen::MatrixXd s_seq = sn->getValuesMatrixView();
178 
179  // first states must be updated to (2.4, 2.4) during pruning
180  EXPECT_DOUBLE_EQ(s_seq(0, 0), 2.4);
181  EXPECT_DOUBLE_EQ(s_seq(1, 0), 2.4);
182  EXPECT_DOUBLE_EQ(s_seq(0, 1), 3.75);
183  EXPECT_DOUBLE_EQ(s_seq(1, 1), 1.25);
184  EXPECT_DOUBLE_EQ(s_seq(0, 2), 5.0);
185  EXPECT_DOUBLE_EQ(s_seq(1, 2), 0.0);
186 
187  corbo::TimeSeries::Ptr un = std::make_shared<corbo::TimeSeries>();
188  grid.getControlInputTimeSeries(un);
189  EXPECT_DOUBLE_EQ(un->getValueDimension(), 2);
190  EXPECT_DOUBLE_EQ(un->getTimeDimension(), 3);
191 
192  Eigen::MatrixXd u_seq = un->getValuesMatrixView();
193  EXPECT_TRUE(u_seq.isZero()) << u_seq;
194 
195  // check fixed flags (first shooting node must be fixed, others not)
196  EXPECT_TRUE(grid.getShootingIntervals()[0].shooting_node->isFixed());
197  for (int i = 1; i < (int)grid.getShootingIntervals().size(); ++i)
198  {
199  EXPECT_FALSE(grid.getShootingIntervals()[i].shooting_node->isFixed());
200  }
201 }
202 
203 TEST_F(TestFullDiscretizationGrid, grid_prune_trajectory_keep_min)
204 {
205  Eigen::Vector2d x0(0, 5);
206  Eigen::Vector2d xf(5, 0);
207 
208  Eigen::Vector2d u0(0, 0);
209 
210  int num_states = 5;
211  double dt = 0.1;
212 
213  grid.setHorizon(num_states, dt);
214  grid.initialize(x0, xf, u0);
215 
216  // now specify final state as new initial state
217  grid.pruneTrajectory(xf);
218 
219  corbo::TimeSeries::Ptr sn = std::make_shared<corbo::TimeSeries>();
220  grid.getShootingNodeTimeSeries(sn);
221  EXPECT_DOUBLE_EQ(sn->getValueDimension(), 2);
222  EXPECT_DOUBLE_EQ(sn->getTimeDimension(), 2); // we always keep 2 samples (1 interval) in our OCP
223 
224  Eigen::MatrixXd s_seq = sn->getValuesMatrixView();
225 
226  // first states must be updated to (2.4, 2.4) during pruning
227  EXPECT_DOUBLE_EQ(s_seq(0, 0), 5.0);
228  EXPECT_DOUBLE_EQ(s_seq(1, 0), 0.0);
229  EXPECT_DOUBLE_EQ(s_seq(0, 1), 5.0);
230  EXPECT_DOUBLE_EQ(s_seq(1, 1), 0.0);
231 
232  corbo::TimeSeries::Ptr un = std::make_shared<corbo::TimeSeries>();
233  grid.getControlInputTimeSeries(un);
234  EXPECT_DOUBLE_EQ(un->getValueDimension(), 2);
235  EXPECT_DOUBLE_EQ(un->getTimeDimension(), 2); // we always keep 2 samples (1 interval) in our OCP
236 
237  Eigen::MatrixXd u_seq = un->getValuesMatrixView();
238  EXPECT_TRUE(u_seq.isZero()) << u_seq;
239 
240  // check fixed flags (first shooting node must be fixed, others not)
241  EXPECT_TRUE(grid.getShootingIntervals()[0].shooting_node->isFixed());
242  for (int i = 1; i < (int)grid.getShootingIntervals().size(); ++i)
243  {
244  EXPECT_FALSE(grid.getShootingIntervals()[i].shooting_node->isFixed());
245  }
246 }
247 
248 TEST_F(TestFullDiscretizationGrid, grid_extrapolate_trajectory)
249 {
250  Eigen::Vector2d x0(0, 5);
251  Eigen::Vector2d xf(5, 0);
252 
253  Eigen::Vector2d u0(0, 0);
254 
255  int num_states = 5;
256  double dt = 0.1;
257 
258  grid.setHorizon(num_states, dt);
259  grid.initialize(x0, xf, u0);
260 
261  grid.extrapolateTrajectory(2);
262 
263  corbo::TimeSeries::Ptr sn = std::make_shared<corbo::TimeSeries>();
264  grid.getShootingNodeTimeSeries(sn);
265  EXPECT_DOUBLE_EQ(sn->getValueDimension(), 2);
266  EXPECT_DOUBLE_EQ(sn->getTimeDimension(), 7);
267 
268  Eigen::MatrixXd s_seq = sn->getValuesMatrixView();
269 
270  EXPECT_DOUBLE_EQ(s_seq(0, 0), 0.0);
271  EXPECT_DOUBLE_EQ(s_seq(1, 0), 5.0);
272  EXPECT_DOUBLE_EQ(s_seq(0, 1), 1.25);
273  EXPECT_DOUBLE_EQ(s_seq(1, 1), 3.75);
274  EXPECT_DOUBLE_EQ(s_seq(0, 2), 2.5);
275  EXPECT_DOUBLE_EQ(s_seq(1, 2), 2.5);
276  EXPECT_DOUBLE_EQ(s_seq(0, 3), 3.75);
277  EXPECT_DOUBLE_EQ(s_seq(1, 3), 1.25);
278  EXPECT_DOUBLE_EQ(s_seq(0, 4), 5.0);
279  EXPECT_DOUBLE_EQ(s_seq(1, 4), 0.0);
280  EXPECT_DOUBLE_EQ(s_seq(0, 5), 6.25);
281  EXPECT_DOUBLE_EQ(s_seq(1, 5), -1.25);
282  EXPECT_DOUBLE_EQ(s_seq(0, 6), 7.5);
283  EXPECT_DOUBLE_EQ(s_seq(1, 6), -2.5);
284 
285  corbo::TimeSeries::Ptr un = std::make_shared<corbo::TimeSeries>();
286  grid.getControlInputTimeSeries(un);
287  EXPECT_DOUBLE_EQ(un->getValueDimension(), 2);
288  EXPECT_DOUBLE_EQ(un->getTimeDimension(), 7);
289 
290  Eigen::MatrixXd u_seq = un->getValuesMatrixView();
291  EXPECT_TRUE(u_seq.isZero()) << u_seq;
292 
293  // check fixed flags (first shooting node must be fixed, others not)
294  EXPECT_TRUE(grid.getShootingIntervals()[0].shooting_node->isFixed());
295  for (int i = 1; i < (int)grid.getShootingIntervals().size(); ++i)
296  {
297  EXPECT_FALSE(grid.getShootingIntervals()[i].shooting_node->isFixed());
298  }
299 }
300 
301 TEST_F(TestFullDiscretizationGrid, resample_trajectory_single_dt)
302 {
303  Eigen::Vector2d x0(0, 0);
304  Eigen::Vector2d xf(1, -1);
305  Eigen::Vector2d u0(0, 0);
306 
307  int num_states = 10;
308  double dt = 0.1;
309 
310  grid.setSingleDt(true);
311 
312  grid.setHorizon(num_states, dt);
313  grid.initialize(x0, xf, u0);
314 
315  // Now resample to 5 states
316  grid.resampleTrajectory(5);
317 
318  // states
319  corbo::TimeSeries::Ptr sn = std::make_shared<corbo::TimeSeries>();
320  grid.getShootingNodeTimeSeries(sn);
321  EXPECT_DOUBLE_EQ(sn->getValueDimension(), 2);
322  EXPECT_DOUBLE_EQ(sn->getTimeDimension(), 5);
323  Eigen::MatrixXd s_seq = sn->getValuesMatrixView();
324 
325  // Check values
326  for (int i = 0; i < s_seq.cols(); ++i)
327  {
328  EXPECT_DOUBLE_EQ(x0[0] + (double)i * (xf[0] - x0[0]) / 4.0, s_seq(0, i));
329  EXPECT_DOUBLE_EQ(x0[1] + (double)i * (xf[1] - x0[1]) / 4.0, s_seq(1, i));
330  }
331 
332  // controls
333  corbo::TimeSeries::Ptr un = std::make_shared<corbo::TimeSeries>();
334  grid.getControlInputTimeSeries(un);
335  EXPECT_DOUBLE_EQ(un->getValueDimension(), 2);
336  EXPECT_DOUBLE_EQ(un->getTimeDimension(), 5);
337 
338  Eigen::MatrixXd u_seq = un->getValuesMatrixView();
339  EXPECT_TRUE(u_seq.isZero()) << u_seq;
340 
341  // dt
342  EXPECT_DOUBLE_EQ(4.0 * grid.getFirstDt(), 0.9) << "dt: " << grid.getFirstDt();
343 
344  // check fixed flags (first shooting node must be fixed, others not)
345  EXPECT_TRUE(grid.getShootingIntervals()[0].shooting_node->isFixed());
346  for (int i = 1; i < (int)grid.getShootingIntervals().size(); ++i)
347  {
348  EXPECT_FALSE(grid.getShootingIntervals()[i].shooting_node->isFixed());
349  }
350 
351  // Now resample to 15 states
352  grid.resampleTrajectory(15);
353 
354  grid.getShootingNodeTimeSeries(sn);
355  EXPECT_DOUBLE_EQ(sn->getValueDimension(), 2);
356  EXPECT_DOUBLE_EQ(sn->getTimeDimension(), 15);
357  s_seq = sn->getValuesMatrixView();
358 
359  // Check values
360  for (int i = 0; i < s_seq.cols(); ++i)
361  {
362  EXPECT_DOUBLE_EQ(x0[0] + (double)i * (xf[0] - x0[0]) / 14.0, s_seq(0, i));
363  EXPECT_DOUBLE_EQ(x0[1] + (double)i * (xf[1] - x0[1]) / 14.0, s_seq(1, i));
364  }
365 
366  // controls
367  grid.getControlInputTimeSeries(un);
368  EXPECT_DOUBLE_EQ(un->getValueDimension(), 2);
369  EXPECT_DOUBLE_EQ(un->getTimeDimension(), 15);
370 
371  u_seq = un->getValuesMatrixView();
372  EXPECT_TRUE(u_seq.isZero()) << u_seq;
373 
374  // dt
375  EXPECT_DOUBLE_EQ(14.0 * grid.getFirstDt(), 0.9) << "dt: " << grid.getFirstDt();
376 
377  // check fixed flags (first shooting node must be fixed, others not)
378  EXPECT_TRUE(grid.getShootingIntervals()[0].shooting_node->isFixed());
379  for (int i = 1; i < (int)grid.getShootingIntervals().size(); ++i)
380  {
381  EXPECT_FALSE(grid.getShootingIntervals()[i].shooting_node->isFixed());
382  }
383 }
384 
385 TEST_F(TestFullDiscretizationGrid, resample_trajectory_single_dt2)
386 {
387  Eigen::Vector2d x0(0, 0);
388  Eigen::Vector2d xf(1, -1);
389  Eigen::Vector2d u0(0, 0);
390 
391  int num_states = 10;
392  double dt = 0.1;
393 
394  grid.setSingleDt(true);
395 
396  grid.setHorizon(num_states, dt);
397  grid.initialize(x0, xf, u0);
398 
399  // Now resample to 11 states
400  grid.resampleTrajectory(11);
401 
402  // states
403  corbo::TimeSeries::Ptr sn = std::make_shared<corbo::TimeSeries>();
404  grid.getShootingNodeTimeSeries(sn);
405  EXPECT_DOUBLE_EQ(sn->getValueDimension(), 2);
406  EXPECT_DOUBLE_EQ(sn->getTimeDimension(), 11);
407  Eigen::MatrixXd s_seq = sn->getValuesMatrixView();
408 
409  // Check values
410  for (int i = 0; i < s_seq.cols(); ++i)
411  {
412  EXPECT_DOUBLE_EQ(x0[0] + (double)i * (xf[0] - x0[0]) / 10.0, s_seq(0, i));
413  EXPECT_DOUBLE_EQ(x0[1] + (double)i * (xf[1] - x0[1]) / 10.0, s_seq(1, i));
414  }
415 
416  // controls
417  corbo::TimeSeries::Ptr un = std::make_shared<corbo::TimeSeries>();
418  grid.getControlInputTimeSeries(un);
419  EXPECT_DOUBLE_EQ(un->getValueDimension(), 2);
420  EXPECT_DOUBLE_EQ(un->getTimeDimension(), 11);
421 
422  Eigen::MatrixXd u_seq = un->getValuesMatrixView();
423  EXPECT_TRUE(u_seq.isZero()) << u_seq;
424 
425  // dt
426  EXPECT_DOUBLE_EQ(10.0 * grid.getFirstDt(), 0.9) << "dt: " << grid.getFirstDt();
427 
428  // check fixed flags (first shooting node must be fixed, others not)
429  EXPECT_TRUE(grid.getShootingIntervals()[0].shooting_node->isFixed());
430  for (int i = 1; i < (int)grid.getShootingIntervals().size(); ++i)
431  {
432  EXPECT_FALSE(grid.getShootingIntervals()[i].shooting_node->isFixed());
433  }
434 
435  // Now resample to 10 states
436  grid.resampleTrajectory(10);
437 
438  grid.getShootingNodeTimeSeries(sn);
439  EXPECT_DOUBLE_EQ(sn->getValueDimension(), 2);
440  EXPECT_DOUBLE_EQ(sn->getTimeDimension(), 10);
441  s_seq = sn->getValuesMatrixView();
442 
443  // Check values
444  for (int i = 0; i < s_seq.cols(); ++i)
445  {
446  EXPECT_DOUBLE_EQ(x0[0] + (double)i * (xf[0] - x0[0]) / 9.0, s_seq(0, i));
447  EXPECT_DOUBLE_EQ(x0[1] + (double)i * (xf[1] - x0[1]) / 9.0, s_seq(1, i));
448  }
449 
450  // controls
451  grid.getControlInputTimeSeries(un);
452  EXPECT_DOUBLE_EQ(un->getValueDimension(), 2);
453  EXPECT_DOUBLE_EQ(un->getTimeDimension(), 10);
454 
455  u_seq = un->getValuesMatrixView();
456  EXPECT_TRUE(u_seq.isZero()) << u_seq;
457 
458  // dt
459  EXPECT_DOUBLE_EQ(9.0 * grid.getFirstDt(), 0.9) << "dt: " << grid.getFirstDt();
460 
461  // check fixed flags (first shooting node must be fixed, others not)
462  EXPECT_TRUE(grid.getShootingIntervals()[0].shooting_node->isFixed());
463  for (int i = 1; i < (int)grid.getShootingIntervals().size(); ++i)
464  {
465  EXPECT_FALSE(grid.getShootingIntervals()[i].shooting_node->isFixed());
466  }
467 }
468 
469 TEST_F(TestFullDiscretizationGrid, grid_shift_trajectory1)
470 {
471  Eigen::Vector2d x0(3, 0);
472  Eigen::Vector2d xf(0, 3);
473 
474  Eigen::Vector2d u0(0, 0);
475 
476  int num_states = 4;
477  double dt = 0.1;
478 
479  grid.setHorizon(num_states, dt);
480  grid.initialize(x0, xf, u0);
481 
482  // set some values for u
483  grid.getShootingIntervalsRaw()[0].controls.front()->values()[0] = 1.5;
484  grid.getShootingIntervalsRaw()[1].controls.front()->values()[0] = 2.5;
485  grid.getShootingIntervalsRaw()[2].controls.front()->values()[0] = 3.5;
486 
487  // now shift grid by 1
488  EXPECT_TRUE(grid.shiftGridValues(1));
489 
490  // get grid values
491  corbo::TimeSeries::Ptr sn = std::make_shared<corbo::TimeSeries>();
492  grid.getShootingNodeTimeSeries(sn);
493  EXPECT_DOUBLE_EQ(sn->getValueDimension(), 2);
494  EXPECT_DOUBLE_EQ(sn->getTimeDimension(), 4);
495 
496  Eigen::MatrixXd s_seq = sn->getValuesMatrixView();
497 
498  EXPECT_DOUBLE_EQ(s_seq(0, 0), 2);
499  EXPECT_DOUBLE_EQ(s_seq(1, 0), 1);
500  EXPECT_DOUBLE_EQ(s_seq(0, 1), 1);
501  EXPECT_DOUBLE_EQ(s_seq(1, 1), 2);
502  EXPECT_DOUBLE_EQ(s_seq(0, 2), 0);
503  EXPECT_DOUBLE_EQ(s_seq(1, 2), 3);
504  EXPECT_DOUBLE_EQ(s_seq(0, 3), -1);
505  EXPECT_DOUBLE_EQ(s_seq(1, 3), 4);
506 
507  // controls
508  corbo::TimeSeries::Ptr un = std::make_shared<corbo::TimeSeries>();
509  grid.getControlInputTimeSeries(un);
510  EXPECT_DOUBLE_EQ(un->getValueDimension(), 2);
511  EXPECT_DOUBLE_EQ(un->getTimeDimension(), 4);
512 
513  Eigen::MatrixXd u_seq = un->getValuesMatrixView();
514  EXPECT_DOUBLE_EQ(u_seq(0, 0), 2.5);
515  EXPECT_DOUBLE_EQ(u_seq(1, 0), 0);
516  EXPECT_DOUBLE_EQ(u_seq(0, 1), 3.5);
517  EXPECT_DOUBLE_EQ(u_seq(1, 1), 0);
518  EXPECT_DOUBLE_EQ(u_seq(0, 2), 3.5); // zero-order hold (extrapolation)
519  EXPECT_DOUBLE_EQ(u_seq(1, 2), 0);
520  // last control is duplicated in getControlInputTimeSeries()...
521 }
522 
523 TEST_F(TestFullDiscretizationGrid, grid_shift_trajectory2)
524 {
525  Eigen::Vector2d x0(3, 0);
526  Eigen::Vector2d xf(0, 3);
527 
528  Eigen::Vector2d u0(0, 0);
529 
530  int num_states = 4;
531  double dt = 0.1;
532 
533  grid.setHorizon(num_states, dt);
534  grid.initialize(x0, xf, u0);
535 
536  // set some values for u
537  grid.getShootingIntervalsRaw()[0].controls.front()->values()[0] = 1.5;
538  grid.getShootingIntervalsRaw()[1].controls.front()->values()[0] = 2.5;
539  grid.getShootingIntervalsRaw()[2].controls.front()->values()[0] = 3.5;
540 
541  // now shift grid by 1
542  EXPECT_TRUE(grid.shiftGridValues(2));
543 
544  // get grid values
545  corbo::TimeSeries::Ptr sn = std::make_shared<corbo::TimeSeries>();
546  grid.getShootingNodeTimeSeries(sn);
547  EXPECT_DOUBLE_EQ(sn->getValueDimension(), 2);
548  EXPECT_DOUBLE_EQ(sn->getTimeDimension(), 4);
549 
550  Eigen::MatrixXd s_seq = sn->getValuesMatrixView();
551 
552  EXPECT_DOUBLE_EQ(s_seq(0, 0), 1);
553  EXPECT_DOUBLE_EQ(s_seq(1, 0), 2);
554  EXPECT_DOUBLE_EQ(s_seq(0, 1), 0);
555  EXPECT_DOUBLE_EQ(s_seq(1, 1), 3);
556  EXPECT_DOUBLE_EQ(s_seq(0, 2), -1);
557  EXPECT_DOUBLE_EQ(s_seq(1, 2), 4);
558  EXPECT_DOUBLE_EQ(s_seq(0, 3), -2);
559  EXPECT_DOUBLE_EQ(s_seq(1, 3), 5);
560 
561  // controls
562  corbo::TimeSeries::Ptr un = std::make_shared<corbo::TimeSeries>();
563  grid.getControlInputTimeSeries(un);
564  EXPECT_DOUBLE_EQ(un->getValueDimension(), 2);
565  EXPECT_DOUBLE_EQ(un->getTimeDimension(), 4);
566 
567  Eigen::MatrixXd u_seq = un->getValuesMatrixView();
568  EXPECT_DOUBLE_EQ(u_seq(0, 0), 3.5);
569  EXPECT_DOUBLE_EQ(u_seq(1, 0), 0);
570  EXPECT_DOUBLE_EQ(u_seq(0, 1), 3.5);
571  EXPECT_DOUBLE_EQ(u_seq(1, 1), 0);
572  EXPECT_DOUBLE_EQ(u_seq(0, 2), 3.5); // zero-order hold (extrapolation)
573  EXPECT_DOUBLE_EQ(u_seq(1, 2), 0);
574  // last control is duplicated in getControlInputTimeSeries()...
575 }
576 
577 TEST_F(TestFullDiscretizationGrid, grid_shift_trajectory_x0)
578 {
579  Eigen::Vector2d x0(3, 0);
580  Eigen::Vector2d xf(0, 3);
581 
582  Eigen::Vector2d u0(0, 0);
583 
584  int num_states = 4;
585  double dt = 0.1;
586 
587  grid.setHorizon(num_states, dt);
588  grid.initialize(x0, xf, u0);
589 
590  // set some values for u
591  grid.getShootingIntervalsRaw()[0].controls.front()->values()[0] = 1.5;
592  grid.getShootingIntervalsRaw()[1].controls.front()->values()[0] = 2.5;
593  grid.getShootingIntervalsRaw()[2].controls.front()->values()[0] = 3.5;
594 
595  // now shift grid by 1
596  EXPECT_TRUE(grid.shiftGridValues(Eigen::Vector2d(2.1, 0.9)));
597 
598  // get grid values
599  corbo::TimeSeries::Ptr sn = std::make_shared<corbo::TimeSeries>();
600  grid.getShootingNodeTimeSeries(sn);
601  EXPECT_DOUBLE_EQ(sn->getValueDimension(), 2);
602  EXPECT_DOUBLE_EQ(sn->getTimeDimension(), 4);
603 
604  Eigen::MatrixXd s_seq = sn->getValuesMatrixView();
605 
606  EXPECT_DOUBLE_EQ(s_seq(0, 0), 2);
607  EXPECT_DOUBLE_EQ(s_seq(1, 0), 1);
608  EXPECT_DOUBLE_EQ(s_seq(0, 1), 1);
609  EXPECT_DOUBLE_EQ(s_seq(1, 1), 2);
610  EXPECT_DOUBLE_EQ(s_seq(0, 2), 0);
611  EXPECT_DOUBLE_EQ(s_seq(1, 2), 3);
612  EXPECT_DOUBLE_EQ(s_seq(0, 3), -1);
613  EXPECT_DOUBLE_EQ(s_seq(1, 3), 4);
614 
615  // controls
616  corbo::TimeSeries::Ptr un = std::make_shared<corbo::TimeSeries>();
617  grid.getControlInputTimeSeries(un);
618  EXPECT_DOUBLE_EQ(un->getValueDimension(), 2);
619  EXPECT_DOUBLE_EQ(un->getTimeDimension(), 4);
620 
621  Eigen::MatrixXd u_seq = un->getValuesMatrixView();
622  EXPECT_DOUBLE_EQ(u_seq(0, 0), 2.5);
623  EXPECT_DOUBLE_EQ(u_seq(1, 0), 0);
624  EXPECT_DOUBLE_EQ(u_seq(0, 1), 3.5);
625  EXPECT_DOUBLE_EQ(u_seq(1, 1), 0);
626  EXPECT_DOUBLE_EQ(u_seq(0, 2), 3.5); // zero-order hold (extrapolation)
627  EXPECT_DOUBLE_EQ(u_seq(1, 2), 0);
628  // last control is duplicated in getControlInputTimeSeries()...
629 }
630 
631 // TEST_F(TestFullDiscretizationGrid, initialize_grid)
632 //{
633 // Eigen::Vector2d x0(0, 1);
634 // Eigen::VectorXd u_ref(1);
635 // u_ref[0] = -1;
636 
637 // StaticReference xref(Eigen::Vector2d(6, 4));
638 // StaticReference uref(u_ref);
639 
640 // corbo::IntegratorSystem::Ptr system = std::make_shared<corbo::IntegratorSystem>(2);
641 
642 // grid.setDt(0.1);
643 // grid.setNumStates(4);
644 
645 // grid.initialize(x0, xref, uref, system);
646 
647 // // check state inputs
648 // EXPECT_EQ(grid.getStateSequence().size(), 3);
649 // EXPECT_DOUBLE_EQ(grid.getStateSequence()[0].values()[0], 0.0);
650 // EXPECT_DOUBLE_EQ(grid.getStateSequence()[0].values()[1], 1.0);
651 // EXPECT_DOUBLE_EQ(grid.getStateSequence()[1].values()[0], 2.0);
652 // EXPECT_DOUBLE_EQ(grid.getStateSequence()[1].values()[1], 2.0);
653 // EXPECT_DOUBLE_EQ(grid.getStateSequence()[2].values()[0], 4.0);
654 // EXPECT_DOUBLE_EQ(grid.getStateSequence()[2].values()[1], 3.0);
655 // EXPECT_DOUBLE_EQ(grid.getFinalState().values()[0], 6.0);
656 // EXPECT_DOUBLE_EQ(grid.getFinalState().values()[1], 4.0);
657 
658 // EXPECT_EQ(grid.getStateSequence().front().getDimensionUnfixed(), 0);
659 // EXPECT_EQ(grid.getFinalState().getDimensionUnfixed(), 2);
660 
661 // // check control inputs
662 // EXPECT_EQ(grid.getControlSequence().size(), 3);
663 // for (const VectorVertex& vertex : grid.getControlSequence())
664 // {
665 // EXPECT_DOUBLE_EQ(vertex.values()[0], -1);
666 // }
667 //}
TestFullDiscretizationGrid::TestFullDiscretizationGrid
TestFullDiscretizationGrid()
Definition: test_full_discretization_grid.cpp:48
macros.h
corbo::StaticReference
Static reference trajectory.
Definition: reference_trajectory.h:134
console.h
TestFullDiscretizationGrid::SetUp
void SetUp() override
Definition: test_full_discretization_grid.cpp:56
value_comparison.h
TEST_F
TEST_F(TestFullDiscretizationGrid, initialize_grid_start_state_only)
Definition: test_full_discretization_grid.cpp:64
TestFullDiscretizationGrid::~TestFullDiscretizationGrid
virtual ~TestFullDiscretizationGrid()
Definition: test_full_discretization_grid.cpp:50
int
return int(ret)+1
TestFullDiscretizationGrid::grid
FullDiscretizationGrid grid
Definition: test_full_discretization_grid.cpp:61
corbo::ZeroReference
Zero reference trajectory.
Definition: reference_trajectory.h:184
TestFullDiscretizationGrid
Definition: test_full_discretization_grid.cpp:44
reference_trajectory.h
corbo::VectorVertex
Vertex implementation that stores an Eigen::VectorXd (dynamic dimension)
Definition: vector_vertex.h:73
corbo::TimeSeries::Ptr
std::shared_ptr< TimeSeries > Ptr
Definition: time_series.h:108

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