Scene.hpp

Go to the documentation of this file.

#ifndef WBC_CORE_SCENE_HPP
#define WBC_CORE_SCENE_HPP
 
#include "Task.hpp"
#include "Constraint.hpp"
#include "QuadraticProgram.hpp"
#include "RobotModel.hpp"
#include "QPSolver.hpp"
#include "../types/JointCommand.hpp"
 
namespace wbc{

class Scene{
protected:
    RobotModelPtr robot_model;
    QPSolverPtr solver;
    Eigen::VectorXd solver_output;
    double hessian_regularizer;
    std::vector<types::Wrench> contact_wrenches;
 
public:
    Scene(RobotModelPtr robot_model, QPSolverPtr solver, const double dt);
    ~Scene();

    virtual bool configure(const std::vector<TaskPtr> &tasks) = 0;

    virtual const HierarchicalQP& update() = 0;

    virtual const types::JointCommand& solve(const HierarchicalQP& hqp) = 0;

    RobotModelPtr getRobotModel(){return robot_model;}

    QPSolverPtr getSolver(){return solver;}

    const Eigen::VectorXd& getSolverOutputRaw() const { return solver_output; }

    void setHessianRegularizer(const double reg){hessian_regularizer=reg;}

    double getHessianRegularizer(){return hessian_regularizer;}

    const std::vector<types::Wrench>& getContactWrenches(){return contact_wrenches;}
};
 
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 // WBC_CORE_SCENE_HPP