2/trajectory_simulator.hpp File Reference

#include <tuw_control/state_sim/state_sim.hpp>
#include <tuw_control/param_func_new/param_func_dist.hpp>
#include <tuw_control/costs_evaluator/costs_evaluator.hpp>
#include <functional>
#include <memory>
#include <queue>

Include dependency graph for 2/trajectory_simulator.hpp:

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Classes
struct	tuw::LatticePoint< TNumType, TStateType >
	Structure containing an evaluation arc and a state pointer. More...
class	tuw::LatticeTypeBaseCRTP< TDerived, TNumType, TSimType, TLatticeCostFuncs >

Namespaces
	tuw

Functions
template<bool canComputeStateGrad = CanComputeStateGrad, typename std::enable_if<(canComputeStateGrad)>::type * = nullptr>
void	tuw::advanceFuncSimGrad (const TNumType &_arcNow)
template<bool useStateSimNm = TUseStateNm, typename std::enable_if<(useStateSimNm)>::type * = nullptr>
void	tuw::bindAdvanceFunc ()
template<bool useStateSimNm = TUseStateNm, typename std::enable_if<(useStateSimNm)>::type * = nullptr>
void	tuw::bindAdvanceFuncGrad ()
size_t	tuw::bindFromPartLatticesToSimLattice (const bool &_saveLatticeStates=false)
template<typename TSimType2 , typename TLatticePointType >
static void	tuw::copyDataFromSimToSimLatticeI (const TSimType2 &_sim, TLatticePointType &_latticePtI)
template<typename TSimType2 , typename TLatticePointType , bool useStateSimNm = TUseStateNm, typename std::enable_if<(useStateSimNm)>::type * = nullptr>
static void	tuw::copyDataNmFromSimToSimLatticeI (const TSimType2 &_sim, TLatticePointType &_latticePtI)
template<typename TSimType2 , typename TLatticePointType , bool canComputeStateGrad = CanComputeStateGrad, typename std::enable_if<(canComputeStateGrad)>::type * = nullptr>
static void	tuw::copyGradDataFromSimToSimLatticeI (const TSimType2 &_sim, TLatticePointType &_latticePtI)
template<typename TSimType2 , typename TLatticePointType , bool useStateSimNm = TUseStateNm, typename std::enable_if<(useStateSimNm)>::type * = nullptr>
static void	tuw::copyGradDataNmFromSimToSimLatticeI (const TSimType2 &_sim, TLatticePointType &_latticePtI)
template<typename TSimType2 , typename TLatticePointType , bool canComputeStateGrad = CanComputeStateGrad, typename std::enable_if<(!canComputeStateGrad)>::type * = nullptr>
void	tuw::copyReqDataFromSimToSimLatticeI (const TSimType2 &_sim, TLatticePointType &_latticePtI)
template<typename TSimType2 , typename TLatticePointType , bool canComputeStateGrad = CanComputeStateGrad, typename std::enable_if<(!canComputeStateGrad)>::type * = nullptr>
static void	tuw::copyStructureFromSimToSimLatticeI (const TSimType2 &_sim, TLatticePointType &_latticePtI)
template<std::size_t IIMax, std::size_t II = 0>
constexpr std::enable_if<(II==IIMax), void >::type	tuw::eval_all_costs (TNumType *&_cfPtr, TNumType *&_cfGradPtr, const size_t &_optParamSize)
template<std::size_t IIMax, std::size_t II = 0>
constexpr std::enable_if<(II< IIMax), void >::type	tuw::eval_all_costs (TNumType *&_cfPtr, TNumType *&_cfGradPtr, const size_t &_optParamSize)
template<size_t FuncNr = 0>
void	tuw::evaluateCosts (TNumType *&_cfPtr, TNumType *&_cfGradPtr, const size_t &_optParamSize)
template<std::size_t II = 0, class FuncT , typename... Tp>
constexpr std::enable_if< II==sizeof...(Tp), void >::type	tuw::for_each_tuple_class (std::tuple< Tp... > &, FuncT &)
void	tuw::getMinArcLatCacheIdx (typename std::vector< LatticePointType >::iterator *&_itMin, size_t &_latVecIdx)
size_t	tuw::populatePartLattices (size_t &_sizeCosts)
void	tuw::resizeCosts (const size_t &_optParamSize, const bool &_simulatingWithGrad)
void	tuw::resizeInternal ()
void	tuw::resizeSimLattice (const size_t &_totalPartLatticePtsSize)
template<bool useStateSimNm = TUseStateNm, typename std::enable_if<(useStateSimNm)>::type * = nullptr>
void	tuw::setXNmDot (const TNumType &_arcNow)
void	tuw::simulateTrajectory (const bool &_saveLatticeStates=false)
void	tuw::simulateTrajectoryImpl (const bool &_saveLatticeStates=false)
template<bool canComputeStateGrad = CanComputeStateGrad, typename std::enable_if<(canComputeStateGrad)>::type * = nullptr>
void	tuw::simulateTrajectoryWithGrad (const bool &_saveLatticeStates=false)

