test_trajectory_simulator_new.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/platform_models/agv_diff_drive_v_w.hpp>
6 #include <tuw_control/trajectory_simulator_new_2/trajectory_simulator.hpp>
7 #include <tuw_control/platform_models/agv_diff_drive_v_w_lattices.hpp>
8 
9 using namespace tuw;
10 using namespace std;
11 using namespace DiffDrive;
12 
13 namespace
14 {
15 using PFS = ParamFuncsStructure;
16 using PfCpD = CtrlPtDim;
17 using PFFeM = FuncEvalMode;
18 using EaG = EvalArcGuarantee;
19 
20 template <class TNumType, class TMapDataType>
21 using LatticeTypeStateSimBeginEndSim = LatticeTypeStateSimBeginEnd<TNumType, TMapDataType>;
22 template <class TNumType, class TMapDataType>
23 using LatticeTypeStateSimEqDtSim = LatticeTypeStateSimEqDt<TNumType, TMapDataType>;
24 template <class TNumType, class TMapDataType>
25 using LatticeTypeStateSimEqDsSim = LatticeTypeStateSimEqDs<TNumType, TMapDataType>;
26 template <class TNumType, class TMapDataType>
27 using LatticeTypeStateSimCtrlPtKnotsSim = LatticeTypeStateSimCtrlPtKnots<TNumType, TMapDataType>;
28 
29 using NumType = double;
30 using MapDataType = double; // dummy
31 template <class TNumType>
32 using DiscrType = heun_abc<TNumType>;
33 // using StateDiffDriveType = StateDiffDriveVW;
34 // template<template<class> class TDiscretizationType> using StateSimDiffDriveType =
35 // StateSimDiffDriveVW<TDiscretizationType>;
36 
37 using StateDiffDriveType = StateWithGradVW<NumType>;
38 using StateSimDiffDriveType = StateWithGradSimVW<NumType, MapDataType, DiscrType>;
39 using StateSimPtr = std::shared_ptr<StateSimDiffDriveType>;
40 using TrajectorySimulatorType =
41  TrajectorySimulator<NumType, StateSimDiffDriveType, true, LatticeTypeStateSimBeginEndSim,
42  LatticeTypeStateSimEqDtSim, LatticeTypeStateSimEqDsSim, LatticeTypeStateSimCtrlPtKnotsSim>;
43 using TrajectorySimulatorSPtr = std::shared_ptr<TrajectorySimulatorType>;
44 
45 using PFV = ParamType<NumType, MapDataType>::ParamFuncVars;
46 
47 // The fixture for testing class Foo.
48 class TrajectorySimulatorTest : public ::testing::Test
49 {
50 protected:
51  // You can remove any or all of the following functions if its body
52  // is empty.
53 
54  TrajectorySimulatorTest()
55  {
56  // construct stateSim object and initalize the parametric functions object
57  sp = std::shared_ptr<StateSimDiffDriveType>(new StateSimDiffDriveType);
58  // sp->setDiscrType( RungeKutta::DiscretizationType::EULER );
59 
60  // vector<PFS> pf( 2 , PFS() );
61  array<PFS, 2> pf;
62  static constexpr const int paramFuncArcRefIdxAll = 0;
63  pf[asInt(PFV::V)].ctrlPtsSize = 3;
64  pf[asInt(PFV::V)].ctrlPtsArcRefIdx = paramFuncArcRefIdxAll;
65  pf[asInt(PFV::W)].ctrlPtsSize = 3;
66  pf[asInt(PFV::W)].ctrlPtsArcRefIdx = paramFuncArcRefIdxAll;
67 
68  pf[asInt(PFV::V)].evalReq[asInt(PFFeM::INT1)] = false;
69  pf[asInt(PFV::V)].evalReq[asInt(PFFeM::INT2)] = false;
70  pf[asInt(PFV::W)].evalReq[asInt(PFFeM::INT1)] = true;
71  pf[asInt(PFV::W)].evalReq[asInt(PFFeM::INT2)] = false;
72  vector<size_t> idxClosedFormV(1, asInt(PFV::V));
73 
74  sp->paramStruct->paramFuncs.init(pf);
75  sp->paramStruct->paramFuncs.setDistCfMode(TraveledDistCfMode::V, idxClosedFormV);
76 
77  // construct trajectory simulator object
78  trajSim = std::shared_ptr<TrajectorySimulatorType>(new TrajectorySimulatorType(sp));
79  }
80 
81  virtual ~TrajectorySimulatorTest()
82  {
83  }
84 
85  // If the constructor and destructor are not enough for setting up
86  // and cleaning up each test, you can define the following methods:
87 
88  virtual void SetUp()
89  {
90  Test::SetUp();
91  // Code here will be called immediately after the constructor (right
92  // before each test).
93  }
94 
95  virtual void TearDown()
96  {
97  Test::TearDown();
98  // Code here will be called immediately after each test (right
99  // before the destructor).
100  }
101  StateSimPtr sp;
102  TrajectorySimulatorSPtr trajSim;
103 
104 public:
105  bool checkSimLatticeConsistency()
106  {
107  bool consistent = true;
108  // for ( size_t i = 0; i < trajSim->simLatticeSize(); ++i ) {
109  // cout<<trajSim->simLatticeI(i).arc<<", ";
110  // // for ( size_t j = 0; j < trajSim->simLatticeI(i).statePtr->stateSize(); ++j ) {
111  // cout<<trajSim->simLatticeI(i).statePtr->data()(j)<<", "; }cout<<endl;
112  // } cout<<endl;
113  for (size_t i = 1; i < trajSim->simLatticeSize(); ++i)
114  {
115  if (!(trajSim->simLatticeI(i - 1).arc < trajSim->simLatticeI(i).arc))
116  {
117  consistent = false;
118  break;
119  }
120  }
121  return consistent;
122  }
123  size_t sumAllPartStates(TrajectorySimulatorSPtr _tsp)
124  {
125  size_t nrPartStates = 0;
126  for_each_tuple(_tsp->partialLattices_, [&nrPartStates](auto& partLatticeI)
127  {
128  nrPartStates += partLatticeI.lattice.size();
129  });
130  return nrPartStates;
131  }
132  // void checkSimLatticeStatesEqual ( vector <TrajectorySimulatorType::LatticePointType >& _latticeRef/*, vector
133  // <TrajectorySimulatorType::LatticePointType >& _latticeTest*/ ) {
134  // // for ( size_t i = 0; i < _latticeTest.size(); ++i ) {
135  // // for ( size_t j = 0; j < _latticeTest[i].statePtr->stateSize(); ++j ) {
136  // cout<<_latticeRef[i].statePtr->value(j)<<":"<<_latticeTest[i].statePtr->value(j)<<", "; }cout<<endl;
137  // // }cout<<endl;
138  // ASSERT_EQ( _latticeRef.size(), trajSim->simLatticeSize() );
139  // for ( size_t i = 0; i < trajSim->simLatticeSize(); ++i ) {
140  // for ( int j = 0; j < trajSim->simLatticeI(i).statePtr->data().size(); ++j ) { EXPECT_DOUBLE_EQ(
141  // _latticeRef[i].statePtr->data()(j), trajSim->simLatticeI(i).statePtr->data()(j) ); }
142  // }
143  // }
144  // void checkPartLatticeStatesEqual ( TrajectorySimulatorType::LatticeVecSPtrVec& _partLatRef,
145  // TrajectorySimulatorType::LatticeVecSPtrVec& _partLatTest ) {
146  // ASSERT_EQ( _partLatRef.size(), _partLatTest.size() );
147  // for ( size_t i = 0; i < _partLatRef.size(); ++i ) {
148  // ASSERT_EQ( _partLatRef[i]->size(), _partLatTest[i]->size() );
149  // for ( size_t j = 0; j < _partLatRef[i]->size(); ++j ) {
150  // EXPECT_DOUBLE_EQ( _partLatRef[i]->at(j).arc, _partLatTest[i]->at(j).arc );
151  // for ( int k = 0; k < _partLatRef[i]->at(j).statePtr->data().size(); ++k ) {
152  // EXPECT_DOUBLE_EQ( _partLatRef[i]->at(j).statePtr->data()(k), _partLatTest[i]->at(j).statePtr->data()(k) );
153  // }
154  // }
155  // }
156  // }
157  size_t expectedDtStates(const double _DeltaT, const double _dt)
158  {
159  return (ceil(_DeltaT / _dt) + 1);
160  }
161 };
162 
164 
165 TEST_F(TrajectorySimulatorTest, SimPrecalcFrom0Dt)
166 {
167  auto* funcs = &trajSim->stateSim()->paramStruct->paramFuncs;
168  // set the parametric functions control points values
169  double initT = 0;
170  size_t funcIdx = 0;
171  funcs->ctrlPtVal(funcIdx, 0, PfCpD::ARC) = 0 + initT;
172  funcs->ctrlPtVal(funcIdx, 1, PfCpD::ARC) = 0.5 + initT;
173  funcs->ctrlPtVal(funcIdx, 2, PfCpD::ARC) = 1 + initT;
174 
175  funcIdx = 0;
176  funcs->ctrlPtVal(funcIdx, 0, PfCpD::VAL) = 3.0 * M_PI;
177  funcs->ctrlPtVal(funcIdx, 1, PfCpD::VAL) = -0.5 * M_PI;
178  funcs->ctrlPtVal(funcIdx, 2, PfCpD::VAL) = 2.0 * M_PI;
179 
180  funcIdx = 1;
181  funcs->ctrlPtVal(funcIdx, 0, PfCpD::VAL) = 3.5 * M_PI;
182  funcs->ctrlPtVal(funcIdx, 1, PfCpD::VAL) = 1.5 * M_PI;
183  funcs->ctrlPtVal(funcIdx, 2, PfCpD::VAL) = -4.5 * M_PI;
184 
185  funcs->precompute();
186 
187  auto& latticeDt = trajSim->partialLattice<LatticeTypeStateSimEqDtSim>();
188  auto& latticeDs = trajSim->partialLattice<LatticeTypeStateSimEqDsSim>();
189  auto& latticeCP = trajSim->partialLattice<LatticeTypeStateSimCtrlPtKnotsSim>();
190 
191  latticeDt.setDt(0.1);
192  latticeDs.ds_ = -1;
193  latticeCP.inUse = false;
194 
195  size_t statesBeginEnd = 2, statesDt, statesDs, statesLattice0;
196  double sumStates = 0;
197 
198  latticeDt.setDt(0.1);
199  trajSim->simulateTrajectory();
200  statesDt = expectedDtStates(funcs->funcsArcEnd(), latticeDt.dt());
201  statesDs = 0;
202  statesLattice0 = 0;
203  sumStates = statesBeginEnd + statesDt + statesDs + statesLattice0;
204  EXPECT_EQ(sumStates, sumAllPartStates(trajSim));
205  EXPECT_EQ(sumStates, trajSim->simLatticeSize());
206  checkSimLatticeConsistency();
207 
208  latticeDt.setDt(0.12);
209  trajSim->simulateTrajectory();
210  statesDt = expectedDtStates(funcs->funcsArcEnd(), latticeDt.dt());
211  statesDs = 0;
212  statesLattice0 = 0;
213  sumStates = statesBeginEnd + statesDt + statesDs + statesLattice0;
214  EXPECT_EQ(sumStates, sumAllPartStates(trajSim));
215  EXPECT_EQ(sumStates /*statesSimExp*/, trajSim->simLatticeSize());
216  checkSimLatticeConsistency();
217 
218  latticeDt.setDt(0.25);
219  trajSim->simulateTrajectory();
220  statesDt = expectedDtStates(funcs->funcsArcEnd(), latticeDt.dt());
221  statesDs = 0;
222  statesLattice0 = 0;
223  sumStates = statesBeginEnd + statesDt + statesDs + statesLattice0;
224  EXPECT_EQ(sumStates, sumAllPartStates(trajSim));
225  EXPECT_EQ(sumStates, trajSim->simLatticeSize());
226  checkSimLatticeConsistency();
227 }
228 
229 TEST_F(TrajectorySimulatorTest, SimPrecalcFrom0DtDsCtrlPt)
230 {
231  auto* funcs = &trajSim->stateSim()->paramStruct->paramFuncs;
232  // set the parametric functions control points values
233  double initT = 0;
234  size_t funcIdx = 0;
235  funcs->ctrlPtVal(funcIdx, 0, PfCpD::ARC) = 0 + initT;
236  funcs->ctrlPtVal(funcIdx, 1, PfCpD::ARC) = 0.557 + initT;
237  funcs->ctrlPtVal(funcIdx, 2, PfCpD::ARC) = 1 + initT;
238 
239  funcIdx = 0;
240  funcs->ctrlPtVal(funcIdx, 0, PfCpD::VAL) = 1.0;
241  funcs->ctrlPtVal(funcIdx, 1, PfCpD::VAL) = 1.0;
242  funcs->ctrlPtVal(funcIdx, 2, PfCpD::VAL) = 1.0;
243 
244  funcIdx = 1;
245  funcs->ctrlPtVal(funcIdx, 0, PfCpD::VAL) = 3.5 * M_PI;
246  funcs->ctrlPtVal(funcIdx, 1, PfCpD::VAL) = 1.5 * M_PI;
247  funcs->ctrlPtVal(funcIdx, 2, PfCpD::VAL) = -4.5 * M_PI;
248 
249  funcs->precompute();
250 
251  // equal dt, equal arc dist and control ponints arcs lattices
252  size_t statesBeginEnd = 2, statesDt, statesDs, statesLattice0;
253  double sumStates = 0;
254 
255  auto& latticeDt = trajSim->partialLattice<LatticeTypeStateSimEqDtSim>();
256  auto& latticeDsVal = trajSim->partialLattice<LatticeTypeStateSimEqDsSim>().ds_;
257  auto& latticeCPVal = trajSim->partialLattice<LatticeTypeStateSimCtrlPtKnotsSim>().inUse;
258  latticeDt.setDt(0.1);
259  latticeDsVal = -1;
260  latticeCPVal = true;
261 
262  latticeDt.setDt(0.1);
263  latticeDsVal = 0.1;
264  trajSim->simulateTrajectory();
265  statesDt = expectedDtStates(funcs->funcsArcEnd(), latticeDt.dt());
266  statesDs = 10 + 1;
267  statesLattice0 = funcs->funcCtrlPtSize(0);
268  sumStates = statesBeginEnd + statesDt + statesDs + statesLattice0;
269  EXPECT_EQ(sumStates, sumAllPartStates(trajSim));
270  // for(size_t i = 0; i < trajSim->simLattice().size(); i++){ cout<<trajSim->simLattice()[i].arc<<", ";
271  // }cout<<endl;
272  EXPECT_EQ(sumStates, trajSim->simLatticeSize()); // ds in same locations as dt
273  checkSimLatticeConsistency();
274 
275  latticeDt.setDt(0.1);
276  latticeDsVal = 0.12;
277  trajSim->simulateTrajectory();
278  statesDt = expectedDtStates(funcs->funcsArcEnd(), latticeDt.dt());
279  statesDs = 9 + 1;
280  statesLattice0 = funcs->funcCtrlPtSize(0);
281  sumStates = statesBeginEnd + statesDt + statesDs + statesLattice0;
282  EXPECT_EQ(sumStates, sumAllPartStates(trajSim));
283  EXPECT_EQ(sumStates, trajSim->simLatticeSize()); // ds in same locations as dt
284  checkSimLatticeConsistency();
285 
286  latticeDt.setDt(0.1);
287  latticeDsVal = 0.01;
288  trajSim->simulateTrajectory();
289  statesDt = expectedDtStates(funcs->funcsArcEnd(), latticeDt.dt());
290  statesDs = 100 + 1;
291  statesLattice0 = funcs->funcCtrlPtSize(0);
292  sumStates = statesBeginEnd + statesDt + statesDs + statesLattice0;
293  EXPECT_EQ(sumStates, sumAllPartStates(trajSim));
294  EXPECT_EQ(sumStates, trajSim->simLatticeSize()); // 3 ds overlay in same locations with dt (.25, .5, .75)
295  checkSimLatticeConsistency();
296 }
297 
299 
300 } // namespace
301 
302 int main(int argc, char** argv)
303 {
304  ::testing::InitGoogleTest(&argc, argv);
305  return RUN_ALL_TESTS();
306 }
tuw::ParamFuncsStructure
Containts parametric function initialization data.
Definition: param_func.hpp:76
tuw::CtrlPtDim::VAL
control point value
tuw::CtrlPtDim::ARC
control point arc parameter
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
agv_diff_drive_v_w.hpp
tuw::for_each_tuple
constexpr std::enable_if< II==sizeof...(Tp), void >::type for_each_tuple(std::tuple< Tp... > &, FuncT)
Definition: utils.h:101
tuw::FuncEvalMode::INT1
function single integral
tuw::TraveledDistCfMode::V
agent base center linear velocity is parametric function
trajectory_simulator.hpp
tuw::EvalArcGuarantee
EvalArcGuarantee
Flags if any guarantees about evaluation arc relative to last evaluation arc are present.
Definition: param_func.hpp:61
tuw::TrajectorySimulator::simulateTrajectory
virtual void simulateTrajectory(double _lastValidArc=0)=0
Simulates (discrete numerical evaluation) an entire trajectory according to the specified intervals a...
tuw::TrajectorySimulatorSPtr
std::shared_ptr< TrajectorySimulator > TrajectorySimulatorSPtr
Definition: trajectory_simulator.h:46
main
int main(int argc, char **argv)
Definition: test_trajectory_simulator_new.cpp:302
tuw::CtrlPtDim
CtrlPtDim
Control point variable dimension.
Definition: param_func.hpp:55
tuw::StateSimPtr
std::shared_ptr< StateSim > StateSimPtr
Definition: discretization_runge_kutta_alias.hpp:44
tuw::TrajectorySimulator::ds_
double ds_
Arc parametrization interval used for the equal distance lattice.
Definition: trajectory_simulator.h:258
agv_diff_drive_v_w_lattices.hpp
tuw::FuncEvalMode
FuncEvalMode
Required type of computation relative to the parametric function.
Definition: param_func.hpp:45
tuw::FuncEvalMode::INT2
function double integral
tuw::heun_abc
Definition: discretization_options.hpp:75

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