HRICS_Natural.hpp

/*
 *  HRICS_Natural.hpp
 *  BioMove3D
 *
 *  Created by Jim Mainprice on 27/04/10.
 *  Copyright 2010 LAAS/CNRS. All rights reserved.
 *
 */

#ifndef HRICS_NATURAL_HPP
#define HRICS_NATURAL_HPP

#include "API/planningAPI.hpp"

#include "HRI_costspace/Grid/HRICS_NaturalGrid.hpp"
#include "HRI_costspace/Grid/HRICS_NaturalCell.hpp"

//#include "HRI_costspace/Grid/HRICS_TwoDGrid.hpp"


#ifdef HRI_PLANNER
#include <libmove3d/hri/hri.h>
#endif

namespace HRICS
{
        class Natural 
        {
        public:
                Natural();
                Natural(Robot* R);
                ~Natural();
                
                void initGeneral();
                void initNaturalJustin();
                void initNaturalAchile();
                void initNaturalHerakles();
                void initNaturalOldDude();
                void initHumanBaseGrid(std::vector<double> box);
                
                void printBodyPos();
    void setRobotToConfortPosture();
                
                double getConfigCost();
                double getCost(const Eigen::Vector3d& WSPoint, bool useLeftvsRightArm, bool withEffect = false);
                
    double getWorkspaceCost(const Eigen::Vector3d& WSPoint);
    bool getWorkspaceIsReachable(const Eigen::Vector3d& WSPoint);
    
                double getJointDisplacement();
                double getEnergy();
                double getDiscomfort();
                
                double basicNaturalArmCost(bool useLeftvsRightArm);
                void setRobotColorFromConfiguration(bool toSet);
    
                /*
                double akinRightArmReachCost();
                double akinLeftArmReachCost();
                 */
                std::vector<double> getUpperBodyHeigth(bool useReference = true);
                double getCustomDistConfig(Configuration& q);
                double getJointLimits(Configuration& q);
                
                double getNumberOfIKCost(const Eigen::Vector3d& WSPoint);
                
    bool computeIsReachableAndMove( const Eigen::Vector3d& WSPoint, bool leftArm);
                

    bool computeIsReachableOnly(const Eigen::Vector3d& WSPoint, bool leftArm);



                Eigen::Affine3d getGridOriginMatrix();

                NaturalGrid* computeNaturalGrid();
                void computeAllCellCost();
                void computeAllReachableCellCost();
                std::vector< Eigen::Vector3d > getSortedReachableWSPoint();
                std::vector< std::pair<double,Eigen::Vector3d> > getReachableWSPoint();
                
                int getObjectDof() { return m_IndexObjectDof; }
                bool IsHuman() { return m_IsHuman; }
    Robot* getRobot() { return m_Robot; }
                NaturalGrid* getGrid() { return m_Grid; }
                
                void setGrid(NaturalGrid* grid) { m_Grid = grid; }
                
        private:
                bool                m_debug;
                
                int                 m_IndexObjectDof;
                
                bool                m_computeNbOfIK;
                
                bool                m_leftArmCost;
                
                bool                m_BestPointsSorted;
                
                Robot*              m_Robot;
    
                NaturalGrid*        m_Grid;
                
                enum Kinematic 
                {
                        Default,
                        Justin,
                        Achile,
                        Herakles,
                        OldDude
                };
                
                bool                    m_IsHuman;
                
#ifdef HRI_PLANNER
                HRI_AGENTS*             m_Agents;
#endif
                
                Kinematic               m_KinType;
                
                /***********************************************/
                int m_JOINT_SPINE;
                int m_JOINT_HEAD;
                
                int m_JOINT_ARM_RIGTH_SHOULDER;
                int m_JOINT_ARM_RIGTH_ELBOW;
                int m_JOINT_ARM_RIGTH_WRIST;
                
                int m_JOINT_ARM_LEFT_SHOULDER;
                int m_JOINT_ARM_LEFT_ELBOW;
                int m_JOINT_ARM_LEFT_WRIST; 
                
                /***********************************************/
                int m_CONFIG_INDEX_SPINE;
                int m_CONFIG_INDEX_HEAD;
                
                int m_CONFIG_INDEX_ARM_RIGTH_SHOULDER;
                int m_CONFIG_INDEX_ARM_RIGTH_ELBOW;
                int m_CONFIG_INDEX_ARM_RIGTH_WRIST;
                
                int m_CONFIG_INDEX_ARM_LEFT_SHOULDER;
                int m_CONFIG_INDEX_ARM_LEFT_ELBOW;
                int m_CONFIG_INDEX_ARM_LEFT_WRIST;
                
                
                std::tr1::shared_ptr<Configuration> m_q_Confort;
                
                std::tr1::shared_ptr<Configuration> m_q_ConfortWeigths;
                
                double                                                          m_G;
                
                std::vector<double>                                     m_mg;
    
    std::vector<double>        m_armHeightL;
    std::vector<double>        m_armHeightR;
                
                std::tr1::shared_ptr<Configuration> m_q_Init;
                std::tr1::shared_ptr<Configuration> m_q_Goal;
                
                std::vector< NaturalCell* >                     m_SortedCells;
        };
}

#endif