configuration.hpp

#ifndef CONFIGURATION_HPP
#define CONFIGURATION_HPP



#include <Eigen/Core>
#define EIGEN_USE_NEW_STDVECTOR
#include <Eigen/Geometry>
#include <Eigen/StdVector>
#include <tr1/memory>

class Robot;

class Configuration {
        
public:
  //constructor and destructor
        //    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        
        Configuration(Robot* R);
        
        Configuration(Robot* R, double* C, bool noCopy = false);
        
        Configuration(const Configuration& conf);
        
        ~Configuration();
  
  Configuration & operator= (const Configuration & other);
        
        double& at(const int &i) { return _Configuration[i]; }
        
        double& operator [] ( const int &i ) const { return _Configuration[i]; }
        
        double& operator () ( const int &i ) const { return _Configuration[i]; }
        
        void Clear();
        
  //Accessors
        Robot* getRobot();
        
        Eigen::Quaterniond getQuaternion();
        
        void setQuaternionsToEuler();
        
        double* getConfigStruct();
  
        double* getConfigStructCopy();
        
        void setConfiguration(double* C);
        void setConfiguration(Configuration& C);
        
        //    bool isQuatInit();
        
        //    void initQuaternions();
        
        //    void initQuaternions(int quatDof,Eigen::Quaternion<double> quat);
        
        void convertToRadian();
        
        std::tr1::shared_ptr<Configuration> getConfigInDegree();
        
        double dist(Configuration& Conf, bool print=false);
        
        double dist(Configuration& q, int distChoice);
        
        double distance(Configuration * q);

        double squareDistance(Configuration * q);

        bool isInCollision();
        
        bool isOutOfBounds(bool print = false);
  
        void adaptCircularJointsLimits();
        
        void setAsNotTested();
        
        
        double distEnv();
        
        double getActiveDoF(unsigned int ith);
        
        void setActiveDoF(unsigned int ith, double value);
        
        bool equal(Configuration& Conf, bool print=false);
        
        bool operator==(Configuration& Conf) { return this->equal(Conf); }
        
        bool operator!=(Configuration& Conf) { return !(this->equal(Conf)); }
        
        std::tr1::shared_ptr<Configuration> copy();
        
        void copyPassive(Configuration& C);
        
        double cost();
        
        void setCostAsNotTested();
        
        bool setConstraintsWithSideEffect();
        
        bool setConstraints();
        
#ifdef LIGHT_PLANNER

        Eigen::Vector3d getTaskPos();
#endif
        
        std::tr1::shared_ptr<Configuration> add(Configuration& C);
        
        std::tr1::shared_ptr<Configuration> operator+(Configuration& Conf) { return this->add(Conf); }
        
        std::tr1::shared_ptr<Configuration> sub(Configuration& C);
        
        std::tr1::shared_ptr<Configuration> operator-(Configuration& Conf) { return this->sub(Conf); }
        
        Configuration& mult(double coeff);
        
        Configuration& operator*(double coeff) { return this->mult(coeff); }
        
        Eigen::VectorXd getEigenVector(); 
  Eigen::VectorXd getEigenVector(const int& startIndex, const int& endIndex); 
        
        void setFromEigenVector(const Eigen::VectorXd& conf);
  void setFromEigenVector(const Eigen::VectorXd& conf, const int& startIndex, const int& endIndex);
        
        void print(bool withPassive = false);

    void printCompare(Configuration *conf,bool withPassive = false);
        
        
private:
        bool _flagInitQuaternions;
        int _QuatDof;
        //        Eigen::Quaterniond _Quaternions;
        
        bool _CollisionTested;
        bool _InCollision;
        
        bool _CostTested;
        double _Cost;
        
        Robot* _Robot; 
        double* _Configuration;
        /*************************************************************************************
         ****************        FlyCrane      ***********************************************
         *************************************************************************************/

public:
    void initializeFlyCraneConfiguration();

    Eigen::Vector3d getPosition() const { return position; }

    double * getOrientation() { return orientation; }

    double * getDihedralAngles() { return dihedralAngles; }

    Eigen::Vector3d * getQuadCMs() { return CM; }

    Eigen::Vector3d * getQuadAnchors() { return A; }

    bool getAssignedQuadAnchors() const { return assignedQuadAnchors; }

    double * getMaximalTensions() { return maximalTensions; }

    double * getMinimalTensions() { return minimalTensions; }

    double * getQuadForces() { return quadForces; }

    void setCentersOfMass(Eigen::Vector3d * centersOfMass);

    void setQuadAnchors(Eigen::Vector3d * anchors);

    void setTensionsAndForces(double maxTensions[], double minTensions[], double forces[]);

    bool isConnectible();

private:
    Eigen::Vector3d position;

    double orientation[3];

    double dihedralAngles[3];

    Eigen::Vector3d CM[3];

    Eigen::Vector3d A[3];

    bool assignedQuadAnchors;

    double maximalTensions[6];

    double minimalTensions[6];

    double quadForces[3];
};

typedef std::tr1::shared_ptr<Configuration> confPtr_t;

#endif