wbc
Scene.hpp
Go to the documentation of this file.
1#ifndef WBC_CORE_SCENE_HPP
2#define WBC_CORE_SCENE_HPP
3
4#include "Task.hpp"
5#include "Constraint.hpp"
7#include "RobotModel.hpp"
8#include "QPSolver.hpp"
10
11namespace wbc{
12
16class Scene{
17protected:
20 Eigen::VectorXd solver_output;
21
22public:
24 ~Scene();
25
30 virtual bool configure(const std::vector<TaskPtr> &tasks) = 0;
31
36 virtual const HierarchicalQP& update() = 0;
37
42 virtual const types::JointCommand& solve(const HierarchicalQP& hqp) = 0;
43
48
53
57 const Eigen::VectorXd& getSolverOutputRaw() const { return solver_output; }
58};
59
60typedef std::shared_ptr<Scene> ScenePtr;
61
62template<typename T,typename RobotModelPtr,typename QPSolverPtr> Scene* createT(RobotModelPtr robot_model, QPSolverPtr solver, const double dt){return new T(robot_model,solver,dt);}
63
65 typedef std::map<std::string, Scene*(*)(RobotModelPtr, QPSolverPtr, double)> SceneMap;
66
67 static Scene *createInstance(const std::string& name, RobotModelPtr robot_model, QPSolverPtr solver, const double dt) {
68 SceneMap::iterator it = getSceneMap()->find(name);
69 if(it == getSceneMap()->end())
70 throw std::runtime_error("Failed to create instance of plugin " + name + ". Is the plugin registered?");
71 return it->second(robot_model, solver, dt);
72 }
73
74 template<typename T>
75 static T* createInstance(const std::string& name, RobotModelPtr robot_model, QPSolverPtr solver, const double dt){
76 Scene* tmp = createInstance(name, robot_model, solver, dt);
77 T* ret = dynamic_cast<T*>(tmp);
78 return ret;
79 }
80
82 if(!scene_map)
83 scene_map = new SceneMap;
84 return scene_map;
85 }
86
87 static void clear(){
88 scene_map->clear();
89 }
90
91private:
92 static SceneMap *scene_map;
93};
94
95template<typename T>
97 SceneRegistry(const std::string& name) {
98 SceneMap::iterator it = getSceneMap()->find(name);
99 if(it != getSceneMap()->end())
100 throw std::runtime_error("Failed to register plugin with name " + name + ". A plugin with the same name is already registered");
101
102 getSceneMap()->insert(std::make_pair(name, &createT<T,RobotModelPtr,QPSolverPtr>));
103 }
104};
105
106} // namespace wbc
107
108#endif // WBC_CORE_SCENE_HPP
Base class for all wbc scenes.
Definition Scene.hpp:16
virtual const HierarchicalQP & update()=0
Update the wbc scene and return the (updated) optimization problem.
RobotModelPtr robot_model
Definition Scene.hpp:18
const Eigen::VectorXd & getSolverOutputRaw() const
Get current solver output in raw values.
Definition Scene.hpp:57
Scene(RobotModelPtr robot_model, QPSolverPtr solver, const double dt)
Definition Scene.cpp:9
QPSolverPtr getSolver()
Return the current solver.
Definition Scene.hpp:52
virtual bool configure(const std::vector< TaskPtr > &tasks)=0
Configure the WBC scene. Create tasks and sort them by priority given the task config.
Eigen::VectorXd solver_output
Definition Scene.hpp:20
virtual const types::JointCommand & solve(const HierarchicalQP &hqp)=0
Solve the given optimization problem.
~Scene()
Definition Scene.cpp:14
RobotModelPtr getRobotModel()
Return the current robot model.
Definition Scene.hpp:47
QPSolverPtr solver
Definition Scene.hpp:19
Definition JointCommand.hpp:8
Definition ContactsAccelerationConstraint.cpp:3
QPSolver * createT()
Definition QPSolver.hpp:35
std::shared_ptr< Scene > ScenePtr
Definition Scene.hpp:60
std::shared_ptr< QPSolver > QPSolverPtr
Definition QPSolver.hpp:33
std::shared_ptr< RobotModel > RobotModelPtr
Definition RobotModel.hpp:315
Describes a hierarchy of quadratic programs.
Definition QuadraticProgram.hpp:53
Definition Scene.hpp:64
static SceneMap * getSceneMap()
Definition Scene.hpp:81
std::map< std::string, Scene *(*)(RobotModelPtr, QPSolverPtr, double)> SceneMap
Definition Scene.hpp:65
static T * createInstance(const std::string &name, RobotModelPtr robot_model, QPSolverPtr solver, const double dt)
Definition Scene.hpp:75
static Scene * createInstance(const std::string &name, RobotModelPtr robot_model, QPSolverPtr solver, const double dt)
Definition Scene.hpp:67
static void clear()
Definition Scene.hpp:87
SceneRegistry(const std::string &name)
Definition Scene.hpp:97