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
21public:
23 ~Scene();
24
29 virtual bool configure(const std::vector<TaskPtr> &tasks) = 0;
30
35 virtual const HierarchicalQP& update() = 0;
36
41 virtual const types::JointCommand& solve(const HierarchicalQP& hqp) = 0;
42
47
52};
53
54typedef std::shared_ptr<Scene> ScenePtr;
55
56template<typename T,typename RobotModelPtr,typename QPSolverPtr> Scene* createT(RobotModelPtr robot_model, QPSolverPtr solver, const double dt){return new T(robot_model,solver,dt);}
57
59 typedef std::map<std::string, Scene*(*)(RobotModelPtr, QPSolverPtr, double)> SceneMap;
60
61 static Scene *createInstance(const std::string& name, RobotModelPtr robot_model, QPSolverPtr solver, const double dt) {
62 SceneMap::iterator it = getSceneMap()->find(name);
63 if(it == getSceneMap()->end())
64 throw std::runtime_error("Failed to create instance of plugin " + name + ". Is the plugin registered?");
65 return it->second(robot_model, solver, dt);
66 }
67
68 template<typename T>
69 static T* createInstance(const std::string& name, RobotModelPtr robot_model, QPSolverPtr solver, const double dt){
70 Scene* tmp = createInstance(name, robot_model, solver, dt);
71 T* ret = dynamic_cast<T*>(tmp);
72 return ret;
73 }
74
76 if(!scene_map)
77 scene_map = new SceneMap;
78 return scene_map;
79 }
80
81 static void clear(){
82 scene_map->clear();
83 }
84
85private:
86 static SceneMap *scene_map;
87};
88
89template<typename T>
91 SceneRegistry(const std::string& name) {
92 SceneMap::iterator it = getSceneMap()->find(name);
93 if(it != getSceneMap()->end())
94 throw std::runtime_error("Failed to register plugin with name " + name + ". A plugin with the same name is already registered");
95
96 getSceneMap()->insert(std::make_pair(name, &createT<T,RobotModelPtr,QPSolverPtr>));
97 }
98};
99
100} // namespace wbc
101
102#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
Scene(RobotModelPtr robot_model, QPSolverPtr solver, const double dt)
Definition Scene.cpp:9
QPSolverPtr getSolver()
Return the current solver.
Definition Scene.hpp:51
virtual bool configure(const std::vector< TaskPtr > &tasks)=0
Configure the WBC scene. Create tasks and sort them by priority given the task config.
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:46
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:54
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:58
static SceneMap * getSceneMap()
Definition Scene.hpp:75
std::map< std::string, Scene *(*)(RobotModelPtr, QPSolverPtr, double)> SceneMap
Definition Scene.hpp:59
static T * createInstance(const std::string &name, RobotModelPtr robot_model, QPSolverPtr solver, const double dt)
Definition Scene.hpp:69
static Scene * createInstance(const std::string &name, RobotModelPtr robot_model, QPSolverPtr solver, const double dt)
Definition Scene.hpp:61
static void clear()
Definition Scene.hpp:81
SceneRegistry(const std::string &name)
Definition Scene.hpp:91