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

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