costOptimization.hpp

/*
 * costOptimization.hpp
 *
 *  Created on: Jun 25, 2009
 *      Author: jmainpri
 */

#ifndef COST_OPTIMIZATION_HPP_
#define COST_OPTIMIZATION_HPP_

#include "API/ConfigSpace/configuration.hpp"
#include "planner/TrajectoryOptim/Classic/smoothing.hpp"

namespace API 
{
        class CostOptimization : public Smoothing 
        {
        public:
                
                CostOptimization();
                CostOptimization(const Trajectory& T);
                CostOptimization(Robot* R,p3d_traj* t);
                
                ~CostOptimization();
    
                void printDebugInfo();
                
                bool deformInCollision() { return m_inCollision; }
                
                void setCheat() { m_cheat = true; }
    
                double getMinCost() { return m_mincost; }
    
                bool oneLoopDeform();
                
                bool oneLoopDeformRecompute();

                static double getLastDescendingConfParam( LocalPath& directionPath );
    
    bool connectConfiguration( confPtr_t q, double step );
                
                static confPtr_t perturbCurrent( confPtr_t qCurrPt, confPtr_t qRandPt, double step, bool descent );
                
                void runDeformation( int nbIteration , int idRun=0 );
    

  protected:
                
                confPtr_t cheat();

                void debugShowTraj(double lPrev,double lNext, confPtr_t qNew , int color);
    
                std::vector<confPtr_t> get3RandSuccesConfAlongTraj(
                                                                                   double& prevDistPt,
                                                                                   double& randDistPt,
                                                                                   double& nextDistPt,
                                                                                   double step);
                
                std::vector<confPtr_t> getClosestConfOnTraj(
                                                                            double& prevDistPt,
                                                                            double& randDistPt,
                                                                            double& nextDistPt,
                                                                            confPtr_t ptrConf,
                                                                            double step);
                
                
        private:
                
                bool                                                                    m_cheat;
                double                                                          m_mincost;
                unsigned int                                    m_nbErrors;
                std::vector<double>             m_Errors;
                bool                                                                    m_DeformBiased;
                bool                                                                    m_inCollision;
                bool                                                                    m_descent;
    int                   m_shortcutRatio;
                
                //      bool oneLoopShortCut(double step);
        };
}

#endif /* COST_OPTIMIZATION_HPP_ */