smoothing.hpp

/*
 * smoothing.hpp
 *
 *  Created on: Jun 26, 2009
 *      Author: jmainpri
 */

#ifndef BASEOPTIMIZATION_HPP_
#define BASEOPTIMIZATION_HPP_

#undef Trajectory

#include "API/Trajectory/trajectory.hpp"

extern std::vector< std::pair<double,double> > traj_convergence_with_time;

namespace API 
{
        class Smoothing : public Trajectory 
        {
        public:
                Smoothing();
                Smoothing(const Trajectory& T);
                Smoothing(Robot* R,traj* t);
                
                ~Smoothing();
    
    int getRunId() { return m_runId; }
    
    void setRunId(int id) { m_runId = id; }
    
    void setStep( double step );
    
    void resetStep();
    
                double getTime() { return m_time; }
    
    int getIteration() { return m_Iteration; }
                
                void setContextName(std::string name) { m_ContextName = name; } 
                
                void saveOptimToFile( std::string str );
    
        void removeRedundantNodes();
    
                void setSortedIndex();
    
    confPtr_t getRandConfAlongTraj(double& randDist, bool use_bias);
    
                bool oneLoopShortCut();

                bool oneLoopShortCutRecompute();
                
                void runShortCut(int nbIteration, int idRun = 0);

        int removeUselessNodes();

        protected:
    
                bool checkStopConditions( unsigned int iter );
    
                std::vector<confPtr_t> get2RandomConf( double step, double& secondDist, double& firstDist);
                
                std::vector<confPtr_t> getConfAtStepAlongTraj( double step, double firstDist, double secondDist );
                
                bool partialShortcut();
                
                bool isLowerCostLargePortion( double lFirst , double lSecond , std::vector<LocalPath*>& paths);
                
                double interpolateOneDoF( unsigned int ithActiveDoF , double init , double goal , double alpha );
                
                void changeIthActiveDofValueOnConf( Configuration& q, unsigned int ithActiveDoF, double value );
                
                void debugShowTraj(double lPrev,double lNext);
                
                
                double getBiasedParamOnTraj();
                
                double closestResolutionToStep(double length,double step);
                
                double gainOfLastIterations( unsigned int n );
    
    void computeStats();
    
    void storeCostAndGain( double NewCost, double CurCost );
                
    int                   m_runId;
    
                std::string                                             m_ContextName;
                std::vector<double>             m_Selected;
                int                                                                             m_nbBiased;
                int                                                                             m_nbReallyBiased;
    
    double                m_currentCost;
                
                double                                                          m_time;
                
    bool                  m_useAutoStep;
                double                                                          m_step;
                
    std::vector< std::pair<double,std::vector<confPtr_t> > > m_convergence_trajs;
    std::vector< std::pair<double,TrajectoryStatistics> > m_convergence_rate;
                std::vector<double>             m_OptimCost;
                std::vector<double>             m_GainCost;
                
    int                   m_Iteration;
                bool                                                                    m_IterationSucceded;
                std::vector<double>             m_GainOfIterations;
                unsigned int                                    m_MaxNumberOfIterations;
                
        private:
                std::vector<uint>                       m_IdSorted;
                bool                                                                    m_ShortCutBiased;
        };
}

#endif /* SHORTCUT_HPP_ */