Variables
template<std::size_t II = 0, class FuncT , typename... Tp>
constexpr std::enable_if< II< sizeof...(Tp), void >::type for_each_tuple_class(std::tuple< Tp... > &t, FuncT &f){f[II]=[&t](const auto &_a, const auto &_b, const auto &_c, auto &_d){std::get< II >t).computeDArcIdPImpl(_a, _b, _c, _d);};for_each_tuple_class< II+1, FuncT, Tp... >t, f);}template< std::size_t IIMax, std::size_t II=0 >constexpr inline typename std::enable_if<(II==IIMax), void >::type get_costs_sizes(auto &partLatI, const size_t &_i, auto &_sizeCostsPerPartLattice, auto &_sizeCostsPerType){}template< std::size_t IIMax, std::size_t II=0 >constexpr inline typename std::enable_if<(II< IIMax), void >::type get_costs_sizes(auto &partLatI, const size_t &_i, auto &_sizeCostsPerPartLattice, auto &_sizeCostsPerType){size_t partLatICostsTypeJ=partLatI.lattice.size()*partLatI.template costFuncsNr< II >);_sizeCostsPerPartLattice[_i]+=partLatICostsTypeJ;_sizeCostsPerType[II]+=partLatICostsTypeJ;get_costs_sizes< IIMax, II+1 >partLatI, _i, _sizeCostsPerPartLattice, _sizeCostsPerType);}template< typename TNumType, typename TSimType, bool TUseStateNm, template< typename, typename > class...TLatticeTypes >class TrajectorySimulator{public:using StateSimSPtr=std::shared_ptr< TSimType >;public:using StateType=typename TSimType::StateType;public:using StateSPtr=std::shared_ptr< StateType >;public:using StateForSimType=typename TSimType::StateForSimType;public:using LatticePointType=LatticePoint< TNumType, StateType >;public:static constexpr const bool CanComputeStateGrad=!std::is_same< EmptyGradType, typename StateMapBaseTraits< StateType >::StateWithGradNmType >::value;public:static constexpr const size_t CostFuncsTypesNr=std::tuple_element< 0, std::tuple< TLatticeTypes< TNumType, TSimType >... > >::type::costFuncsTypesNr();public:TrajectorySimulator():stateSim_(std::shared_ptr< TSimType >new TSimType)){for_each_tuple_class(partialLattices_, correctStateGradFunc);for(size_t i=0;i< gradCostsMap_.size();++i){gradCostsMap_[i]=std::shared_ptr< Eigen::Map< Eigen::Matrix< TNumType,-1,-1, Eigen::RowMajor > > >new Eigen::Map< Eigen::Matrix< TNumType,-1,-1, Eigen::RowMajor > >nullptr, 0, 0));}}public:TrajectorySimulator(StateSimSPtr &_stateSim):stateSim_(_stateSim){for_each_tuple_class(partialLattices_, correctStateGradFunc);for(size_t i=0;i< gradCostsMap_.size();++i){gradCostsMap_[i]=std::shared_ptr< Eigen::Map< Eigen::Matrix< TNumType,-1,-1, Eigen::RowMajor > > >new Eigen::Map< Eigen::Matrix< TNumType,-1,-1, Eigen::RowMajor > >nullptr, 0, 0));}}public:StateSimSPtr &stateSim(){return stateSim_;}public:const StateSimSPtr &stateSim() const {return stateSim_;}public:LatticePointType &simLatticeI(const size_t &_i){return simulationLattice_[_i];}public:const LatticePointType &simLatticeI(const size_t &_i) const {return simulationLattice_[_i];}public:size_t simLatticeSize() const {return simulationLatticeActiveSize_;}public:template< template< typename, typename > class TLatticeType > TLatticeType< TNumType, TSimType > &partialLattice(){return std::get< TLatticeType< TNumType, TSimType > >partialLattices_);}public:template< template< typename, typename > class TLatticeType > const TLatticeType< TNumType, TSimType > &partialLattice() const {return std::get< TLatticeType< TNumType, TSimType > >partialLattices_);}public:template< size_t TLatticeIdx > auto &partialLattice(){return std::get< TLatticeIdx >partialLattices_);}public:template< size_t TLatticeIdx > const auto &partialLattice() const {return std::get< TLatticeIdx >partialLattices_);}private:using AdvanceFunction=void(TrajectorySimulator< TNumType, TSimType, TUseStateNm, TLatticeTypes... >::*)(const TNumType &);private:AdvanceFunction advanceFunc;private:void advanceFuncSimEmpty(const TNumType &_arcNow){}private:void advanceFuncSim(const TNumType &_arcNow){stateSim_->	tuw::advance (_arcNow)
std::array< std::function< void(const TSimType &, const TNumType &, const size_t &, typename StateMapBaseTraits< typename StateType::StateMapBaseType >::StateGradType &)>, sizeof...(TLatticeTypes)>	tuw::correctStateGradFunc
StateMapArray< TNumType, StateMapVector< TNumType, TNumType >, CostFuncsTypesNr >	tuw::costs_
StateMapArray< TNumType, StateMapVector< TNumType, StateMapVector< TNumType, TNumType > >, CostFuncsTypesNr >	tuw::gradCosts_
std::array< std::shared_ptr< Eigen::Map< Eigen::Matrix< TNumType,-1,-1, Eigen::RowMajor > > >, CostFuncsTypesNr >	tuw::gradCostsMap_
std::tuple< TLatticeTypes< TNumType, TSimType >... >	tuw::partialLattices_
	Vector containing the ordered sequence of arc parametrizations for each of the used lattices. More...
bool	tuw::simulatingWithGrad
std::vector< LatticePointType >	tuw::simulationLattice_
	Lattice requesting each simulated trajectory state. More...
size_t	tuw::simulationLatticeActiveSize_
std::array< size_t, sizeof...(TLatticeTypes)>	tuw::sizeCosts_
std::array< size_t, sizeof...(TLatticeTypes)>	tuw::sizeCostsPerPartLattice_
std::array< size_t, CostFuncsTypesNr >	tuw::sizeCostsPerType_
StateSimSPtr	tuw::stateSim_
	State simulator object. More...