robot.hpp

#ifndef ROBOT_HPP
#define ROBOT_HPP

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

#include "API/Device/joint.hpp"
#include "API/Device/objectrob.hpp"
#include "API/ConfigSpace/configuration.hpp"
//#ifdef HRI_PLANNER
#include <libmove3d/hri/hri.h>
//#endif


class Scene;
class ObjectRob;

namespace API { class Trajectory; }

#ifndef _DEVICE_H
struct rob;
struct jnt;
#endif

#ifndef _TRAJ_H
struct traj;
#endif

class Robot {
        
public:
  //constructor and destructor
        Robot(rob* R , bool copy = false );
        
        virtual ~Robot();
        
        rob* copyRobotStruct(rob* robotPt);
        
  //Accessor
        rob* getRobotStruct();
        
        std::string getName();

    HRI_AGENT* getHriAgent();

    void setHriAgent(HRI_AGENT* ag);

    ObjectRob* getObjectRob();

    void setObjectRob(ObjectRob* ore);
        
        void setActiveScene(Scene* sc) { m_ActiveScene = sc; }
        
        Scene* getActiveScene() { return m_ActiveScene; }
        
        traj* getTrajStruct();
        
        API::Trajectory getCurrentTraj();
        
        unsigned int getNumberOfJoints();
        
        Joint* getJoint(unsigned int i);
  
        Joint* getJoint(std::string name);
  
  const std::vector<Joint*>& getAllJoints();
        
        std::vector<Eigen::Vector3d> getObjectBox();
        
        void initObjectBox();
        
    std::tr1::shared_ptr<Configuration> shootQuadConf();

        std::tr1::shared_ptr<Configuration> shoot(bool samplePassive = false);

    std::tr1::shared_ptr<Configuration> shootDirections(std::tr1::shared_ptr<Configuration> from , int nactive, int scale=1);
        
        std::tr1::shared_ptr<Configuration> shootDir(bool samplePassive = false);
        
        std::tr1::shared_ptr<Configuration> shootFreeFlyer(double* box);
  
        int setAndUpdateFreeFlyer(const Eigen::Vector3d& pos);
        
        int setAndUpdate(Configuration& q, bool withoutFreeFlyers = false);
        
        bool setAndUpdateMultiSol(Configuration& q);
        
        void setAndUpdateWithoutConstraints(Configuration& q);
        
        bool setAndUpdateHumanArms(Configuration& q);
        
        std::tr1::shared_ptr<Configuration> getInitialPosition();
        
        bool isInCollision();

        bool isInCollisionWithOthersAndEnv();
        
        void setInitialPosition(Configuration& conf);
        
        std::tr1::shared_ptr<Configuration> getGoTo();
        
        void setGoTo(Configuration& conf);
        
        std::tr1::shared_ptr<Configuration> getCurrentPos();
        
        std::tr1::shared_ptr<Configuration> getNewConfig();
        
    Eigen::Matrix4d getJointAbsPos(int id);
        
        Eigen::Vector3d getJointPos(int id);
        
         unsigned int getNumberOfActiveDoF();
        
        Joint* getIthActiveDoFJoint(unsigned int ithActiveDoF , unsigned int& ithDofOnJoint  );
        
        void getListOfPassiveJoints(std::list<Joint *> & passiveJoints);

        unsigned getNbConf() const { return _Robot->nconf; }

        confPtr_t getConf(unsigned i) {
                confPtr_t conf(new Configuration(this, _Robot->conf[i]->q));
                return conf;
        }

#ifdef LIGHT_PLANNER

        int getObjectDof();
        
        bool isActiveCcConstraint();
        
        void activateCcConstraint();
        
        void deactivateCcConstraint();
        
        jnt* getBaseJnt();

    unsigned int getBaseJointIndex();

        std::tr1::shared_ptr<Configuration> shootRandomDirection();
        
        std::tr1::shared_ptr<Configuration> shootBase();
        
        std::tr1::shared_ptr<Configuration> shootBaseWithoutCC();
        
        std::tr1::shared_ptr<Configuration> shootAllExceptBase();
        
        bool setAndUpdateAllExceptBase(Configuration& Conf);
        
        void shootObjectJoint(Configuration& Conf);

    bool isCarryingObject(int armId);
#endif

    bool isAgent(){return isAnAgent;}
    void isAgent(bool isAg){isAnAgent = isAg;}

    void attachObj(Robot* obj, int jointId);
    void dettachObj(Robot* obj);
    void dettachObj(int jointId);
    bool isObjectAttached();
    bool isObjectAttached(Robot* obj, int jointId);
    Joint* whereObjectAttached(Robot* obj);
    p3d_rob *whichObjectAttached(int jointId);
    p3d_rob* isAttached();

private:
        rob* _Robot; 
    HRI_AGENT* _agent;
    bool isAnAgent;
    ObjectRob* _objectRob;
        std::string _Name; 
        Scene* m_ActiveScene;
        bool _copy; 
        std::vector<Joint*> m_Joints;
        
        Eigen::Vector3d m_ObjectBoxCenter;
        Eigen::Vector3d m_ObjectBoxDimentions;
};

extern Robot* API_activeRobot;

#endif