Scene.hpp

Go to the documentation of this file.

#ifndef Scene_HPP
#define Scene_HPP
 
#include "TaskStatus.hpp"
#include "Constraint.hpp"
#include "QuadraticProgram.hpp"
#include "RobotModel.hpp"
#include "QPSolver.hpp"
#include "SceneConfig.hpp"
 
namespace wbc{
 
class Scene{
protected:
    RobotModelPtr robot_model;
    QPSolverPtr solver;
    std::vector< std::vector<TaskPtr> > tasks;
    std::vector< std::vector<ConstraintPtr> > constraints;
    TasksStatus tasks_status;
    HierarchicalQP hqp;
    std::vector<int> n_task_variables_per_prio;
    bool configured;
    base::commands::Joints solver_output_joints;
    JointWeights joint_weights, actuated_joint_weights;
    std::vector<TaskConfig> wbc_config;
    base::VectorXd solver_output;
 
    virtual TaskPtr createTask(const TaskConfig &config) = 0;
 
    void clearTasks();
 
public:
    Scene(RobotModelPtr robot_model, QPSolverPtr solver, const double dt);
    ~Scene();
 
    virtual bool configure(const std::vector<TaskConfig> &config);
 
    virtual const HierarchicalQP& update() = 0;
 
    virtual const base::commands::Joints& solve(const HierarchicalQP& hqp) = 0;
 
    void setReference(const std::string& task_name, const base::samples::Joints& ref);
 
    void setReference(const std::string& task_name, const base::samples::RigidBodyStateSE3& ref);
 
    void setTaskWeights(const std::string& task_name, const base::VectorXd &weights);
    void setTaskActivation(const std::string& task_name, double activation);
    TaskPtr getTask(const std::string& name);
 
    bool hasTask(const std::string& name);
 
    const TasksStatus& getTasksStatus() const { return tasks_status; }
 
    static void sortTaskConfig(const std::vector<TaskConfig>& config, std::vector< std::vector<TaskConfig> >& sorted_config);
 
    static uint getNTaskVariables(const std::vector<TaskConfig>& config);
 
    static std::vector<int> getNTaskVariablesPerPrio(const std::vector<TaskConfig>& config);
 
    virtual const TasksStatus& updateTasksStatus() = 0;
 
    void getHierarchicalQP(HierarchicalQP& _hqp){_hqp = hqp;}
 
    const base::commands::Joints& getSolverOutput() const { return solver_output_joints; }
 
    const base::VectorXd& getSolverOutputRaw() const { return solver_output; }
 
    void setJointWeights(const JointWeights& weights);
 
    const JointWeights& getJointWeights() const { return joint_weights; }
 
    const JointWeights& getActuatedJointWeights() const { return actuated_joint_weights; }
 
    RobotModelPtr getRobotModel(){return robot_model;}
 
    QPSolverPtr getSolver(){return solver;}
 
    std::vector<TaskConfig> getWbcConfig(){return wbc_config;}
 
};
 
typedef std::shared_ptr<Scene> ScenePtr;
 
template<typename T,typename RobotModelPtr,typename QPSolverPtr> Scene* createT(RobotModelPtr robot_model, QPSolverPtr solver, const double dt){return new T(robot_model,solver,dt);}
 
struct SceneFactory{
    typedef std::map<std::string, Scene*(*)(RobotModelPtr, QPSolverPtr, double)> SceneMap;
 
    static Scene *createInstance(const std::string& name, RobotModelPtr robot_model, QPSolverPtr solver, const double dt) {
        SceneMap::iterator it = getSceneMap()->find(name);
        if(it == getSceneMap()->end())
            throw std::runtime_error("Failed to create instance of plugin " + name + ". Is the plugin registered?");
        return it->second(robot_model, solver, dt);
    }
 
    template<typename T>
    static T* createInstance(const std::string& name, RobotModelPtr robot_model, QPSolverPtr solver, const double dt){
        Scene* tmp = createInstance(name, robot_model, solver, dt);
        T* ret = dynamic_cast<T*>(tmp);
        return ret;
    }
 
    static SceneMap *getSceneMap(){
        if(!scene_map)
            scene_map = new SceneMap;
        return scene_map;
    }
 
    static void clear(){
        scene_map->clear();
    }
 
private:
    static SceneMap *scene_map;
};
 
template<typename T>
struct SceneRegistry : SceneFactory{
    SceneRegistry(const std::string& name) {
        SceneMap::iterator it = getSceneMap()->find(name);
        if(it != getSceneMap()->end())
            throw std::runtime_error("Failed to register plugin with name " + name + ". A plugin with the same name is already registered");
 
        getSceneMap()->insert(std::make_pair(name, &createT<T,RobotModelPtr,QPSolverPtr>));
    }
};
 
} // namespace wbc
 
#endif