cost_space.hpp

#ifndef COST_SPACE_HPP_INCLUDED
#define COST_SPACE_HPP_INCLUDED

#include <boost/function.hpp>
#include <map>
#include <string>

#include "API/ConfigSpace/configuration.hpp"
#include "API/ConfigSpace/localpath.hpp"
#include "API/Roadmap/graph.hpp"
#include "Logging/Logger.h"


typedef Eigen::Matrix<double, 6, 1> Vector6d;
typedef Eigen::Matrix<double, 6, 6> Matrix6d;
typedef Eigen::Matrix<double, 5, 6> Matrix56d;


enum CostSpaceDeltaStepMethod 
{
        cs_integral,
        cs_mechanical_work,
        cs_visibility,
        cs_average,
        cs_config_cost_and_dist,
        cs_boltzman_cost,
  cs_max,
    cs_average_and_max
};

enum CostSpaceResolutionMethod
{
  cs_classic,
  cs_pr2_manip
};

class CostSpace
{
    MOVE3D_STATIC_LOGGER;
public:
  CostSpace();
  
  // Get selected cost function name
  std::string getSelectedCostName();
  
  // Get All Cost Functions
  std::vector<std::string> getAllCost();

  boost::function<double(Configuration&)> getCostFunction(std::string name);
  boost::function<double(std::vector<LocalPath*>&)> getCostFunctionTrajectories(std::string name);
  
  // Select the cost function with the given name in the map
  bool setCost(std::string name);
  bool setCostTraj(std::string name);
  
  // Register a new cost function.
  void addCost(std::string name, 
               boost::function<double(Configuration&)> f);
  void addCostTrajectories(std::string name,
               boost::function<double(std::vector<LocalPath*>&)> f);

  // Delete a cost function
  void deleteCost(std::string name);
  void deleteCostTraj(std::string name);

  // Compute the cost of the configuration conf.
  double cost(Configuration& conf);

  // Compute the cost of
  double cost(LocalPath * path);

  double cost(std::vector<LocalPath*>::iterator first, std::vector<LocalPath*>::iterator last);
  double cost(std::vector<LocalPath*>& path);

  double costAggregationOverTraj(std::vector<LocalPath*> &path);
  double costAggregationOverTraj(std::vector<LocalPath*>::iterator first, std::vector<LocalPath*>::iterator last);

  // Compute the cost of
  double cost(LocalPath& path, int& nb_test);

        // Set node cost
        void setNodeCost(Node* node, Node* parent);
        
  // Initializes the Cost space motion planning problem
  void initMotionPlanning(graph* graph, node* start, node* goal);
  
  // Set DeltaStepCost
  void setDistanceMethod(CostSpaceResolutionMethod method) { m_resolution = method; }
        
  // Set DeltaStepCost
  void setDeltaStepMethod(CostSpaceDeltaStepMethod method) { m_deltaMethod = method; }
  
  // Get DeltaStepCost
  CostSpaceDeltaStepMethod getDeltaStepMethod() { return m_deltaMethod; }
        
        // Compute the delta step cost
  double deltaStepCost(double cost1, double cost2, double length);
  double edgeCost(confPtr_t from, confPtr_t to);
  double edgeCost(LocalPath lp);

  //Composite cost
  double compositeCost(Configuration &conf);
  void setCompositeCostFactor(std::string func, double factor);
  double getCompositeCostFactor(std::string func);
  doubleContainer *getCompositeCostFactorObject(std::string func);

  void initCompositeCostFactors();
  void setCompositeSumFunction(std::string f);
  std::vector<std::string> getCompositeSumFunctions(void);
  std::string getCompositeSumFunction();

  // distance start goal
  double startGoalDist_ratio() const;
  void setStartGoalDist_ratio(double startGoalDist_ratio);
  double startGoalDist_power() const;
  void setStartGoalDist_power(double startGoalDist_power);




  
protected:
  std::string mSelectedCostName;
  boost::function<double(Configuration&)> mSelectedCost;
  std::map<std::string, boost::function<double(Configuration&)> > mFunctions;

  std::string mSelectedCostTrajectoriesName;
  boost::function<double(std::vector<LocalPath*>&)>  mSelectedFunctionsTrajectories;
  std::map<std::string, boost::function<double(std::vector<LocalPath*>&)> > mFunctionsTrajectories;
  
  void getPr2ArmConfiguration( Eigen::VectorXd& x, confPtr_t q );
        double getPr2ArmDistance( Robot* robot, Eigen::VectorXd& q_i, Eigen::VectorXd& q_f );
private:
        
  // Delta
  enum CostSpaceDeltaStepMethod m_deltaMethod;
  enum CostSpaceResolutionMethod m_resolution;
  
  // Composite
  std::map<std::string , doubleContainer* > mCompositionFactors;
  enum{ COST_COMPOSITE_PLUS, COST_COMPOSITE_MAX, COST_COMPOSITE_MIN} mCompostionFunction;

  // distance start goal
  double mStartGoalDist_ratio, mStartGoalDist_power;

  double m_dmax;

    /*************************************************************************************
     ****************        FlyCrane      ***********************************************
            NOTATION:
                                     (quadrotor 1)
                                         A21
                                        .   .
                                       .     .
                                      .       .
                                     .         .
                                    B2---------B1
                                  //             \\
                                 B3               B0
                                 .\               / .
                                .  \  Platform   /   .
                               .    \           /     .
                              .      \         /       .
                             .        \       /         .
           (quadrotor 2)   A43 . . . . \     / . . . . A05  (quadrotor 0)
                                        B4==B5
     *************************************************************************************/

public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    void initializeFlyCraneCostSpace();

    Eigen::Matrix3d RotationMatrix(double orientation[]);

    void computeQuadAnchorPoints(Configuration * conf);

    double flyCraneCost(Configuration * conf);

private:
    Eigen::Vector3d B[6];

    double cableLength[6];

    double admissibleTension[6][2];

    Vector6d wrench;

    Eigen::Vector3d wrenchPoint;

    Matrix6d Ellipsoid;

    double quadMass[3];

    double quadThrust[3];

        double offsetAnchorCM[3];

        double dCMmotor[3];

    Eigen::Vector3d vB[3];

    double beta[3];

    Eigen::Vector3d quadWeight[3];
};

namespace GlobalCostSpace 
{
        void initialize();
};

double computeIntersectionWithGround(Configuration& conf);
double computeFlatCost(Configuration& conf);
double computeClearance(Configuration& conf);
double computeOBBClearance(Configuration& conf);
double computeClearanceAll(Configuration& conf);
double computeDistanceToObstacles(Configuration& conf);
double computeARCAScost(Configuration& conf);
double computeInCollisionCost(Configuration& conf);
double computeCollisionSpaceCost(Configuration& conf);
double computeLocalpathKinematicCost(p3d_rob* rob, p3d_localpath* LP);
double computeHotCost(Configuration &conf);
double computeCompositeCost(Configuration &conf);
double computeHandoverCost(Configuration &conf);
double computeStartGoalMin(Configuration &conf);
double computeStraightPenalty(Configuration &conf);
double computeSaphariCost(Configuration &conf);

double computeFlyCranecost(Configuration& conf);


extern CostSpace* global_costSpace;

#endif