test_trajectory_simulators.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 
8 #include <tuw_control/state/state_sim_template.hpp>
9 #include <tuw_control/param_func/param_func_spline/param_func_spline0_dist.h>
10 using namespace tuw;
11 using namespace std;
12 
13 namespace
14 {
15 class StateSimTest : public StateSimTemplate<7, 3>
16 {
17 public:
18  enum class StateVars
19  {
20  X,
21  Y,
22  THETA,
23  V,
24  W,
25  T,
26  S
27  };
28 
29 public:
30  enum class StateNmVars
31  {
32  X,
33  Y,
34  THETA
35  };
36 
37 public:
38  enum class StateCfVars
39  {
40  V,
41  W,
42  T,
43  S
44  };
45 
46 public:
47  enum class ParamFuncVars
48  {
49  V,
50  W
51  };
52 
53 public:
54  enum class ParamFuncLattices
55  {
56  T
57  };
58 
59 public:
60  StateSimTest()
61  {
62  }
63 
64 public:
65  StateSimUPtr cloneStateSim() const override
66  {
67  return StateSimUPtr(new StateSimTest(*this)); /*::make_unique< StateSimTest >(*this);*/
68  }
69 
70 public:
71  double stateArc() const override
72  {
73  return stateCf_.value(asInt(StateCfVars::T));
74  }
75 
76 public:
77  double stateDist() const override
78  {
79  return stateCf_.value(asInt(StateCfVars::S));
80  }
81 
82 public:
83  ParamFuncs* paramFuncs() override
84  {
85  return &paramFuncs_;
86  }
87 
88 public:
89  ParamFuncsDist* paramFuncsDist() override
90  {
91  return &paramFuncs_;
92  }
93 
94 private:
95  void setStateCf(const double& _arc, const ParamFuncs::EvalArcGuarantee& _evalArcGuarantee =
96  ParamFuncs::EvalArcGuarantee::AFTER_LAST) override
97  {
98  paramFuncs_.setEvalArc(_arc, _evalArcGuarantee);
99  stateCf_.value(asInt(StateCfVars::T)) = _arc;
100  stateCf_.value(asInt(StateCfVars::V)) = paramFuncs_.computeFuncVal(asInt(ParamFuncVars::V));
101  stateCf_.value(asInt(StateCfVars::W)) = paramFuncs_.computeFuncVal(asInt(ParamFuncVars::W));
102  stateCf_.value(asInt(StateCfVars::S)) = paramFuncs_.computeS();
103  }
104 
105 private:
106  State& stateNmDot() override
107  {
108  stateNmDotCache_.value(asInt(StateNmVars::X)) =
109  stateCf_.value(asInt(StateCfVars::V)) * cos(stateNm_.value(asInt(StateNmVars::THETA)));
110  stateNmDotCache_.value(asInt(StateNmVars::Y)) =
111  stateCf_.value(asInt(StateCfVars::V)) * sin(stateNm_.value(asInt(StateNmVars::THETA)));
112  stateNmDotCache_.value(asInt(StateNmVars::THETA)) = stateCf_.value(asInt(StateCfVars::W));
113  return stateNmDotCache_;
114  }
115 
116 private:
117  State& stateNmDelta(const double& _dArc) override
118  {
119  const double& v = stateCf_.value(asInt(StateCfVars::V));
120  const double& w = stateCf_.value(asInt(StateCfVars::W));
121  const double& theta = stateNm_.value(asInt(StateNmVars::THETA));
122  double& dx = stateNmDotCache_.value(asInt(StateNmVars::X));
123  double& dy = stateNmDotCache_.value(asInt(StateNmVars::Y));
124  double& da = stateNmDotCache_.value(asInt(StateNmVars::THETA));
125 
126  if (fabs(w) > FLT_MIN)
127  {
128  const double r = v / w;
129  dx = (-r * sin(theta) + r * sin(theta + w * _dArc));
130  dy = (+r * cos(theta) - r * cos(theta + w * _dArc));
131  da = w * _dArc;
132  }
133  else
134  {
135  dx = v * _dArc * cos(theta);
136  dy = v * _dArc * sin(theta);
137  da = 0.0;
138  }
139  return stateNmDotCache_;
140  }
141 
142 private:
143  ParamFuncsSpline0Dist paramFuncs_;
144 };
145 
146 using SNV = StateSimTest::StateNmVars;
147 using SCF = StateSimTest::StateCfVars;
148 using PFV = StateSimTest::ParamFuncVars;
149 
150 using PFS = ParamFuncs::ParamFuncsStructure;
151 using PfCpD = ParamFuncs::CtrlPtDim;
152 using PFFeM = ParamFuncs::FuncEvalMode;
153 using EaG = ParamFuncs::EvalArcGuarantee;
154 
156 // namespace cost_functions {
157 //
158 // template<typename MapData>
159 // class CFLatMap1Weight<TrajectorySimulator::LatticeVec, MapData> : public
160 // CFLatMap1WeightBase<TrajectorySimulator::LatticeVec, MapData> {
161 // public : CFLatMap1Weight() { this->arcAcc = [this](const size_t& _i){ return this->latticePtr_->at(_i).arc; };
162 // }
163 // };
164 //
165 // }
166 //
167 // class DiffDriveCostV : public cost_functions::CFLatMap1Weight<TrajectorySimulator::LatticeVec, double> {
168 // public : DiffDriveCostV() {
169 // stAcc = [this](const size_t& _i){ return latticePtr_->at(_i).statePtr->state(
170 // asInt(StateSimDiffDrive::StateVars::V) ); };
171 // cost_functions::funcPredef_FL1::lin (this);
172 // cost_functions::funcPredef_FL2::intTrap (this);
173 // cost_functions::funcPredef_FL3::weightNorm(this);
174 // weight_ = 1;
175 // cost0_ = 0;
176 // }
177 // public : void reset () override {
178 // setBoundIdxBegin( 0 );
179 // double boundArc = FLT_MAX; if (latticePtr_->size() > 0 ){ boundArc = latticePtr_->back().arc; }
180 // setBoundArcEnd(boundArc);
181 // CFLatMap1Weight<TrajectorySimulator::LatticeVec, double>::reset();
182 // }
183 // public : size_t latFuncLayerIdx() override { return 2; }
184 // };
185 //
186 //
187 // class DiffDriveCostConstrV : public cost_functions::CfSum<TrajectorySimulator::LatticeVec, double> {
188 // public : DiffDriveCostConstrV() {
189 // stAcc = [this](const size_t& _i){ return latticePtr_->at(_i).statePtr->state( asInt(StateSimDiffDrive::StateVars::S)
190 // ); };
191 // cost_functions::funcPredef_FL2::intTrap(this);
192 // cost_functions::funcPredef_FL3::weight (this);
193 // weight_ = 1;
194 // cost0_ = 0;
195 // }
196 // public : size_t latFuncLayerIdx() override { return 2; }
197 // };
198 // class DiffDriveConstVOnKnots : public cost_functions::CostsArrayOnLatKnotVec
199 // <TrajectorySimulator::LatticeVec, double, DiffDriveCostConstrV > {
200 // public : DiffDriveConstVOnKnots() { cost0_ = 0; }
201 // public : size_t latKnotLayerIdx() override { return 2; }
202 // };
203 //
204 // class DiffDriveCostConstrVMin : public cost_functions::CfSum<TrajectorySimulator::LatticeVec, double> {
205 // public : DiffDriveCostConstrVMin() {
206 // stAcc = [this](const size_t& _i){ return latticePtr_->at(_i).statePtr->state( asInt(StateSimDiffDrive::StateVars::S)
207 // ); };
208 // cost_functions::funcPredef_FL2::min (this);
209 // cost_functions::funcPredef_FL3::weightNorm(this);
210 // weight_ = 1;
211 // cost0_ = 0;
212 // }
213 // public : size_t latFuncLayerIdx() override { return 2; }
214 // };
215 // class DiffDriveConstVBetweenKnots : public
216 // cost_functions::CostsArrayBetweenLatKnotVec<TrajectorySimulator::LatticeVec, double, DiffDriveCostConstrVMin> {
217 // public : DiffDriveConstVBetweenKnots() { cost0_ = FLT_MAX; }
218 // public : size_t latKnotLayerIdx() override { return 2; }
219 // };
220 
221 // class DiffDriveCostsEvaluator : public CostsEvaluator<TrajectorySimulator::LatticeVec, double> {
222 // public : DiffDriveCostsEvaluator(std::shared_ptr<double>& _mapDataPtr) : CostsEvaluator(_mapDataPtr) {}
223 // public : void setCosts() override {
224 // partialCosts_ [asInt(CostEvaluatorCostType::F)].resize(1);
225 // partialCosts_ [asInt(CostEvaluatorCostType::F)][0] = make_unique<DiffDriveCostV>();
226 //
227 // partialCostsArray_[asInt(CostEvaluatorCostType::F)].resize(1);
228 // partialCostsArray_[asInt(CostEvaluatorCostType::F)][0] = make_unique<DiffDriveConstVOnKnots>();
229 //
230 //
231 // partialCosts_ [asInt(CostEvaluatorCostType::H)].resize(2);
232 // partialCosts_ [asInt(CostEvaluatorCostType::H)][0] = make_unique<DiffDriveCostV>();
233 // partialCosts_ [asInt(CostEvaluatorCostType::H)][1] = make_unique<DiffDriveCostV>();
234 //
235 // partialCostsArray_[asInt(CostEvaluatorCostType::H)].resize(2);
236 // partialCostsArray_[asInt(CostEvaluatorCostType::H)][0] = make_unique<DiffDriveConstVBetweenKnots>();
237 // partialCostsArray_[asInt(CostEvaluatorCostType::H)][1] = make_unique<DiffDriveConstVOnKnots >();
238 // }
239 // };
240 //
241 // class DiffDriveTrajectoryOptimizer : public TrajectoryOptimizer {
242 //
243 // using SVR = StateSimDiffDrive::StateVars;
244 // using PFV = StateSimDiffDrive::ParamFuncVars;
245 // using PFL = StateSimDiffDrive::ParamFuncLattices;
246 //
247 // public : DiffDriveTrajectoryOptimizer ( StateSimPtr& _stateSim, std::unique_ptr<CostsEvaluatorClass>
248 // _costsEvaluator ) : TrajectoryOptimizer(_stateSim, std::move(_costsEvaluator) ) {}
249 // public : void setVars() override {
250 // varsIdx_[asInt(AccessType::STATE_0 )] = {asInt(SVR::X), asInt(SVR::Y), asInt(SVR::THETA)};
251 // varsIdx_[asInt(AccessType::PARAM_CP )] = {asInt(PFV::V), asInt(PFV::W)};
252 // varsIdx_[asInt(AccessType::PARAM_ARC )] = {asInt(PFL::T)};
253 // varsIdx_[asInt(AccessType::PARAM_CP_END )] = {asInt(PFV::V), asInt(PFV::W)};
254 // varsIdx_[asInt(AccessType::PARAM_ARC_END)] = {asInt(PFL::T)};
255 // }
256 // };
257 //
258 
259 // The fixture for testing class Foo.
260 class TrajectorySimulatorsTest : public ::testing::Test
261 {
262 protected:
263  // You can remove any or all of the following functions if its body
264  // is empty.
265 
266  TrajectorySimulatorsTest()
267  {
268  // construct stateSim object and initalize the parametric functions object
269  sp = std::shared_ptr<StateSimTest>(new StateSimTest);
270  sp->setDiscrType(RungeKutta::DiscretizationType::EULER);
271 
272  vector<PFS> pf(2, PFS());
273  static constexpr const int paramFuncArcRefIdxAll = 0;
274  pf[asInt(PFV::V)].ctrlPtsSize = 3;
275  pf[asInt(PFV::V)].ctrlPtsArcRefIdx = paramFuncArcRefIdxAll;
276  pf[asInt(PFV::W)].ctrlPtsSize = 3;
277  pf[asInt(PFV::W)].ctrlPtsArcRefIdx = paramFuncArcRefIdxAll;
278 
279  pf[asInt(PFV::V)].evalReq[asInt(PFFeM::INT1)] = false;
280  pf[asInt(PFV::V)].evalReq[asInt(PFFeM::INT2)] = false;
281  pf[asInt(PFV::W)].evalReq[asInt(PFFeM::INT1)] = false;
282  pf[asInt(PFV::W)].evalReq[asInt(PFFeM::INT2)] = false;
283  vector<size_t> idxClosedFormV(1, asInt(PFV::V));
284 
285  sp->paramFuncs()->init(pf);
286  sp->paramFuncsDist()->setDistCfMode(ParamFuncsDist::TraveledDistCfMode::V, idxClosedFormV);
287 
288  // construct trajectory simulator object
289  trajSim = std::shared_ptr<TrajectorySimulatorPrecalc>(new TrajectorySimulatorPrecalc(sp));
290  trajSimOnline = std::shared_ptr<TrajectorySimulatorOnline>(new TrajectorySimulatorOnline(sp));
291  }
292 
293  virtual ~TrajectorySimulatorsTest()
294  {
295  }
296 
297  // If the constructor and destructor are not enough for setting up
298  // and cleaning up each test, you can define the following methods:
299 
300  virtual void SetUp()
301  {
302  Test::SetUp();
303  // Code here will be called immediately after the constructor (right
304  // before each test).
305  }
306 
307  virtual void TearDown()
308  {
309  Test::TearDown();
310  // Code here will be called immediately after each test (right
311  // before the destructor).
312  }
313  StateSimPtr sp;
314  TrajectorySimulatorSPtr trajSim;
315  TrajectorySimulatorSPtr trajSimOnline;
316 
317 public:
318  bool checkSimLatticeConsistency(vector<TrajectorySimulator::LatticePointType>& _lattice)
319  {
320  bool consistent = true;
321  // for ( size_t i = 0; i < _lattice.size(); ++i ) {
322  // for ( size_t j = 0; j < _lattice[i].statePtr->stateSize(); ++j ) {
323  // cout<<_lattice[i].statePtr->value(j)<<", "; }cout<<endl;
324  // }cout<<endl;
325  for (size_t i = 1; i < trajSim->simulationLattice_.size(); ++i)
326  {
327  if (!(trajSim->simulationLattice_[i - 1].arc <= trajSim->simulationLattice_[i].arc))
328  {
329  consistent = false;
330  break;
331  }
332  }
333  return consistent;
334  }
335  size_t sumAllPartStates(TrajectorySimulatorSPtr _tsp)
336  {
337  size_t nrPartStates = 0;
338  for (size_t i = 0; i < _tsp->partLattices_.size(); ++i)
339  {
340  nrPartStates += _tsp->partLattices_[i]->size();
341  }
342  return nrPartStates;
343  }
344  void checkSimLatticeStatesEqual(vector<TrajectorySimulator::LatticePointType>& _latticeRef,
345  vector<TrajectorySimulator::LatticePointType>& _latticeTest)
346  {
347  // for ( size_t i = 0; i < _latticeTest.size(); ++i ) {
348  // for ( size_t j = 0; j < _latticeTest[i].statePtr->stateSize(); ++j ) {
349  // cout<<_latticeRef[i].statePtr->value(j)<<":"<<_latticeTest[i].statePtr->value(j)<<", "; }cout<<endl;
350  // }cout<<endl;
351  ASSERT_EQ(_latticeRef.size(), _latticeTest.size());
352  for (size_t i = 0; i < _latticeTest.size(); ++i)
353  {
354  for (size_t j = 0; j < _latticeTest[i].statePtr->valueSize(); ++j)
355  {
356  EXPECT_DOUBLE_EQ(_latticeRef[i].statePtr->value(j), _latticeTest[i].statePtr->value(j));
357  }
358  }
359  }
360  void checkPartLatticeStatesEqual(TrajectorySimulator::LatticeVecSPtrVec& _partLatRef,
361  TrajectorySimulator::LatticeVecSPtrVec& _partLatTest)
362  {
363  ASSERT_EQ(_partLatRef.size(), _partLatTest.size());
364  for (size_t i = 0; i < _partLatRef.size(); ++i)
365  {
366  ASSERT_EQ(_partLatRef[i]->size(), _partLatTest[i]->size());
367  for (size_t j = 0; j < _partLatRef[i]->size(); ++j)
368  {
369  EXPECT_DOUBLE_EQ(_partLatRef[i]->at(j).arc, _partLatTest[i]->at(j).arc);
370  for (size_t k = 0; k < _partLatRef[i]->at(j).statePtr->valueSize(); ++k)
371  {
372  EXPECT_DOUBLE_EQ(_partLatRef[i]->at(j).statePtr->value(k), _partLatTest[i]->at(j).statePtr->value(k));
373  }
374  }
375  }
376  }
377  size_t expectedDtStates(const double _DeltaT, const double _dt)
378  {
379  return (ceil(_DeltaT / _dt) + 1);
380  }
381 };
382 
384 
385 TEST_F(TrajectorySimulatorsTest, SimPrecalcFrom0Dt)
386 {
387  ParamFuncs* funcs = trajSim->stateSim()->paramFuncs();
388  // set the parametric functions control points values
389  double initT = 0;
390  size_t funcIdx = 0;
391  funcs->ctrlPtVal(funcIdx, 0, PfCpD::ARC) = 0 + initT;
392  funcs->ctrlPtVal(funcIdx, 1, PfCpD::ARC) = 0.5 + initT;
393  funcs->ctrlPtVal(funcIdx, 2, PfCpD::ARC) = 1 + initT;
394 
395  funcIdx = 0;
396  funcs->ctrlPtVal(funcIdx, 0, PfCpD::VAL) = 3.0 * M_PI;
397  funcs->ctrlPtVal(funcIdx, 1, PfCpD::VAL) = -0.5 * M_PI;
398  funcs->ctrlPtVal(funcIdx, 2, PfCpD::VAL) = 2.0 * M_PI;
399 
400  funcIdx = 1;
401  funcs->ctrlPtVal(funcIdx, 0, PfCpD::VAL) = 3.5 * M_PI;
402  funcs->ctrlPtVal(funcIdx, 1, PfCpD::VAL) = 1.5 * M_PI;
403  funcs->ctrlPtVal(funcIdx, 2, PfCpD::VAL) = -4.5 * M_PI;
404 
405  funcs->precompute();
406 
407  // set lattice types
408  trajSim->dsBase() = -1; // only dt lattice types
409 
410  size_t statesBeginEnd = 2, statesDt, statesDs, statesLattice0;
411  double sumStates = 0;
412  size_t statesSimExp;
413 
414  trajSim->dtBase() = 0.1;
415  trajSim->simulateTrajectory(0);
416  statesDt = expectedDtStates(funcs->funcsArcEnd(), trajSim->dt());
417  statesDs = 0;
418  statesLattice0 = 0;
419  sumStates = statesBeginEnd + statesDt + statesDs + statesLattice0;
420  EXPECT_EQ(sumStates, sumAllPartStates(trajSim));
421  statesSimExp = statesDt;
422  EXPECT_EQ(statesSimExp, trajSim->simLattice().size());
423  checkSimLatticeConsistency(trajSim->simulationLattice_);
424 
425  trajSim->dtBase() = 0.12;
426  trajSim->simulateTrajectory(0);
427  statesDt = expectedDtStates(funcs->funcsArcEnd(), trajSim->dt());
428  statesDs = 0;
429  statesLattice0 = 0;
430  sumStates = statesBeginEnd + statesDt + statesDs + statesLattice0;
431  EXPECT_EQ(sumStates, sumAllPartStates(trajSim));
432  statesSimExp = statesDt;
433  EXPECT_EQ(statesSimExp, trajSim->simLattice().size());
434  checkSimLatticeConsistency(trajSim->simulationLattice_);
435 
436  trajSim->dtBase() = 0.25;
437  trajSim->simulateTrajectory(0);
438  statesDt = expectedDtStates(funcs->funcsArcEnd(), trajSim->dt());
439  statesDs = 0;
440  statesLattice0 = 0;
441  sumStates = statesBeginEnd + statesDt + statesDs + statesLattice0;
442  EXPECT_EQ(sumStates, sumAllPartStates(trajSim));
443  statesSimExp = statesDt;
444  EXPECT_EQ(statesSimExp, trajSim->simLattice().size());
445  checkSimLatticeConsistency(trajSim->simulationLattice_);
446 }
447 
448 TEST_F(TrajectorySimulatorsTest, SimPrecalcFrom0DtDsCtrlPt)
449 {
450  ParamFuncs* funcs = trajSim->stateSim()->paramFuncs();
451  // set the parametric functions control points values
452  double initT = 0;
453  size_t funcIdx = 0;
454  funcs->ctrlPtVal(funcIdx, 0, PfCpD::ARC) = 0 + initT;
455  funcs->ctrlPtVal(funcIdx, 1, PfCpD::ARC) = 0.557 + initT;
456  funcs->ctrlPtVal(funcIdx, 2, PfCpD::ARC) = 1 + initT;
457 
458  funcIdx = 0;
459  funcs->ctrlPtVal(funcIdx, 0, PfCpD::VAL) = 1.0;
460  funcs->ctrlPtVal(funcIdx, 1, PfCpD::VAL) = 1.0;
461  funcs->ctrlPtVal(funcIdx, 2, PfCpD::VAL) = 1.0;
462 
463  funcIdx = 1;
464  funcs->ctrlPtVal(funcIdx, 0, PfCpD::VAL) = 3.5 * M_PI;
465  funcs->ctrlPtVal(funcIdx, 1, PfCpD::VAL) = 1.5 * M_PI;
466  funcs->ctrlPtVal(funcIdx, 2, PfCpD::VAL) = -4.5 * M_PI;
467 
468  funcs->precompute();
469 
470  // equal dt, equal arc dist and control ponints arcs lattices
471  vector<vector<double*> > funcKnotsLattice(1, vector<double*>(funcs->funcsArcSize(0), 0));
472  for (size_t i = 0; i < funcKnotsLattice[0].size(); ++i)
473  {
474  funcKnotsLattice[0][i] = &funcs->funcsArc(0, i);
475  }
476  trajSim->setUserDefLattice(funcKnotsLattice);
477  size_t statesBeginEnd = 2, statesDt, statesDs, statesLattice0;
478  size_t statesSimExp;
479  double sumStates = 0;
480 
481  trajSim->dtBase() = 0.1;
482  trajSim->dsBase() = 0.1;
483  trajSim->simulateTrajectory(0);
484  statesDt = expectedDtStates(funcs->funcsArcEnd(), trajSim->dt());
485  statesDs = 10 + 1;
486  statesLattice0 = funcs->funcCtrlPtSize(0);
487  sumStates = statesBeginEnd + statesDt + statesDs + statesLattice0;
488  EXPECT_EQ(sumStates, sumAllPartStates(trajSim));
489  // for(size_t i = 0; i < trajSim->simLattice().size(); i++){ cout<<trajSim->simLattice()[i].arc<<", ";
490  // }cout<<endl;
491  statesSimExp = statesDt + /*statesDs - 2 +*/ statesLattice0 - 2;
492  EXPECT_EQ(statesSimExp, trajSim->simLattice().size()); // ds in same locations as dt
493  checkSimLatticeConsistency(trajSim->simulationLattice_);
494 
495  trajSim->dtBase() = 0.12;
496  trajSim->dsBase() = 0.12;
497  trajSim->simulateTrajectory(0);
498  statesDt = expectedDtStates(funcs->funcsArcEnd(), trajSim->dt());
499  statesDs = 9 + 1;
500  statesLattice0 = funcs->funcCtrlPtSize(0);
501  sumStates = statesBeginEnd + statesDt + statesDs + statesLattice0;
502  EXPECT_EQ(sumStates, sumAllPartStates(trajSim));
503  statesSimExp = statesDt + /*statesDs - 2 +*/ statesLattice0 - 2;
504  EXPECT_EQ(statesSimExp, trajSim->simLattice().size()); // ds in same locations as dt
505  checkSimLatticeConsistency(trajSim->simulationLattice_);
506 
507  trajSim->dtBase() = 0.25;
508  trajSim->dsBase() = 0.01;
509  trajSim->simulateTrajectory(0);
510  statesDt = expectedDtStates(funcs->funcsArcEnd(), trajSim->dt());
511  statesDs = 100 + 1;
512  statesLattice0 = funcs->funcCtrlPtSize(0);
513  sumStates = statesBeginEnd + statesDt + statesDs + statesLattice0;
514  EXPECT_EQ(sumStates, sumAllPartStates(trajSim));
515  statesSimExp = statesDt + statesDs - 2 - 3 + statesLattice0 - 2;
516  EXPECT_EQ(statesSimExp, trajSim->simLattice().size()); // 3 ds overlay in same locations with dt (.25, .5, .75)
517  checkSimLatticeConsistency(trajSim->simulationLattice_);
518 }
519 
520 TEST_F(TrajectorySimulatorsTest, SimPrecalcFromNon0DtDsCtrlPt)
521 {
522  ParamFuncs* funcs = trajSim->stateSim()->paramFuncs();
523  // set the parametric functions control points values
524  double initT = 0;
525  size_t funcIdx = 0;
526  funcs->ctrlPtVal(funcIdx, 0, PfCpD::ARC) = 0 + initT;
527  funcs->ctrlPtVal(funcIdx, 1, PfCpD::ARC) = 0.557 + initT;
528  funcs->ctrlPtVal(funcIdx, 2, PfCpD::ARC) = 1 + initT;
529 
530  funcIdx = 0;
531  funcs->ctrlPtVal(funcIdx, 0, PfCpD::VAL) = 1.0;
532  funcs->ctrlPtVal(funcIdx, 1, PfCpD::VAL) = 1.0;
533  funcs->ctrlPtVal(funcIdx, 2, PfCpD::VAL) = 1.0;
534 
535  funcIdx = 1;
536  funcs->ctrlPtVal(funcIdx, 0, PfCpD::VAL) = 3.5 * M_PI;
537  funcs->ctrlPtVal(funcIdx, 1, PfCpD::VAL) = 1.5 * M_PI;
538  funcs->ctrlPtVal(funcIdx, 2, PfCpD::VAL) = -4.5 * M_PI;
539 
540  funcs->precompute();
541 
542  // equal dt, equal arc dist and control ponints arcs lattices
543  vector<vector<double*> > funcKnotsLattice(1, vector<double*>(funcs->funcsArcSize(0), 0));
544  for (size_t i = 0; i < funcKnotsLattice[0].size(); ++i)
545  {
546  funcKnotsLattice[0][i] = &funcs->funcsArc(0, i);
547  }
548  trajSim->setUserDefLattice(funcKnotsLattice);
549 
550  trajSim->dtBase() = 0.13;
551  trajSim->dsBase() = 0.0795;
552  trajSim->simulateTrajectory(0);
553 
554  vector<TrajectorySimulator::LatticePointType> trajStatesRef;
555  for (size_t i = 0; i < trajSim->simulationLattice_.size(); ++i)
556  {
557  StateSPtr stateNow = trajSim->simulationLattice_[i].statePtr->cloneState();
558  trajStatesRef.emplace_back(TrajectorySimulator::LatticePointType(
559  trajSim->simulationLattice_[i].arc, trajSim->simulationLattice_[i].latticeType, stateNow));
560  }
561  TrajectorySimulator::LatticeVecSPtrVec partLatRef;
562  partLatRef.resize(trajSim->partLattices_.size());
563  for (auto& latRefI : partLatRef)
564  {
565  latRefI = shared_ptr<TrajectorySimulator::LatticeVec>(new TrajectorySimulator::LatticeVec);
566  }
567  for (size_t i = 0; i < trajSim->partLattices_.size(); ++i)
568  {
569  partLatRef[i]->resize(trajSim->partLattices_[i]->size());
570  for (size_t j = 0; j < trajSim->partLattices_[i]->size(); ++j)
571  {
572  partLatRef[i]->at(j).arc = trajSim->partLattices_[i]->at(j).arc;
573  partLatRef[i]->at(j).statePtr = trajSim->partLattices_[i]->at(j).statePtr->cloneState();
574  }
575  }
576 
577  trajSim->simulateTrajectory(0.10);
578  checkSimLatticeStatesEqual(trajStatesRef, trajSim->simulationLattice_);
579  checkPartLatticeStatesEqual(partLatRef, trajSim->partLattices_);
580  trajSim->simulateTrajectory(0.34);
581  checkSimLatticeStatesEqual(trajStatesRef, trajSim->simulationLattice_);
582  checkPartLatticeStatesEqual(partLatRef, trajSim->partLattices_);
583  trajSim->simulateTrajectory(0.79);
584  checkSimLatticeStatesEqual(trajStatesRef, trajSim->simulationLattice_);
585  checkPartLatticeStatesEqual(partLatRef, trajSim->partLattices_);
586  trajSim->simulateTrajectory(1.00);
587  checkSimLatticeStatesEqual(trajStatesRef, trajSim->simulationLattice_);
588  checkPartLatticeStatesEqual(partLatRef, trajSim->partLattices_);
589  trajSim->simulateTrajectory(0.54);
590  checkSimLatticeStatesEqual(trajStatesRef, trajSim->simulationLattice_);
591  checkPartLatticeStatesEqual(partLatRef, trajSim->partLattices_);
592  trajSim->simulateTrajectory(1.50);
593  checkSimLatticeStatesEqual(trajStatesRef, trajSim->simulationLattice_);
594  checkPartLatticeStatesEqual(partLatRef, trajSim->partLattices_);
595 }
596 
598 
599 TEST_F(TrajectorySimulatorsTest, SimOnlineFrom0Dt)
600 {
601  ParamFuncs* funcs = trajSim->stateSim()->paramFuncs();
602  // set the parametric functions control points values
603  double initT = 0;
604  size_t funcIdx = 0;
605  funcs->ctrlPtVal(funcIdx, 0, PfCpD::ARC) = 0 + initT;
606  funcs->ctrlPtVal(funcIdx, 1, PfCpD::ARC) = 0.5 + initT;
607  funcs->ctrlPtVal(funcIdx, 2, PfCpD::ARC) = 1 + initT;
608 
609  funcIdx = 0;
610  funcs->ctrlPtVal(funcIdx, 0, PfCpD::VAL) = 3.0 * M_PI;
611  funcs->ctrlPtVal(funcIdx, 1, PfCpD::VAL) = -0.5 * M_PI;
612  funcs->ctrlPtVal(funcIdx, 2, PfCpD::VAL) = 2.0 * M_PI;
613 
614  funcIdx = 1;
615  funcs->ctrlPtVal(funcIdx, 0, PfCpD::VAL) = 3.5 * M_PI;
616  funcs->ctrlPtVal(funcIdx, 1, PfCpD::VAL) = 1.5 * M_PI;
617  funcs->ctrlPtVal(funcIdx, 2, PfCpD::VAL) = -4.5 * M_PI;
618 
619  funcs->precompute();
620 
621  // set lattice types
622  trajSim->dsBase() = -1; // only dt lattice types
623  trajSimOnline->dsBase() = -1;
624 
625  trajSim->dtBase() = 0.10;
626  trajSim->simulateTrajectory(0);
627  trajSimOnline->dtBase() = 0.10;
628  trajSimOnline->simulateTrajectory(0);
629  checkSimLatticeStatesEqual(trajSim->simulationLattice_, trajSimOnline->simulationLattice_);
630  checkPartLatticeStatesEqual(trajSim->partLattices_, trajSimOnline->partLattices_);
631 
632  trajSim->dtBase() = 0.12;
633  trajSim->simulateTrajectory(0);
634  trajSimOnline->dtBase() = 0.12;
635  trajSimOnline->simulateTrajectory(0);
636  checkSimLatticeStatesEqual(trajSim->simulationLattice_, trajSimOnline->simulationLattice_);
637  checkPartLatticeStatesEqual(trajSim->partLattices_, trajSimOnline->partLattices_);
638 
639  trajSim->dtBase() = 0.25;
640  trajSim->simulateTrajectory(0);
641  trajSimOnline->dtBase() = 0.25;
642  trajSimOnline->simulateTrajectory(0);
643  checkSimLatticeStatesEqual(trajSim->simulationLattice_, trajSimOnline->simulationLattice_);
644  checkPartLatticeStatesEqual(trajSim->partLattices_, trajSimOnline->partLattices_);
645 }
646 
647 TEST_F(TrajectorySimulatorsTest, SimOnlineFrom0DtDsCtrlPt)
648 {
649  ParamFuncs* funcs = trajSim->stateSim()->paramFuncs();
650  // set the parametric functions control points values
651  double initT = 0;
652  size_t funcIdx = 0;
653  funcs->ctrlPtVal(funcIdx, 0, PfCpD::ARC) = 0 + initT;
654  funcs->ctrlPtVal(funcIdx, 1, PfCpD::ARC) = 0.557 + initT;
655  funcs->ctrlPtVal(funcIdx, 2, PfCpD::ARC) = 1 + initT;
656 
657  funcIdx = 0;
658  funcs->ctrlPtVal(funcIdx, 0, PfCpD::VAL) = 1.0;
659  funcs->ctrlPtVal(funcIdx, 1, PfCpD::VAL) = 1.0;
660  funcs->ctrlPtVal(funcIdx, 2, PfCpD::VAL) = 1.0;
661 
662  funcIdx = 1;
663  funcs->ctrlPtVal(funcIdx, 0, PfCpD::VAL) = 3.5 * M_PI;
664  funcs->ctrlPtVal(funcIdx, 1, PfCpD::VAL) = 1.5 * M_PI;
665  funcs->ctrlPtVal(funcIdx, 2, PfCpD::VAL) = -4.5 * M_PI;
666 
667  funcs->precompute();
668 
669  vector<vector<double*> > funcKnotsLattice(1, vector<double*>(funcs->funcsArcSize(0), 0));
670  for (size_t i = 0; i < funcKnotsLattice[0].size(); ++i)
671  {
672  funcKnotsLattice[0][i] = &funcs->funcsArc(0, i);
673  }
674  trajSim->setUserDefLattice(funcKnotsLattice);
675  trajSimOnline->setUserDefLattice(funcKnotsLattice);
676 
677  trajSim->dtBase() = 0.10;
678  trajSim->dsBase() = 0.10;
679  trajSim->simulateTrajectory(0);
680  trajSimOnline->dtBase() = 0.10;
681  trajSimOnline->dsBase() = 0.10;
682  trajSimOnline->simulateTrajectory(0);
683  checkSimLatticeStatesEqual(trajSim->simulationLattice_, trajSimOnline->simulationLattice_);
684  checkPartLatticeStatesEqual(trajSim->partLattices_, trajSimOnline->partLattices_);
685 
686  trajSim->dtBase() = 0.12;
687  trajSim->dsBase() = 0.12;
688  trajSim->simulateTrajectory(0);
689  trajSimOnline->dtBase() = 0.12;
690  trajSimOnline->dsBase() = 0.12;
691  trajSimOnline->simulateTrajectory(0);
692  checkSimLatticeStatesEqual(trajSim->simulationLattice_, trajSimOnline->simulationLattice_);
693  checkPartLatticeStatesEqual(trajSim->partLattices_, trajSimOnline->partLattices_);
694 
695  trajSim->dtBase() = 0.25;
696  trajSim->dsBase() = 0.01;
697  trajSim->simulateTrajectory(0);
698  trajSimOnline->dtBase() = 0.25;
699  trajSimOnline->dsBase() = 0.01;
700  trajSimOnline->simulateTrajectory(0);
701  checkSimLatticeStatesEqual(trajSim->simulationLattice_, trajSimOnline->simulationLattice_);
702  checkPartLatticeStatesEqual(trajSim->partLattices_, trajSimOnline->partLattices_);
703 }
704 
705 TEST_F(TrajectorySimulatorsTest, SimOnlineFromNon0DtDsCtrlPt)
706 {
707  ParamFuncs* funcs = trajSim->stateSim()->paramFuncs();
708  // set the parametric functions control points values
709  double initT = 0;
710  size_t funcIdx = 0;
711  funcs->ctrlPtVal(funcIdx, 0, PfCpD::ARC) = 0 + initT;
712  funcs->ctrlPtVal(funcIdx, 1, PfCpD::ARC) = 0.557 + initT;
713  funcs->ctrlPtVal(funcIdx, 2, PfCpD::ARC) = 1 + initT;
714 
715  funcIdx = 0;
716  funcs->ctrlPtVal(funcIdx, 0, PfCpD::VAL) = 0.45;
717  funcs->ctrlPtVal(funcIdx, 1, PfCpD::VAL) = -1.0;
718  funcs->ctrlPtVal(funcIdx, 2, PfCpD::VAL) = 2.4;
719 
720  funcIdx = 1;
721  funcs->ctrlPtVal(funcIdx, 0, PfCpD::VAL) = 3.5 * M_PI;
722  funcs->ctrlPtVal(funcIdx, 1, PfCpD::VAL) = 1.5 * M_PI;
723  funcs->ctrlPtVal(funcIdx, 2, PfCpD::VAL) = -4.5 * M_PI;
724 
725  funcs->precompute();
726 
727  vector<vector<double*> > funcKnotsLattice(1, vector<double*>(funcs->funcsArcSize(0), 0));
728  for (size_t i = 0; i < funcKnotsLattice[0].size(); ++i)
729  {
730  funcKnotsLattice[0][i] = &funcs->funcsArc(0, i);
731  }
732  trajSim->setUserDefLattice(funcKnotsLattice);
733  trajSimOnline->setUserDefLattice(funcKnotsLattice);
734 
735  trajSim->dtBase() = 0.13;
736  trajSim->dsBase() = 0.0795;
737  trajSim->simulateTrajectory(0);
738  trajSimOnline->dtBase() = 0.13;
739  trajSimOnline->dsBase() = 0.0795;
740  trajSimOnline->simulateTrajectory(0);
741 
742  trajSim->simulateTrajectory(0.10);
743  trajSimOnline->simulateTrajectory(0.10);
744  checkSimLatticeStatesEqual(trajSim->simulationLattice_, trajSimOnline->simulationLattice_);
745  checkPartLatticeStatesEqual(trajSim->partLattices_, trajSimOnline->partLattices_);
746  trajSim->simulateTrajectory(0.34);
747  trajSimOnline->simulateTrajectory(0.34);
748  checkSimLatticeStatesEqual(trajSim->simulationLattice_, trajSimOnline->simulationLattice_);
749  checkPartLatticeStatesEqual(trajSim->partLattices_, trajSimOnline->partLattices_);
750  trajSim->simulateTrajectory(0.79);
751  trajSimOnline->simulateTrajectory(0.79);
752  checkSimLatticeStatesEqual(trajSim->simulationLattice_, trajSimOnline->simulationLattice_);
753  checkPartLatticeStatesEqual(trajSim->partLattices_, trajSimOnline->partLattices_);
754  trajSim->simulateTrajectory(1.00);
755  trajSimOnline->simulateTrajectory(1.00);
756  checkSimLatticeStatesEqual(trajSim->simulationLattice_, trajSimOnline->simulationLattice_);
757  checkPartLatticeStatesEqual(trajSim->partLattices_, trajSimOnline->partLattices_);
758  trajSim->simulateTrajectory(0.52);
759  trajSimOnline->simulateTrajectory(0.52);
760  checkSimLatticeStatesEqual(trajSim->simulationLattice_, trajSimOnline->simulationLattice_);
761  checkPartLatticeStatesEqual(trajSim->partLattices_, trajSimOnline->partLattices_);
762  trajSim->simulateTrajectory(1.50);
763  trajSimOnline->simulateTrajectory(1.50);
764  checkSimLatticeStatesEqual(trajSim->simulationLattice_, trajSimOnline->simulationLattice_);
765  checkPartLatticeStatesEqual(trajSim->partLattices_, trajSimOnline->partLattices_);
766 }
767 
769 
770 } // namespace
771 
772 int main(int argc, char** argv)
773 {
774  ::testing::InitGoogleTest(&argc, argv);
775  return RUN_ALL_TESTS();
776 }
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::StateSPtr
std::shared_ptr< State > StateSPtr
Definition: state.h:129
tuw::CtrlPtDim::VAL
control point value
tuw::ParamFuncs::EvalArcGuarantee::AFTER_LAST
previous evaluation arc <= this evaluation arc
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
tuw::TrajectorySimulator::LatticePointType
Structure containing the lattice type afferent to a.
Definition: trajectory_simulator.h:102
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::TrajectorySimulator::LatticeVec
std::vector< LatticePoint > LatticeVec
Definition: trajectory_simulator.h:96
tuw::ParamFuncs::funcsArcEnd
double & funcsArcEnd()
Access to the final value of the arc parametrization.
Definition: param_func.cpp:205
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.
main
int main(int argc, char **argv)
Definition: test_trajectory_simulators.cpp:772
tuw::ParamFuncs::funcCtrlPtSize
std::size_t funcCtrlPtSize(const std::size_t &_i) const
Number of control points for a parametric function.
Definition: param_func.cpp:188
tuw::ParamFuncs::CtrlPtDim
CtrlPtDim
Control point variable dimension.
Definition: param_func.h:73
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::TrajectorySimulatorSPtr
std::shared_ptr< TrajectorySimulator > TrajectorySimulatorSPtr
Definition: trajectory_simulator.h:46
tuw::ParamFuncs::FuncEvalMode
FuncEvalMode
Required type of computation relative to the parametric function.
Definition: param_func.h:62
tuw::ParamFuncs::funcsArcSize
std::size_t funcsArcSize() const
Number of arc parametrizations.
Definition: param_func.cpp:192
tuw::TrajectorySimulator::LatticeVecSPtrVec
std::vector< std::shared_ptr< std::vector< LatticePoint > > > LatticeVecSPtrVec
Definition: trajectory_simulator.h:98
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