test_trajectory_sim_grade.cpp
Go to the documentation of this file.
1 
2 #include "gtest/gtest.h"
3 #include <array>
4 #include <boost/concept_check.hpp>
5 #include <tuw_control/trajectory_simulator/trajectory_simulator_precalc.h>
6 #include <tuw_control/trajectory_simulator/trajectory_simulator_online.h>
7 #include <tuw_control/trajectory_evaluator/trajectory_sim_grade.h>
8 #include <tuw_control/costs_evaluator/cost_functions.hpp>
9 
10 #include <tuw_control/state/state_sim_template.hpp>
11 #include <tuw_control/param_func/param_func_spline/param_func_spline0_dist.h>
12 
13 using namespace tuw;
14 using namespace std;
15 
16 namespace tuw
17 {
18 namespace cost_functions
19 {
20 template <typename MapData>
21 class CFLatMap1Weight<TrajectorySimulator::LatticeVec, MapData>
22  : public CFLatMap1WeightBase<TrajectorySimulator::LatticeVec, MapData>
23 {
24 public:
25  CFLatMap1Weight()
26  {
27  this->arcAcc = [this](const size_t& _i)
28  {
29  return this->latticePtr_->at(_i).arc;
30  };
31  }
32 };
33 }
34 }
35 
36 namespace
37 {
38 class StateSimTest : public StateSimTemplate<7, 3>
39 {
40 public:
41  enum class StateVars
42  {
43  X,
44  Y,
45  THETA,
46  V,
47  W,
48  T,
49  S
50  };
51 
52 public:
53  enum class StateNmVars
54  {
55  X,
56  Y,
57  THETA
58  };
59 
60 public:
61  enum class StateCfVars
62  {
63  V,
64  W,
65  T,
66  S
67  };
68 
69 public:
70  enum class ParamFuncVars
71  {
72  V,
73  W
74  };
75 
76 public:
77  enum class ParamFuncLattices
78  {
79  T
80  };
81 
82 public:
83  StateSimTest()
84  {
85  }
86 
87 public:
88  StateSimUPtr cloneStateSim() const override
89  {
90  return StateSimUPtr(new StateSimTest(*this)); /*::make_unique< StateSimTest >(*this);*/
91  }
92 
93 public:
94  double stateArc() const override
95  {
96  return stateCf_.value(asInt(StateCfVars::T));
97  }
98 
99 public:
100  double stateDist() const override
101  {
102  return stateCf_.value(asInt(StateCfVars::S));
103  }
104 
105 public:
106  ParamFuncs* paramFuncs() override
107  {
108  return &paramFuncs_;
109  }
110 
111 public:
112  ParamFuncsDist* paramFuncsDist() override
113  {
114  return &paramFuncs_;
115  }
116 
117 private:
118  void setStateCf(const double& _arc, const ParamFuncs::EvalArcGuarantee& _evalArcGuarantee =
119  ParamFuncs::EvalArcGuarantee::AFTER_LAST) override
120  {
121  paramFuncs_.setEvalArc(_arc, _evalArcGuarantee);
122  stateCf_.value(asInt(StateCfVars::T)) = _arc;
123  stateCf_.value(asInt(StateCfVars::V)) = paramFuncs_.computeFuncVal(asInt(ParamFuncVars::V));
124  stateCf_.value(asInt(StateCfVars::W)) = paramFuncs_.computeFuncVal(asInt(ParamFuncVars::W));
125  stateCf_.value(asInt(StateCfVars::S)) = paramFuncs_.computeS();
126  }
127 
128 private:
129  State& stateNmDot() override
130  {
131  stateNmDotCache_.value(asInt(StateNmVars::X)) =
132  stateCf_.value(asInt(StateCfVars::V)) * cos(stateNm_.value(asInt(StateNmVars::THETA)));
133  stateNmDotCache_.value(asInt(StateNmVars::Y)) =
134  stateCf_.value(asInt(StateCfVars::V)) * sin(stateNm_.value(asInt(StateNmVars::THETA)));
135  stateNmDotCache_.value(asInt(StateNmVars::THETA)) = stateCf_.value(asInt(StateCfVars::W));
136  return stateNmDotCache_;
137  }
138 
139 private:
140  State& stateNmDelta(const double& _dArc) override
141  {
142  const double& v = stateCf_.value(asInt(StateCfVars::V));
143  const double& w = stateCf_.value(asInt(StateCfVars::W));
144  const double& theta = stateNm_.value(asInt(StateNmVars::THETA));
145  double& dx = stateNmDotCache_.value(asInt(StateNmVars::X));
146  double& dy = stateNmDotCache_.value(asInt(StateNmVars::Y));
147  double& da = stateNmDotCache_.value(asInt(StateNmVars::THETA));
148 
149  if (fabs(w) > FLT_MIN)
150  {
151  const double r = v / w;
152  dx = (-r * sin(theta) + r * sin(theta + w * _dArc));
153  dy = (+r * cos(theta) - r * cos(theta + w * _dArc));
154  da = w * _dArc;
155  }
156  else
157  {
158  dx = v * _dArc * cos(theta);
159  dy = v * _dArc * sin(theta);
160  da = 0.0;
161  }
162  return stateNmDotCache_;
163  }
164 
165 private:
166  ParamFuncsSpline0Dist paramFuncs_;
167 };
168 
169 using SNF = StateSimTest::StateVars;
170 using SNV = StateSimTest::StateNmVars;
171 using SCF = StateSimTest::StateCfVars;
172 using PFV = StateSimTest::ParamFuncVars;
173 
174 // void coutCostEvalCosts(TrajectorySimGradeSPtr& trajSimGrade) {
175 // cout<<"Cost Functions:"<<endl;
176 // cout<<"f = "<<trajSimGrade->trajSim()->costsEvaluator_->f<<endl<<"h = ";
177 // for(size_t i = 0; i < trajSimGrade->trajSim()->costsEvaluator_->h.size(); ++i) {
178 // cout<<trajSimGrade->trajSim()->costsEvaluator_->h[i]<<", "; } cout<<endl;cout<<endl;
179 // }
180 
181 // The fixture for testing class Foo.
182 class TrajSimGradeTest : public ::testing::Test
183 {
184 protected:
185  // You can remove any or all of the following functions if its body
186  // is empty.
187 
188  TrajSimGradeTest()
189  {
190  dummyMapData = shared_ptr<double>(new double(0));
191 
192  // initialize parametric functions structure
193  using PFS = ParamFuncs::ParamFuncsStructure;
194  using PfCpD = ParamFuncs::CtrlPtDim;
195  using FeM = ParamFuncs::FuncEvalMode; // using EaG = ParamFuncs::EvalArcGuarantee;
196  size_t funcIdx = 0;
197  vector<PFS> pf(2, PFS());
198  static constexpr const int paramFuncArcRefIdxAll = 0;
199  pf[0].ctrlPtsSize = 3;
200  pf[0].ctrlPtsArcRefIdx = paramFuncArcRefIdxAll;
201  pf[1].ctrlPtsSize = 3;
202  pf[1].ctrlPtsArcRefIdx = paramFuncArcRefIdxAll;
203  pf[0].evalReq[asInt(FeM::INT1)] = false;
204  pf[0].evalReq[asInt(FeM::INT2)] = false;
205 
206  // construct stateSim object and initalize the parametric functions object
207  stateSimPtr = std::shared_ptr<StateSimTest>(new StateSimTest);
208  stateSimPtr->paramFuncs()->init(pf);
209  stateSimPtr->paramFuncsDist()->setDistCfMode(ParamFuncsDist::TraveledDistCfMode::V, vector<size_t>(1, 0));
210 
211  // set the parametric functions control points
212  ParamFuncs* funcs = stateSimPtr->paramFuncs();
213  double initT = 0;
214  funcIdx = 0;
215  funcs->ctrlPtVal(funcIdx, 0, PfCpD::ARC) = 0 + initT;
216  funcs->ctrlPtVal(funcIdx, 1, PfCpD::ARC) = 0.5 + initT;
217  funcs->ctrlPtVal(funcIdx, 2, PfCpD::ARC) = 1 + initT;
218 
219  funcIdx = 0;
220  funcs->ctrlPtVal(funcIdx, 0, PfCpD::VAL) = 1.;
221  funcs->ctrlPtVal(funcIdx, 1, PfCpD::VAL) = 1.;
222  funcs->ctrlPtVal(funcIdx, 2, PfCpD::VAL) = 1.;
223 
224  funcIdx = 1;
225  funcs->ctrlPtVal(funcIdx, 0, PfCpD::VAL) = 3.5 * M_PI;
226  funcs->ctrlPtVal(funcIdx, 1, PfCpD::VAL) = 1.5 * M_PI;
227  funcs->ctrlPtVal(funcIdx, 2, PfCpD::VAL) = -4.5 * M_PI;
228  funcs->precompute();
229  }
230  virtual ~TrajSimGradeTest()
231  {
232  }
233  // If the constructor and destructor are not enough for setting up
234  // and cleaning up each test, you can define the following methods:
235  virtual void SetUp()
236  {
237  Test::SetUp();
238  // Code here will be called immediately after the constructor (right
239  // before each test).
240  }
241  virtual void TearDown()
242  {
243  Test::TearDown();
244  // Code here will be called immediately after each test (right
245  // before the destructor).
246  }
247  StateSimPtr stateSimPtr;
248  TrajectorySimGradeSPtr trajSimGrade;
249  shared_ptr<double> dummyMapData;
250 
251 public:
252  template <typename CostEvalType>
253  void multiCheckCostFunc(const double& _dt, const double& _ds, std::function<double(double, double)> costExpFunc)
254  {
255  trajSimGrade = shared_ptr<TrajectorySimGrade>(new TrajectorySimGrade(stateSimPtr, std::make_unique<CostEvalType>(dummyMapData)));
256  trajSimGrade->trajSim()->stateSim()->setDiscrType(RungeKutta::DiscretizationType::HEUN);
257  trajSimGrade->setSimMode(TrajectorySimulator::SimMode::PRECALC);
258 
259  trajSimGrade->trajSim()->dtBase() = _dt;
260  trajSimGrade->evaluateTrajectory();
261 
262  double sumEndAns, sumEndExp;
263  sumEndAns = trajSimGrade->trajSim()->costsEvaluator_->f;
264  sumEndExp = costExpFunc(_dt, _ds);
265  EXPECT_DOUBLE_EQ(sumEndExp, sumEndAns);
266 
267  trajSimGrade->setSimMode(TrajectorySimulator::SimMode::ONLINE);
268  trajSimGrade->trajSim()->dtBase() = _dt;
269  trajSimGrade->evaluateTrajectory();
270 
271  sumEndAns = trajSimGrade->trajSim()->costsEvaluator_->f;
272  sumEndExp = costExpFunc(_dt, _ds);
273  EXPECT_DOUBLE_EQ(sumEndExp, sumEndAns);
274 
275  trajSimGrade->trajSim()->dtBase() = _dt;
276  trajSimGrade->trajSim()->dsBase() = _ds;
277  trajSimGrade->evaluateTrajectory();
278 
279  sumEndAns = trajSimGrade->trajSim()->costsEvaluator_->f;
280  sumEndExp = costExpFunc(_dt, _ds);
281  EXPECT_DOUBLE_EQ(sumEndExp, sumEndAns);
282 
283  trajSimGrade->setSimMode(TrajectorySimulator::SimMode::PRECALC);
284  trajSimGrade->trajSim()->dtBase() = _dt;
285  trajSimGrade->trajSim()->dsBase() = _ds;
286  trajSimGrade->evaluateTrajectory();
287 
288  sumEndAns = trajSimGrade->trajSim()->costsEvaluator_->f;
289  sumEndExp = costExpFunc(_dt, _ds);
290  EXPECT_DOUBLE_EQ(sumEndExp, sumEndAns);
291  }
292 
293  template <typename CostEvalType>
294  void multiCheckCostFuncArray(const double& _dt, const double& _ds,
295  function<shared_ptr<vector<double>>(double, double)> costExpFunc)
296  {
297  trajSimGrade = shared_ptr<TrajectorySimGrade>(new TrajectorySimGrade(stateSimPtr, std::make_unique<CostEvalType>(dummyMapData)));
298  trajSimGrade->trajSim()->stateSim()->setDiscrType(RungeKutta::DiscretizationType::HEUN);
299  trajSimGrade->setSimMode(TrajectorySimulator::SimMode::PRECALC);
300 
301  trajSimGrade->trajSim()->dtBase() = _dt;
302  trajSimGrade->evaluateTrajectory();
303 
304  shared_ptr<vector<double>> ans = shared_ptr<vector<double>>(new vector<double>);
305  shared_ptr<vector<double>> exp;
306  ans->resize(trajSimGrade->trajSim()->costsEvaluator_->h.size());
307  for (size_t i = 0; i < ans->size(); ++i)
308  {
309  ans->at(i) = trajSimGrade->trajSim()->costsEvaluator_->h[i];
310  }
311  exp = costExpFunc(_dt, _ds);
312  ASSERT_EQ(exp->size(), ans->size());
313  for (size_t i = 0; i < ans->size(); ++i)
314  {
315  EXPECT_DOUBLE_EQ(exp->at(i), ans->at(i));
316  }
317 
318  trajSimGrade->evaluateTrajectory(0.2);
319  ans->resize(trajSimGrade->trajSim()->costsEvaluator_->h.size());
320  for (size_t i = 0; i < ans->size(); ++i)
321  {
322  ans->at(i) = trajSimGrade->trajSim()->costsEvaluator_->h[i];
323  }
324  exp = costExpFunc(_dt, _ds);
325  ASSERT_EQ(exp->size(), ans->size());
326  for (size_t i = 0; i < ans->size(); ++i)
327  {
328  EXPECT_DOUBLE_EQ(exp->at(i), ans->at(i));
329  }
330 
331  trajSimGrade->setSimMode(TrajectorySimulator::SimMode::ONLINE);
332  trajSimGrade->trajSim()->dtBase() = _dt;
333  trajSimGrade->evaluateTrajectory();
334 
335  ans->resize(trajSimGrade->trajSim()->costsEvaluator_->h.size());
336  for (size_t i = 0; i < ans->size(); ++i)
337  {
338  ans->at(i) = trajSimGrade->trajSim()->costsEvaluator_->h[i];
339  }
340  exp = costExpFunc(_dt, _ds);
341  ASSERT_EQ(exp->size(), ans->size());
342  for (size_t i = 0; i < ans->size(); ++i)
343  {
344  EXPECT_DOUBLE_EQ(exp->at(i), ans->at(i));
345  }
346 
347  trajSimGrade->trajSim()->dtBase() = _dt;
348  trajSimGrade->trajSim()->dsBase() = _ds;
349  trajSimGrade->evaluateTrajectory();
350 
351  ans->resize(trajSimGrade->trajSim()->costsEvaluator_->h.size());
352  for (size_t i = 0; i < ans->size(); ++i)
353  {
354  ans->at(i) = trajSimGrade->trajSim()->costsEvaluator_->h[i];
355  }
356  exp = costExpFunc(_dt, _ds);
357  ASSERT_EQ(exp->size(), ans->size());
358  for (size_t i = 0; i < ans->size(); ++i)
359  {
360  EXPECT_DOUBLE_EQ(exp->at(i), ans->at(i));
361  }
362 
363  trajSimGrade->setSimMode(TrajectorySimulator::SimMode::PRECALC);
364  trajSimGrade->trajSim()->dtBase() = _dt;
365  trajSimGrade->trajSim()->dsBase() = _ds;
366  trajSimGrade->evaluateTrajectory();
367 
368  ans->resize(trajSimGrade->trajSim()->costsEvaluator_->h.size());
369  for (size_t i = 0; i < ans->size(); ++i)
370  {
371  ans->at(i) = trajSimGrade->trajSim()->costsEvaluator_->h[i];
372  }
373  exp = costExpFunc(_dt, _ds);
374  ASSERT_EQ(exp->size(), ans->size());
375  for (size_t i = 0; i < ans->size(); ++i)
376  {
377  EXPECT_DOUBLE_EQ(exp->at(i), ans->at(i));
378  }
379  }
380 
381  template <typename CostEvalType>
382  void multiCheckCostFuncArrayKnot(const double& _dt, const double& _ds, bool checkEqCostsNr)
383  {
384  trajSimGrade = shared_ptr<TrajectorySimGrade>(new TrajectorySimGrade(stateSimPtr, std::make_unique<CostEvalType>(dummyMapData)));
385  trajSimGrade->trajSim()->stateSim()->setDiscrType(RungeKutta::DiscretizationType::HEUN);
386 
387  ParamFuncs* funcs = trajSimGrade->trajSim()->stateSim()->paramFuncs();
388  vector<vector<double*>> funcKnotsLattice(1, vector<double*>(funcs->funcsArcSize(0), 0));
389  for (size_t i = 0; i < funcKnotsLattice[0].size(); ++i)
390  {
391  funcKnotsLattice[0][i] = &funcs->funcsArc(0, i);
392  }
393  trajSimGrade->trajSim()->setUserDefLattice(funcKnotsLattice);
394 
395  trajSimGrade->setSimMode(TrajectorySimulator::SimMode::PRECALC);
396 
397  trajSimGrade->trajSim()->dtBase() = _dt;
398  trajSimGrade->evaluateTrajectory();
399  trajSimGrade->trajSim()->dsBase() = -1;
400  shared_ptr<vector<double>> expPreEq = shared_ptr<vector<double>>(new vector<double>);
401  expPreEq->resize(trajSimGrade->trajSim()->costsEvaluator_->h.size());
402  for (size_t i = 0; i < expPreEq->size(); ++i)
403  {
404  expPreEq->at(i) = trajSimGrade->trajSim()->costsEvaluator_->h[i];
405  }
406  // coutCostEvalCosts(trajSimGrade);
407 
408  trajSimGrade->trajSim()->dtBase() = _dt / 10.;
409  trajSimGrade->trajSim()->dsBase() = _ds;
410  trajSimGrade->evaluateTrajectory();
411  shared_ptr<vector<double>> expPreSm = shared_ptr<vector<double>>(new vector<double>);
412  expPreSm->resize(trajSimGrade->trajSim()->costsEvaluator_->h.size());
413  for (size_t i = 0; i < expPreSm->size(); ++i)
414  {
415  expPreSm->at(i) = trajSimGrade->trajSim()->costsEvaluator_->h[i];
416  }
417  // coutCostEvalCosts(trajSimGrade);
418 
419  trajSimGrade->trajSim()->dtBase() = _dt * 10.;
420  trajSimGrade->trajSim()->dsBase() = -1;
421  trajSimGrade->evaluateTrajectory();
422  shared_ptr<vector<double>> expPreGr = shared_ptr<vector<double>>(new vector<double>);
423  expPreGr->resize(trajSimGrade->trajSim()->costsEvaluator_->h.size());
424  for (size_t i = 0; i < expPreGr->size(); ++i)
425  {
426  expPreGr->at(i) = trajSimGrade->trajSim()->costsEvaluator_->h[i];
427  }
428  // coutCostEvalCosts(trajSimGrade);
429 
431  trajSimGrade->setSimMode(TrajectorySimulator::SimMode::ONLINE);
432  shared_ptr<vector<double>> ansOnline = shared_ptr<vector<double>>(new vector<double>);
433 
434  for (size_t i = 0; i < funcKnotsLattice[0].size(); ++i)
435  {
436  funcKnotsLattice[0][i] = &funcs->funcsArc(0, i);
437  }
438  trajSimGrade->trajSim()->setUserDefLattice(funcKnotsLattice);
439  trajSimGrade->trajSim()->dtBase() = _dt;
440  trajSimGrade->trajSim()->dsBase() = -1;
441  trajSimGrade->evaluateTrajectory();
442  // coutCostEvalCosts(trajSimGrade);
443  ansOnline->resize(trajSimGrade->trajSim()->costsEvaluator_->h.size());
444  for (size_t i = 0; i < ansOnline->size(); ++i)
445  {
446  ansOnline->at(i) = trajSimGrade->trajSim()->costsEvaluator_->h[i];
447  }
448  if (checkEqCostsNr)
449  {
450  ASSERT_EQ(expPreEq->size(), ansOnline->size());
451  }
452  for (size_t i = 0; i < ansOnline->size(); ++i)
453  {
454  EXPECT_TRUE(ansOnline->at(i) >= 0);
455  EXPECT_DOUBLE_EQ(expPreEq->at(i), ansOnline->at(i));
456  }
457 
458  for (size_t i = 0; i < funcKnotsLattice[0].size(); ++i)
459  {
460  funcKnotsLattice[0][i] = &funcs->funcsArc(0, i);
461  }
462  trajSimGrade->trajSim()->setUserDefLattice(funcKnotsLattice);
463  trajSimGrade->trajSim()->dtBase() = _dt / 10.;
464  trajSimGrade->trajSim()->dsBase() = _ds;
465  trajSimGrade->evaluateTrajectory();
466  // coutCostEvalCosts(trajSimGrade);
467  ansOnline->resize(trajSimGrade->trajSim()->costsEvaluator_->h.size());
468  for (size_t i = 0; i < ansOnline->size(); ++i)
469  {
470  ansOnline->at(i) = trajSimGrade->trajSim()->costsEvaluator_->h[i];
471  }
472  if (checkEqCostsNr)
473  {
474  ASSERT_EQ(expPreSm->size(), ansOnline->size());
475  }
476  for (size_t i = 0; i < ansOnline->size(); ++i)
477  {
478  EXPECT_TRUE(ansOnline->at(i) >= 0);
479  EXPECT_DOUBLE_EQ(expPreSm->at(i), ansOnline->at(i));
480  }
481 
482  for (size_t i = 0; i < funcKnotsLattice[0].size(); ++i)
483  {
484  funcKnotsLattice[0][i] = &funcs->funcsArc(0, i);
485  }
486  trajSimGrade->trajSim()->setUserDefLattice(funcKnotsLattice);
487  trajSimGrade->trajSim()->dtBase() = _dt * 10;
488  trajSimGrade->trajSim()->dsBase() = -1;
489  trajSimGrade->evaluateTrajectory();
490  // coutCostEvalCosts(trajSimGrade);
491  ansOnline->resize(trajSimGrade->trajSim()->costsEvaluator_->h.size());
492  for (size_t i = 0; i < ansOnline->size(); ++i)
493  {
494  ansOnline->at(i) = trajSimGrade->trajSim()->costsEvaluator_->h[i];
495  }
496  if (checkEqCostsNr)
497  {
498  ASSERT_EQ(expPreGr->size(), ansOnline->size());
499  }
500  for (size_t i = 0; i < ansOnline->size(); ++i)
501  {
502  EXPECT_TRUE(ansOnline->at(i) >= 0);
503  EXPECT_DOUBLE_EQ(expPreGr->at(i), ansOnline->at(i));
504  }
505 
506  for (size_t i = 0; i < funcKnotsLattice[0].size(); ++i)
507  {
508  funcKnotsLattice[0][i] = &funcs->funcsArc(0, i);
509  }
510  trajSimGrade->trajSim()->setUserDefLattice(funcKnotsLattice);
511  trajSimGrade->trajSim()->dtBase() = _dt / 10.;
512  trajSimGrade->trajSim()->dsBase() = _ds;
513  trajSimGrade->evaluateTrajectory();
514  // coutCostEvalCosts(trajSimGrade);
515  ansOnline->resize(trajSimGrade->trajSim()->costsEvaluator_->h.size());
516  for (size_t i = 0; i < ansOnline->size(); ++i)
517  {
518  ansOnline->at(i) = trajSimGrade->trajSim()->costsEvaluator_->h[i];
519  }
520  if (checkEqCostsNr)
521  {
522  ASSERT_EQ(expPreEq->size(), ansOnline->size());
523  }
524  for (size_t i = 0; i < ansOnline->size(); ++i)
525  {
526  EXPECT_TRUE(ansOnline->at(i) >= 0);
527  EXPECT_DOUBLE_EQ(expPreSm->at(i), ansOnline->at(i));
528  }
529  }
530 
531  template <typename CostEvalType>
532  void multiCheckCostFuncArrayKnotOnlyDt(const double& _dt)
533  {
534  trajSimGrade = shared_ptr<TrajectorySimGrade>(new TrajectorySimGrade(stateSimPtr, std::make_unique<CostEvalType>(dummyMapData)));
535  trajSimGrade->trajSim()->stateSim()->setDiscrType(RungeKutta::DiscretizationType::HEUN);
536  trajSimGrade->setSimMode(TrajectorySimulator::SimMode::PRECALC);
537 
538  trajSimGrade->trajSim()->dtBase() = _dt;
539  trajSimGrade->evaluateTrajectory();
540  shared_ptr<vector<double>> expPreEq = shared_ptr<vector<double>>(new vector<double>);
541  expPreEq->resize(trajSimGrade->trajSim()->costsEvaluator_->h.size());
542  for (size_t i = 0; i < expPreEq->size(); ++i)
543  {
544  expPreEq->at(i) = trajSimGrade->trajSim()->costsEvaluator_->h[i];
545  }
546  // coutCostEvalCosts(trajSimGrade);
547 
548  trajSimGrade->trajSim()->dtBase() = _dt / 10.;
549  trajSimGrade->evaluateTrajectory();
550  shared_ptr<vector<double>> expPreSm = shared_ptr<vector<double>>(new vector<double>);
551  expPreSm->resize(trajSimGrade->trajSim()->costsEvaluator_->h.size());
552  for (size_t i = 0; i < expPreSm->size(); ++i)
553  {
554  expPreSm->at(i) = trajSimGrade->trajSim()->costsEvaluator_->h[i];
555  }
556  // coutCostEvalCosts(trajSimGrade);
557 
558  trajSimGrade->trajSim()->dtBase() = _dt * 10.;
559  trajSimGrade->evaluateTrajectory();
560  shared_ptr<vector<double>> expPreGr = shared_ptr<vector<double>>(new vector<double>);
561  expPreGr->resize(trajSimGrade->trajSim()->costsEvaluator_->h.size());
562  for (size_t i = 0; i < expPreGr->size(); ++i)
563  {
564  expPreGr->at(i) = trajSimGrade->trajSim()->costsEvaluator_->h[i];
565  }
566  // coutCostEvalCosts(trajSimGrade);
567 
569  trajSimGrade->setSimMode(TrajectorySimulator::SimMode::ONLINE);
570  shared_ptr<vector<double>> ansOnline = shared_ptr<vector<double>>(new vector<double>);
571 
572  trajSimGrade->trajSim()->dtBase() = _dt;
573  trajSimGrade->evaluateTrajectory();
574  // coutCostEvalCosts(trajSimGrade);
575  ansOnline->resize(trajSimGrade->trajSim()->costsEvaluator_->h.size());
576  for (size_t i = 0; i < ansOnline->size(); ++i)
577  {
578  ansOnline->at(i) = trajSimGrade->trajSim()->costsEvaluator_->h[i];
579  }
580  for (size_t i = 0; i < ansOnline->size(); ++i)
581  {
582  EXPECT_TRUE(ansOnline->at(i) >= 0);
583  EXPECT_DOUBLE_EQ(expPreEq->at(i), ansOnline->at(i));
584  }
585 
586  trajSimGrade->trajSim()->dtBase() = _dt / 10.;
587  trajSimGrade->evaluateTrajectory();
588  // coutCostEvalCosts(trajSimGrade);
589  ansOnline->resize(trajSimGrade->trajSim()->costsEvaluator_->h.size());
590  for (size_t i = 0; i < ansOnline->size(); ++i)
591  {
592  ansOnline->at(i) = trajSimGrade->trajSim()->costsEvaluator_->h[i];
593  }
594  for (size_t i = 0; i < ansOnline->size(); ++i)
595  {
596  EXPECT_TRUE(ansOnline->at(i) >= 0);
597  EXPECT_DOUBLE_EQ(expPreSm->at(i), ansOnline->at(i));
598  }
599 
600  trajSimGrade->trajSim()->dtBase() = _dt * 10;
601  trajSimGrade->evaluateTrajectory();
602  // coutCostEvalCosts(trajSimGrade);
603  ansOnline->resize(trajSimGrade->trajSim()->costsEvaluator_->h.size());
604  for (size_t i = 0; i < ansOnline->size(); ++i)
605  {
606  ansOnline->at(i) = trajSimGrade->trajSim()->costsEvaluator_->h[i];
607  }
608  for (size_t i = 0; i < ansOnline->size(); ++i)
609  {
610  EXPECT_TRUE(ansOnline->at(i) >= 0);
611  EXPECT_DOUBLE_EQ(expPreGr->at(i), ansOnline->at(i));
612  }
613 
614  trajSimGrade->trajSim()->dtBase() = _dt / 10.;
615  trajSimGrade->evaluateTrajectory();
616  // coutCostEvalCosts(trajSimGrade);
617  ansOnline->resize(trajSimGrade->trajSim()->costsEvaluator_->h.size());
618  for (size_t i = 0; i < ansOnline->size(); ++i)
619  {
620  ansOnline->at(i) = trajSimGrade->trajSim()->costsEvaluator_->h[i];
621  }
622  for (size_t i = 0; i < ansOnline->size(); ++i)
623  {
624  EXPECT_TRUE(ansOnline->at(i) >= 0);
625  EXPECT_DOUBLE_EQ(expPreSm->at(i), ansOnline->at(i));
626  }
627  }
628 };
629 
631 
632 using TS = TrajectorySimulator;
633 using TSBSLT = TS::BaseSimLatticeType;
634 using TSLatVec = TS::LatticeVec;
635 
636 using MapData = double;
637 using namespace tuw::cost_functions;
638 
640 class TestCost_LinSumW : public CFLatMap1Weight<TSLatVec, MapData>
641 {
642 public:
643  TestCost_LinSumW()
644  {
645  stAcc = [this](const size_t& _i)
646  {
647  return latticePtr_->at(_i).statePtr->value(asInt(SNF::T));
648  };
649  funcPredef_FL1::lin(this);
650  funcPredef_FL2::sum(this);
651  funcPredef_FL3::weight(this);
652  weight_ = 1;
653  cost0_ = 0;
654  }
655 };
656 
658 class TestCost_LinSumWNorm : public CFLatMap1Weight<TSLatVec, MapData>
659 {
660 public:
661  TestCost_LinSumWNorm()
662  {
663  stAcc = [this](const size_t& _i)
664  {
665  return latticePtr_->at(_i).statePtr->value(asInt(SNF::T));
666  };
667  funcPredef_FL1::lin(this);
668  funcPredef_FL2::sum(this);
669  funcPredef_FL3::weightNorm(this);
670  weight_ = 1;
671  cost0_ = 0;
672  }
673 };
675 class TestCost_LinIntWNorm : public CFLatMap1Weight<TSLatVec, MapData>
676 {
677 public:
678  TestCost_LinIntWNorm()
679  {
680  stAcc = [this](const size_t& _i)
681  {
682  return latticePtr_->at(_i).statePtr->value(asInt(SNF::T));
683  };
684  funcPredef_FL1::lin(this);
685  funcPredef_FL2::intTrap(this);
686  funcPredef_FL3::weightNorm(this);
687  weight_ = 1;
688  cost0_ = 0;
689  }
690 };
692 
693 class Test1CostArray_LinSumW : public CostsArrayLat<TSLatVec, MapData, TestCost_LinSumW>
694 {
695  size_t latFuncLayerIdx() override
696  {
697  return TS::lattTypeIdx(asInt(TSBSLT::LATTICE_ARC_EQ_DT));
698  }
699  size_t latKnotLayerIdx() override
700  {
701  return TS::lattTypeIdx(asInt(TSBSLT::ARC_BG_BK));
702  }
703 };
704 class Test1CostArray_LinSumWNorm : public CostsArrayLat<TSLatVec, MapData, TestCost_LinSumWNorm>
705 {
706  size_t latFuncLayerIdx() override
707  {
708  return TS::lattTypeIdx(asInt(TSBSLT::LATTICE_ARC_EQ_DT));
709  }
710  size_t latKnotLayerIdx() override
711  {
712  return TS::lattTypeIdx(asInt(TSBSLT::ARC_BG_BK));
713  }
714 };
715 class Test1CostArray_LinIntWNorm : public CostsArrayLat<TSLatVec, MapData, TestCost_LinIntWNorm>
716 {
717  size_t latFuncLayerIdx() override
718  {
719  return TS::lattTypeIdx(asInt(TSBSLT::LATTICE_ARC_EQ_DT));
720  }
721  size_t latKnotLayerIdx() override
722  {
723  return TS::lattTypeIdx(asInt(TSBSLT::ARC_BG_BK));
724  }
725 };
726 
727 class TestCostsEvaluatorT1C1 : public CostsEvaluator<TSLatVec, MapData>
728 {
729 public:
730  TestCostsEvaluatorT1C1(std::shared_ptr<MapData>& _mapDataPtr) : CostsEvaluator(_mapDataPtr)
731  {
732  }
733 
734 public:
735  void loadCostFunctions() override
736  {
737  partialCostsArray_[asInt(CostEvaluatorCostType::F)].resize(1);
738  partialCostsArray_[asInt(CostEvaluatorCostType::F)][0] = std::make_unique<Test1CostArray_LinSumW>();
739  }
740 };
741 class TestCostsEvaluatorT1C2 : public CostsEvaluator<TSLatVec, MapData>
742 {
743 public:
744  TestCostsEvaluatorT1C2(std::shared_ptr<MapData>& _mapDataPtr) : CostsEvaluator(_mapDataPtr)
745  {
746  }
747 
748 public:
749  void loadCostFunctions() override
750  {
751  partialCostsArray_[asInt(CostEvaluatorCostType::F)].resize(1);
752  partialCostsArray_[asInt(CostEvaluatorCostType::F)][0] = std::make_unique<Test1CostArray_LinSumWNorm>();
753  }
754 };
755 class TestCostsEvaluatorT1C3 : public CostsEvaluator<TSLatVec, MapData>
756 {
757 public:
758  TestCostsEvaluatorT1C3(std::shared_ptr<MapData>& _mapDataPtr) : CostsEvaluator(_mapDataPtr)
759  {
760  }
761 
762 public:
763  void loadCostFunctions() override
764  {
765  partialCostsArray_[asInt(CostEvaluatorCostType::F)].resize(1);
766  partialCostsArray_[asInt(CostEvaluatorCostType::F)][0] = std::make_unique<Test1CostArray_LinIntWNorm>();
767  }
768 };
769 
770 TEST_F(TrajSimGradeTest, CostFuncsOnTOnT)
771 {
772  double dt, ds;
773 
774  {
775  auto sumeEndExpFunc = [this](double dt, double ds)
776  {
777  double sumEndExp = 0;
778  size_t i = 1;
779  for (; i <= 1. / trajSimGrade->trajSim()->dt(); ++i)
780  {
781  sumEndExp += (double)i * dt;
782  } /*sumEndExp+=1- --i*dt;*/
783  return sumEndExp;
784  };
785 
786  dt = 0.01;
787  ds = 0.02;
788  multiCheckCostFunc<TestCostsEvaluatorT1C1>(dt, ds, sumeEndExpFunc);
789  dt = 1;
790  ds = 0.05;
791  multiCheckCostFunc<TestCostsEvaluatorT1C1>(dt, ds, sumeEndExpFunc);
792  dt = 1e-4;
793  ds = 0.02;
794  multiCheckCostFunc<TestCostsEvaluatorT1C1>(dt, ds, sumeEndExpFunc);
795  dt = 0.1;
796  ds = -1;
797  multiCheckCostFunc<TestCostsEvaluatorT1C1>(dt, ds, sumeEndExpFunc);
798  }
799 
800  {
801  auto sumeEndExpFunc = [this](double dt, double ds)
802  {
803  double sumEndExp = 0;
804  size_t i = 1;
805  for (; i <= 1. / trajSimGrade->trajSim()->dt(); ++i)
806  {
807  sumEndExp += (double)i * dt;
808  }
809  return sumEndExp / ((double)--i * dt);
810  };
811 
812  dt = 0.01;
813  ds = 0.02;
814  multiCheckCostFunc<TestCostsEvaluatorT1C2>(dt, ds, sumeEndExpFunc);
815  dt = 1;
816  ds = 0.05;
817  multiCheckCostFunc<TestCostsEvaluatorT1C2>(dt, ds, sumeEndExpFunc);
818  dt = 1e-4;
819  ds = 0.02;
820  multiCheckCostFunc<TestCostsEvaluatorT1C2>(dt, ds, sumeEndExpFunc);
821  dt = 0.1;
822  ds = -1;
823  multiCheckCostFunc<TestCostsEvaluatorT1C2>(dt, ds, sumeEndExpFunc);
824  }
825 
826  {
827  auto sumeEndExpFunc = [this](double dt, double ds)
828  {
829  double sumEndExp = 0;
830  size_t i = 1;
831  for (; i <= 1. / trajSimGrade->trajSim()->dt(); ++i)
832  {
833  sumEndExp += 0.5 * dt * ((i + i - 1) * dt);
834  }
835  return sumEndExp / ((double)--i * dt);
836  };
837 
838  dt = 0.01;
839  ds = 0.02;
840  multiCheckCostFunc<TestCostsEvaluatorT1C3>(dt, ds, sumeEndExpFunc);
841  dt = 1;
842  ds = 0.05;
843  multiCheckCostFunc<TestCostsEvaluatorT1C3>(dt, ds, sumeEndExpFunc);
844  dt = 1e-4;
845  ds = 0.02;
846  multiCheckCostFunc<TestCostsEvaluatorT1C3>(dt, ds, sumeEndExpFunc);
847  dt = 0.1;
848  ds = -1;
849  multiCheckCostFunc<TestCostsEvaluatorT1C3>(dt, ds, sumeEndExpFunc);
850  }
851 }
852 
853 class Test2CostArray_LinSumW : public CostsArrayLat<TSLatVec, MapData, TestCost_LinSumW>
854 {
855  size_t latFuncLayerIdx() override
856  {
857  return TS::lattTypeIdx(asInt(TSBSLT::LATTICE_ARC_EQ_DT));
858  }
859  size_t latKnotLayerIdx() override
860  {
861  return TS::lattTypeIdx(asInt(TSBSLT::LATTICE_ARC_EQ_DT));
862  }
863 };
864 class Test2CostArray_LinSumWNorm : public CostsArrayLat<TSLatVec, MapData, TestCost_LinSumWNorm>
865 {
866  size_t latFuncLayerIdx() override
867  {
868  return TS::lattTypeIdx(asInt(TSBSLT::LATTICE_ARC_EQ_DT));
869  }
870  size_t latKnotLayerIdx() override
871  {
872  return TS::lattTypeIdx(asInt(TSBSLT::LATTICE_ARC_EQ_DT));
873  }
874 };
875 class Test2CostArray_LinIntWNorm : public CostsArrayLat<TSLatVec, MapData, TestCost_LinIntWNorm>
876 {
877  size_t latFuncLayerIdx() override
878  {
879  return TS::lattTypeIdx(asInt(TSBSLT::LATTICE_ARC_EQ_DT));
880  }
881  size_t latKnotLayerIdx() override
882  {
883  return TS::lattTypeIdx(asInt(TSBSLT::LATTICE_ARC_EQ_DT));
884  }
885 };
886 
887 class TestCostsEvaluatorT2C1 : public CostsEvaluator<TSLatVec, MapData>
888 {
889 public:
890  TestCostsEvaluatorT2C1(std::shared_ptr<MapData>& _mapDataPtr) : CostsEvaluator(_mapDataPtr)
891  {
892  }
893 
894 public:
895  void loadCostFunctions() override
896  {
897  partialCostsArray_[asInt(CostEvaluatorCostType::H)].resize(1);
898  partialCostsArray_[asInt(CostEvaluatorCostType::H)][0] = std::make_unique<Test2CostArray_LinSumW>();
899  }
900 };
901 class TestCostsEvaluatorT2C2 : public CostsEvaluator<TSLatVec, MapData>
902 {
903 public:
904  TestCostsEvaluatorT2C2(std::shared_ptr<MapData>& _mapDataPtr) : CostsEvaluator(_mapDataPtr)
905  {
906  }
907 
908 public:
909  void loadCostFunctions() override
910  {
911  partialCostsArray_[asInt(CostEvaluatorCostType::H)].resize(1);
912  partialCostsArray_[asInt(CostEvaluatorCostType::H)][0] = std::make_unique<Test2CostArray_LinSumWNorm>();
913  }
914 };
915 class TestCostsEvaluatorT2C3 : public CostsEvaluator<TSLatVec, MapData>
916 {
917 public:
918  TestCostsEvaluatorT2C3(std::shared_ptr<MapData>& _mapDataPtr) : CostsEvaluator(_mapDataPtr)
919  {
920  }
921 
922 public:
923  void loadCostFunctions() override
924  {
925  partialCostsArray_[asInt(CostEvaluatorCostType::H)].resize(1);
926  partialCostsArray_[asInt(CostEvaluatorCostType::H)][0] = std::make_unique<Test2CostArray_LinIntWNorm>();
927  }
928 };
929 
930 TEST_F(TrajSimGradeTest, CostFuncsArrayOnTOnTLat)
931 {
932  double dt, ds;
933 
934  {
935  auto sumeEndExpFunc = [this](double dt, double ds)
936  {
937  shared_ptr<vector<double>> costsExp =
938  shared_ptr<vector<double>>(new vector<double>(trajSimGrade->trajSim()->stateSim()->paramFuncs()->funcsArcEnd() / (double)dt));
939  for (size_t i = 1; i <= 1. / trajSimGrade->trajSim()->dt(); ++i)
940  {
941  costsExp->at(i - 1) = (double)i * dt;
942  }
943  return costsExp;
944  };
945 
946  dt = 0.01;
947  ds = 0.02;
948  multiCheckCostFuncArray<TestCostsEvaluatorT2C1>(dt, ds, sumeEndExpFunc);
949  dt = 1;
950  ds = 0.05;
951  multiCheckCostFuncArray<TestCostsEvaluatorT2C1>(dt, ds, sumeEndExpFunc);
952  dt = 1e-3;
953  ds = 0.02;
954  multiCheckCostFuncArray<TestCostsEvaluatorT2C1>(dt, ds, sumeEndExpFunc);
955  dt = 0.1;
956  ds = -1;
957  multiCheckCostFuncArray<TestCostsEvaluatorT2C1>(dt, ds, sumeEndExpFunc);
958  }
959 
960  {
961  auto sumeEndExpFunc = [this](double dt, double ds)
962  {
963  shared_ptr<vector<double>> costsExp =
964  shared_ptr<vector<double>>(new vector<double>(trajSimGrade->trajSim()->stateSim()->paramFuncs()->funcsArcEnd() / (double)dt));
965  for (size_t i = 1; i <= 1. / trajSimGrade->trajSim()->dt(); ++i)
966  {
967  costsExp->at(i - 1) = (double)i * dt / (i * dt - (i - 1) * dt);
968  }
969  return costsExp;
970  };
971 
972  dt = 0.01;
973  ds = 0.02;
974  multiCheckCostFuncArray<TestCostsEvaluatorT2C2>(dt, ds, sumeEndExpFunc);
975  dt = 1;
976  ds = 0.05;
977  multiCheckCostFuncArray<TestCostsEvaluatorT2C2>(dt, ds, sumeEndExpFunc);
978  dt = 1e-3;
979  ds = 0.02;
980  multiCheckCostFuncArray<TestCostsEvaluatorT2C2>(dt, ds, sumeEndExpFunc);
981  dt = 0.1;
982  ds = -1;
983  multiCheckCostFuncArray<TestCostsEvaluatorT2C2>(dt, ds, sumeEndExpFunc);
984  }
985 
986  {
987  auto sumeEndExpFunc = [this](double dt, double ds)
988  {
989  shared_ptr<vector<double>> costsExp =
990  shared_ptr<vector<double>>(new vector<double>(trajSimGrade->trajSim()->stateSim()->paramFuncs()->funcsArcEnd() / (double)dt));
991  for (size_t i = 1; i <= 1. / trajSimGrade->trajSim()->dt(); ++i)
992  {
993  double ii = i;
994  costsExp->at(i - 1) = (ii * dt - (ii - 1) * dt) * (ii * dt + (ii - 1) * dt) / (ii * dt - (ii - 1) * dt) / 2.;
995  }
996  return costsExp;
997  };
998 
999  dt = 0.01;
1000  ds = 0.02;
1001  multiCheckCostFuncArray<TestCostsEvaluatorT2C3>(dt, ds, sumeEndExpFunc);
1002  dt = 1;
1003  ds = 0.05;
1004  multiCheckCostFuncArray<TestCostsEvaluatorT2C3>(dt, ds, sumeEndExpFunc);
1005  dt = 1e-3;
1006  ds = 0.02;
1007  multiCheckCostFuncArray<TestCostsEvaluatorT2C3>(dt, ds, sumeEndExpFunc);
1008  dt = 0.1;
1009  ds = -1;
1010  multiCheckCostFuncArray<TestCostsEvaluatorT2C3>(dt, ds, sumeEndExpFunc);
1011  }
1012 }
1013 
1014 class Test3CostArray_LinSumW : public Test2CostArray_LinSumW
1015 {
1016  size_t latKnotLayerIdx() override
1017  {
1018  return TS::lattTypeIdx(0);
1019  }
1020 };
1021 class Test3CostArray_LinSumWNorm : public Test2CostArray_LinSumWNorm
1022 {
1023  size_t latKnotLayerIdx() override
1024  {
1025  return TS::lattTypeIdx(0);
1026  }
1027 };
1028 class Test3CostArray_LinIntWNorm : public Test2CostArray_LinIntWNorm
1029 {
1030  size_t latKnotLayerIdx() override
1031  {
1032  return TS::lattTypeIdx(0);
1033  }
1034 };
1035 
1036 class TestCostsEvaluatorT3C1 : public CostsEvaluator<TSLatVec, MapData>
1037 {
1038 public:
1039  TestCostsEvaluatorT3C1(std::shared_ptr<MapData>& _mapDataPtr) : CostsEvaluator(_mapDataPtr)
1040  {
1041  }
1042 
1043 public:
1044  void loadCostFunctions() override
1045  {
1046  partialCostsArray_[asInt(CostEvaluatorCostType::H)].resize(1);
1047  partialCostsArray_[asInt(CostEvaluatorCostType::H)][0] = std::make_unique<Test3CostArray_LinSumW>();
1048  }
1049 };
1050 class TestCostsEvaluatorT3C2 : public CostsEvaluator<TSLatVec, MapData>
1051 {
1052 public:
1053  TestCostsEvaluatorT3C2(std::shared_ptr<MapData>& _mapDataPtr) : CostsEvaluator(_mapDataPtr)
1054  {
1055  }
1056 
1057 public:
1058  void loadCostFunctions() override
1059  {
1060  partialCostsArray_[asInt(CostEvaluatorCostType::H)].resize(1);
1061  partialCostsArray_[asInt(CostEvaluatorCostType::H)][0] = std::make_unique<Test3CostArray_LinSumWNorm>();
1062  }
1063 };
1064 class TestCostsEvaluatorT3C3 : public CostsEvaluator<TSLatVec, MapData>
1065 {
1066 public:
1067  TestCostsEvaluatorT3C3(std::shared_ptr<MapData>& _mapDataPtr) : CostsEvaluator(_mapDataPtr)
1068  {
1069  }
1070 
1071 public:
1072  void loadCostFunctions() override
1073  {
1074  partialCostsArray_[asInt(CostEvaluatorCostType::H)].resize(1);
1075  partialCostsArray_[asInt(CostEvaluatorCostType::H)][0] = std::make_unique<Test3CostArray_LinIntWNorm>();
1076  }
1077 };
1078 
1079 TEST_F(TrajSimGradeTest, CostFuncsArrayOnTOnTLatFuncKnots)
1080 {
1081  double dt, ds;
1082 
1083  dt = 0.01;
1084  ds = 0.02;
1085  multiCheckCostFuncArrayKnot<TestCostsEvaluatorT3C1>(dt, ds, true);
1086  dt = 0.1;
1087  ds = 0.05;
1088  multiCheckCostFuncArrayKnot<TestCostsEvaluatorT3C1>(dt, ds, true);
1089  dt = 0.001;
1090  ds = -1;
1091  multiCheckCostFuncArrayKnot<TestCostsEvaluatorT3C1>(dt, ds, true);
1092 
1093  dt = 0.01;
1094  ds = 0.02;
1095  multiCheckCostFuncArrayKnot<TestCostsEvaluatorT3C2>(dt, ds, true);
1096  dt = 0.1;
1097  ds = 0.05;
1098  multiCheckCostFuncArrayKnot<TestCostsEvaluatorT3C2>(dt, ds, true);
1099  dt = 0.001;
1100  ds = -1;
1101  multiCheckCostFuncArrayKnot<TestCostsEvaluatorT3C2>(dt, ds, true);
1102 
1103  dt = 0.01;
1104  ds = 0.02;
1105  multiCheckCostFuncArrayKnot<TestCostsEvaluatorT3C3>(dt, ds, true);
1106  dt = 0.1;
1107  ds = 0.05;
1108  multiCheckCostFuncArrayKnot<TestCostsEvaluatorT3C3>(dt, ds, true);
1109  dt = 0.001;
1110  ds = -1;
1111  multiCheckCostFuncArrayKnot<TestCostsEvaluatorT3C3>(dt, ds, true);
1112 }
1113 
1114 class TestCost_LinSumW_onX : public TestCost_LinSumW
1115 {
1116 public:
1117  TestCost_LinSumW_onX() : TestCost_LinSumW()
1118  {
1119  stAcc = [this](const size_t& _i)
1120  {
1121  return latticePtr_->at(_i).statePtr->value(asInt(SNF::X));
1122  };
1123  }
1124 };
1125 class TestCost_LinSumWNorm_onX : public TestCost_LinSumWNorm
1126 {
1127 public:
1128  TestCost_LinSumWNorm_onX() : TestCost_LinSumWNorm()
1129  {
1130  stAcc = [this](const size_t& _i)
1131  {
1132  return latticePtr_->at(_i).statePtr->value(asInt(SNF::X));
1133  };
1134  }
1135 };
1136 class TestCost_LinIntWNorm_onX : public TestCost_LinIntWNorm
1137 {
1138 public:
1139  TestCost_LinIntWNorm_onX() : TestCost_LinIntWNorm()
1140  {
1141  stAcc = [this](const size_t& _i)
1142  {
1143  return latticePtr_->at(_i).statePtr->value(asInt(SNF::X));
1144  };
1145  }
1146 };
1147 
1148 class Test4CostArray_LinSumW : public CostsArrayLat<TSLatVec, MapData, TestCost_LinSumW_onX>
1149 {
1150  size_t latFuncLayerIdx() override
1151  {
1152  return TS::lattTypeIdx(asInt(TSBSLT::LATTICE_ARC_EQ_DT));
1153  }
1154  size_t latKnotLayerIdx() override
1155  {
1156  return TS::lattTypeIdx(asInt(TSBSLT::LATTICE_ARC_EQ_DT));
1157  }
1158 };
1159 class Test4CostArray_LinSumWNorm : public CostsArrayLat<TSLatVec, MapData, TestCost_LinSumWNorm_onX>
1160 {
1161  size_t latFuncLayerIdx() override
1162  {
1163  return TS::lattTypeIdx(asInt(TSBSLT::LATTICE_ARC_EQ_DT));
1164  }
1165  size_t latKnotLayerIdx() override
1166  {
1167  return TS::lattTypeIdx(asInt(TSBSLT::LATTICE_ARC_EQ_DT));
1168  }
1169 };
1170 class Test4CostArray_LinIntWNorm : public CostsArrayLat<TSLatVec, MapData, TestCost_LinIntWNorm_onX>
1171 {
1172  size_t latFuncLayerIdx() override
1173  {
1174  return TS::lattTypeIdx(asInt(TSBSLT::LATTICE_ARC_EQ_DT));
1175  }
1176  size_t latKnotLayerIdx() override
1177  {
1178  return TS::lattTypeIdx(asInt(TSBSLT::LATTICE_ARC_EQ_DT));
1179  }
1180 };
1181 
1182 class TestCostsEvaluatorT4C1 : public CostsEvaluator<TSLatVec, MapData>
1183 {
1184 public:
1185  TestCostsEvaluatorT4C1(std::shared_ptr<MapData>& _mapDataPtr) : CostsEvaluator(_mapDataPtr)
1186  {
1187  }
1188 
1189 public:
1190  void loadCostFunctions() override
1191  {
1192  partialCostsArray_[asInt(CostEvaluatorCostType::H)].resize(1);
1193  partialCostsArray_[asInt(CostEvaluatorCostType::H)][0] = std::make_unique<Test4CostArray_LinSumW>();
1194  }
1195 };
1196 class TestCostsEvaluatorT4C2 : public CostsEvaluator<TSLatVec, MapData>
1197 {
1198 public:
1199  TestCostsEvaluatorT4C2(std::shared_ptr<MapData>& _mapDataPtr) : CostsEvaluator(_mapDataPtr)
1200  {
1201  }
1202 
1203 public:
1204  void loadCostFunctions() override
1205  {
1206  partialCostsArray_[asInt(CostEvaluatorCostType::H)].resize(1);
1207  partialCostsArray_[asInt(CostEvaluatorCostType::H)][0] = std::make_unique<Test4CostArray_LinSumWNorm>();
1208  }
1209 };
1210 class TestCostsEvaluatorT4C3 : public CostsEvaluator<TSLatVec, MapData>
1211 {
1212 public:
1213  TestCostsEvaluatorT4C3(std::shared_ptr<MapData>& _mapDataPtr) : CostsEvaluator(_mapDataPtr)
1214  {
1215  }
1216 
1217 public:
1218  void loadCostFunctions() override
1219  {
1220  partialCostsArray_[asInt(CostEvaluatorCostType::H)].resize(1);
1221  partialCostsArray_[asInt(CostEvaluatorCostType::H)][0] = std::make_unique<Test4CostArray_LinIntWNorm>();
1222  }
1223 };
1224 
1225 TEST_F(TrajSimGradeTest, CostFuncsArrayOnTOnTLatFuncKnotsOnlineInterrupt)
1226 {
1227  double dt, ds;
1228 
1229  dt = 0.01;
1230  ds = 0.02;
1231  multiCheckCostFuncArrayKnot<TestCostsEvaluatorT4C1>(dt, ds, false);
1232  dt = 0.1;
1233  ds = 0.05;
1234  multiCheckCostFuncArrayKnot<TestCostsEvaluatorT4C1>(dt, ds, false);
1235  dt = 0.001;
1236  ds = -1;
1237  multiCheckCostFuncArrayKnot<TestCostsEvaluatorT4C1>(dt, ds, false);
1238 
1239  dt = 0.01;
1240  ds = 0.02;
1241  multiCheckCostFuncArrayKnot<TestCostsEvaluatorT4C2>(dt, ds, false);
1242  dt = 0.1;
1243  ds = 0.05;
1244  multiCheckCostFuncArrayKnot<TestCostsEvaluatorT4C2>(dt, ds, false);
1245  dt = 0.001;
1246  ds = -1;
1247  multiCheckCostFuncArrayKnot<TestCostsEvaluatorT4C2>(dt, ds, false);
1248 
1249  dt = 0.01;
1250  ds = 0.02;
1251  multiCheckCostFuncArrayKnot<TestCostsEvaluatorT4C3>(dt, ds, false);
1252  dt = 0.1;
1253  ds = 0.05;
1254  multiCheckCostFuncArrayKnot<TestCostsEvaluatorT4C3>(dt, ds, false);
1255  dt = 0.001;
1256  ds = -1;
1257  multiCheckCostFuncArrayKnot<TestCostsEvaluatorT4C3>(dt, ds, false);
1258 }
1259 
1260 TEST_F(TrajSimGradeTest, CostFuncsArrayOnTOnTLatFuncKnotsOnlineInterruptOnlyT)
1261 {
1262  double dt;
1263 
1264  dt = 0.01;
1265  multiCheckCostFuncArrayKnotOnlyDt<TestCostsEvaluatorT4C1>(dt);
1266  dt = 0.1;
1267  multiCheckCostFuncArrayKnotOnlyDt<TestCostsEvaluatorT4C1>(dt);
1268 
1269  dt = 0.01;
1270  multiCheckCostFuncArrayKnotOnlyDt<TestCostsEvaluatorT4C2>(dt);
1271  dt = 0.1;
1272  multiCheckCostFuncArrayKnotOnlyDt<TestCostsEvaluatorT4C2>(dt);
1273 
1274  dt = 0.01;
1275  multiCheckCostFuncArrayKnotOnlyDt<TestCostsEvaluatorT4C3>(dt);
1276  dt = 0.1;
1277  multiCheckCostFuncArrayKnotOnlyDt<TestCostsEvaluatorT4C3>(dt);
1278 }
1279 
1280 class Test6CostArray_LinSumW : public CostsArrayLat<TSLatVec, MapData, TestCost_LinSumW_onX>
1281 {
1282  size_t latFuncLayerIdx() override
1283  {
1284  return TS::lattTypeIdx(asInt(TSBSLT::LATTICE_ARC_EQ_DT));
1285  }
1286  size_t latKnotLayerIdx() override
1287  {
1288  return TS::lattTypeIdx(asInt(TSBSLT::ARC_BG_BK));
1289  }
1290 };
1291 class Test6CostArray_LinSumWNorm : public CostsArrayLat<TSLatVec, MapData, TestCost_LinSumWNorm_onX>
1292 {
1293  size_t latFuncLayerIdx() override
1294  {
1295  return TS::lattTypeIdx(asInt(TSBSLT::LATTICE_ARC_EQ_DT));
1296  }
1297  size_t latKnotLayerIdx() override
1298  {
1299  return TS::lattTypeIdx(asInt(TSBSLT::ARC_BG_BK));
1300  }
1301 };
1302 class Test6CostArray_LinIntWNorm : public CostsArrayLat<TSLatVec, MapData, TestCost_LinIntWNorm_onX>
1303 {
1304  size_t latFuncLayerIdx() override
1305  {
1306  return TS::lattTypeIdx(asInt(TSBSLT::LATTICE_ARC_EQ_DT));
1307  }
1308  size_t latKnotLayerIdx() override
1309  {
1310  return TS::lattTypeIdx(asInt(TSBSLT::ARC_BG_BK));
1311  }
1312 };
1313 
1314 class TestCostsEvaluatorT6C1 : public CostsEvaluator<TSLatVec, MapData>
1315 {
1316 public:
1317  TestCostsEvaluatorT6C1(std::shared_ptr<MapData>& _mapDataPtr) : CostsEvaluator(_mapDataPtr)
1318  {
1319  }
1320 
1321 public:
1322  void loadCostFunctions() override
1323  {
1324  partialCostsArray_[asInt(CostEvaluatorCostType::H)].resize(1);
1325  partialCostsArray_[asInt(CostEvaluatorCostType::H)][0] = std::make_unique<Test6CostArray_LinSumW>();
1326  }
1327 };
1328 class TestCostsEvaluatorT6C2 : public CostsEvaluator<TSLatVec, MapData>
1329 {
1330 public:
1331  TestCostsEvaluatorT6C2(std::shared_ptr<MapData>& _mapDataPtr) : CostsEvaluator(_mapDataPtr)
1332  {
1333  }
1334 
1335 public:
1336  void loadCostFunctions() override
1337  {
1338  partialCostsArray_[asInt(CostEvaluatorCostType::H)].resize(1);
1339  partialCostsArray_[asInt(CostEvaluatorCostType::H)][0] = std::make_unique<Test6CostArray_LinSumWNorm>();
1340  }
1341 };
1342 class TestCostsEvaluatorT6C3 : public CostsEvaluator<TSLatVec, MapData>
1343 {
1344 public:
1345  TestCostsEvaluatorT6C3(std::shared_ptr<MapData>& _mapDataPtr) : CostsEvaluator(_mapDataPtr)
1346  {
1347  }
1348 
1349 public:
1350  void loadCostFunctions() override
1351  {
1352  partialCostsArray_[asInt(CostEvaluatorCostType::H)].resize(1);
1353  partialCostsArray_[asInt(CostEvaluatorCostType::H)][0] = std::make_unique<Test6CostArray_LinIntWNorm>();
1354  }
1355 };
1356 
1357 TEST_F(TrajSimGradeTest, CostFuncsArrayOnTOnTLatFuncKnotsOnlineInterruptOnlyTSmallerKnots)
1358 {
1359  double dt;
1360 
1361  dt = 0.1;
1362  multiCheckCostFuncArrayKnotOnlyDt<TestCostsEvaluatorT6C1>(dt);
1363  dt = 0.01;
1364  multiCheckCostFuncArrayKnotOnlyDt<TestCostsEvaluatorT6C1>(dt);
1365 
1366  dt = 0.01;
1367  multiCheckCostFuncArrayKnotOnlyDt<TestCostsEvaluatorT6C2>(dt);
1368  dt = 0.1;
1369  multiCheckCostFuncArrayKnotOnlyDt<TestCostsEvaluatorT6C2>(dt);
1370 
1371  dt = 0.01;
1372  multiCheckCostFuncArrayKnotOnlyDt<TestCostsEvaluatorT6C3>(dt);
1373  dt = 0.1;
1374  multiCheckCostFuncArrayKnotOnlyDt<TestCostsEvaluatorT6C3>(dt);
1375 }
1376 
1377 class Test7CostArray_LinSumW : public CostsArrayLat<TSLatVec, MapData, TestCost_LinSumW_onX>
1378 {
1379  size_t latFuncLayerIdx() override
1380  {
1381  return TS::lattTypeIdx(asInt(TSBSLT::ARC_BG_BK));
1382  }
1383  size_t latKnotLayerIdx() override
1384  {
1385  return TS::lattTypeIdx(asInt(TSBSLT::LATTICE_ARC_EQ_DT));
1386  }
1387 };
1388 class Test7CostArray_LinSumWNorm : public CostsArrayLat<TSLatVec, MapData, TestCost_LinSumWNorm_onX>
1389 {
1390  size_t latFuncLayerIdx() override
1391  {
1392  return TS::lattTypeIdx(asInt(TSBSLT::ARC_BG_BK));
1393  }
1394  size_t latKnotLayerIdx() override
1395  {
1396  return TS::lattTypeIdx(asInt(TSBSLT::LATTICE_ARC_EQ_DT));
1397  }
1398 };
1399 class Test7CostArray_LinIntWNorm : public CostsArrayLat<TSLatVec, MapData, TestCost_LinIntWNorm_onX>
1400 {
1401  size_t latFuncLayerIdx() override
1402  {
1403  return TS::lattTypeIdx(asInt(TSBSLT::ARC_BG_BK));
1404  }
1405  size_t latKnotLayerIdx() override
1406  {
1407  return TS::lattTypeIdx(asInt(TSBSLT::LATTICE_ARC_EQ_DT));
1408  }
1409 };
1410 
1411 class TestCostsEvaluatorT7C1 : public CostsEvaluator<TSLatVec, MapData>
1412 {
1413 public:
1414  TestCostsEvaluatorT7C1(std::shared_ptr<MapData>& _mapDataPtr) : CostsEvaluator(_mapDataPtr)
1415  {
1416  }
1417 
1418 public:
1419  void loadCostFunctions() override
1420  {
1421  partialCostsArray_[asInt(CostEvaluatorCostType::H)].resize(1);
1422  partialCostsArray_[asInt(CostEvaluatorCostType::H)][0] = std::make_unique<Test7CostArray_LinSumW>();
1423  }
1424 };
1425 class TestCostsEvaluatorT7C2 : public CostsEvaluator<TSLatVec, MapData>
1426 {
1427 public:
1428  TestCostsEvaluatorT7C2(std::shared_ptr<MapData>& _mapDataPtr) : CostsEvaluator(_mapDataPtr)
1429  {
1430  }
1431 
1432 public:
1433  void loadCostFunctions() override
1434  {
1435  partialCostsArray_[asInt(CostEvaluatorCostType::H)].resize(1);
1436  partialCostsArray_[asInt(CostEvaluatorCostType::H)][0] = std::make_unique<Test7CostArray_LinSumWNorm>();
1437  }
1438 };
1439 class TestCostsEvaluatorT7C3 : public CostsEvaluator<TSLatVec, MapData>
1440 {
1441 public:
1442  TestCostsEvaluatorT7C3(std::shared_ptr<MapData>& _mapDataPtr) : CostsEvaluator(_mapDataPtr)
1443  {
1444  }
1445 
1446 public:
1447  void loadCostFunctions() override
1448  {
1449  partialCostsArray_[asInt(CostEvaluatorCostType::H)].resize(1);
1450  partialCostsArray_[asInt(CostEvaluatorCostType::H)][0] = std::make_unique<Test7CostArray_LinIntWNorm>();
1451  }
1452 };
1453 
1454 TEST_F(TrajSimGradeTest, CostFuncsArrayOnTOnBELatPrecalc)
1455 {
1456  trajSimGrade = shared_ptr<TrajectorySimGrade>(new TrajectorySimGrade(stateSimPtr, std::make_unique<TestCostsEvaluatorT7C1>(dummyMapData)));
1457  trajSimGrade->trajSim()->stateSim()->setDiscrType(RungeKutta::DiscretizationType::HEUN);
1458  trajSimGrade->setSimMode(TrajectorySimulator::SimMode::PRECALC);
1459 
1460  trajSimGrade->trajSim()->dtBase() = 0.1;
1461  trajSimGrade->evaluateTrajectory();
1462  shared_ptr<vector<double>> expPreEq = shared_ptr<vector<double>>(new vector<double>);
1463  expPreEq->resize(trajSimGrade->trajSim()->costsEvaluator_->h.size());
1464  for (size_t i = 0; i < expPreEq->size(); ++i)
1465  {
1466  expPreEq->at(i) = trajSimGrade->trajSim()->costsEvaluator_->h[i];
1467  }
1468  // coutCostEvalCosts(trajSimGrade);
1469 }
1470 
1472 
1473 } // namespace
1474 
1475 int main(int argc, char** argv)
1476 {
1477  ::testing::InitGoogleTest(&argc, argv);
1478  return RUN_ALL_TESTS();
1479 }
tuw::ParamFuncs::funcsArc
double & funcsArc(const std::size_t &_i, const std::size_t &_j)
Access to the arc parameter vector at index _i, control point _j.
tuw::CostsEvaluatorBase::h
std::vector< double > h
Definition: costs_evaluator.hpp:170
tuw::CtrlPtDim::VAL
control point value
main
int main(int argc, char **argv)
Definition: test_trajectory_sim_grade.cpp:1475
tuw::ParamFuncs::EvalArcGuarantee::AFTER_LAST
previous evaluation arc <= this evaluation arc
tuw::cost_functions::CFLatMap1WeightBase
Definition: cost_functions.hpp:225
tuw::State::value
virtual double & value(const std::size_t &_i)=0
Access state variable based on index _i.
tuw::StateSimUPtr
std::unique_ptr< StateSim > StateSimUPtr
Definition: state_sim.h:56
tuw::CtrlPtDim::ARC
control point arc parameter
state_sim_template.hpp
std
Helper function needed to upgrade c++ 2011.
Definition: utils.h:193
tuw::asInt
constexpr auto asInt(Enumeration const value) -> typename std::underlying_type< Enumeration >::type
Definition: utils.h:87
tuw::cost_functions
Definition: cost_functions.hpp:47
trajectory_sim_grade.h
tuw::FuncEvalMode::INT1
function single integral
trajectory_simulator_precalc.h
tuw::StateSimTemplate
Templated partial implementation of StateSim.
Definition: state_sim_template.hpp:48
trajectory_simulator_online.h
tuw::ParamFuncs::EvalArcGuarantee
EvalArcGuarantee
Flags if any guarantees about evaluation arc relative to last evaluation arc are present.
Definition: param_func.h:80
tuw::TraveledDistCfMode::V
agent base center linear velocity is parametric function
tuw::ParamFuncs::ParamFuncsStructure
Containts parametric function initialization data.
Definition: param_func.h:100
tuw::State
Generic tree-like recursive structure that allows sub-structure access as well as ordered (array-like...
Definition: state.h:135
tuw::ParamFuncs::ctrlPtVal
double & ctrlPtVal(const std::size_t &_funcIdx, const std::size_t &_funcCtrlPtIdx, const CtrlPtDim &_ctrlPtDim=CtrlPtDim::VAL)
Access of a parametric function control point dimension.
tuw::TrajectorySimGradeSPtr
std::shared_ptr< TrajectorySimGrade > TrajectorySimGradeSPtr
Definition: trajectory_sim_grade.h:47
tuw::ParamFuncs::CtrlPtDim
CtrlPtDim
Control point variable dimension.
Definition: param_func.h:73
tuw::cost_functions::CostsArrayLat
Definition: cost_functions.hpp:616
param_func_spline0_dist.h
tuw::ParamFuncsDist
Extends manipulation of parametric functions collection with closed-form arc length (distance) comput...
Definition: param_func_dist.h:51
tuw::ParamFuncs::FuncEvalMode
FuncEvalMode
Required type of computation relative to the parametric function.
Definition: param_func.h:62
cost_functions.hpp
tuw::ParamFuncs::funcsArcSize
std::size_t funcsArcSize() const
Number of arc parametrizations.
Definition: param_func.cpp:192
tuw::cost_functions::CFLatMap1Weight
Definition: cost_functions.hpp:463
tuw::cost_functions::CFLatMap1Weight< TrajectorySimulator::LatticeVec, MapData >::CFLatMap1Weight
CFLatMap1Weight()
Definition: test_trajectory_sim_grade.cpp:25
tuw::StateSimPtr
std::shared_ptr< StateSim > StateSimPtr
Definition: discretization_runge_kutta_alias.hpp:44
tuw::ParamFuncs::precompute
virtual void precompute()=0
Precomputes cached data.
tuw::ParamFuncsDist::TraveledDistCfMode::V
agent base center linear velocity is parametric function
tuw::FuncEvalMode::INT2
function double integral
tuw::ParamFuncs::setEvalArc
virtual void setEvalArc(const double &_funcsArcEval, const EvalArcGuarantee &_evalArcGuarantee=EvalArcGuarantee::NONE)=0
Sets parametric function evaluation arc.
tuw::ParamFuncs
Storage and manipulation of parametric functions collection.
Definition: param_func.h:57

tuw_control
Author(s): George Todoran
autogenerated on Mon Jun 10 2019 15:27:22