1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
|
/*
* Scene.hpp
*
* Created on: Jan 5, 2009
* Author: rubensmits
*/
#ifndef SCENE_HPP_
#define SCENE_HPP_
#include "eigen_types.hpp"
#include "WorldObject.hpp"
#include "ConstraintSet.hpp"
#include "Solver.hpp"
#include <map>
namespace iTaSC {
class SceneLock;
class Scene {
friend class SceneLock;
public:
enum SceneParam {
MIN_TIMESTEP = 0,
MAX_TIMESTEP,
COUNT
};
Scene();
virtual ~Scene();
bool addObject(const std::string& name, Object* object, UncontrolledObject* base=&Object::world, const std::string& baseFrame="");
bool addConstraintSet(const std::string& name, ConstraintSet* task,const std::string& object1,const std::string& object2,const std::string& ee1="",const std::string& ee2="");
bool addSolver(Solver* _solver);
bool addCache(Cache* _cache);
bool initialize();
bool update(const struct EvaluationContext *eval_ctx, double timestamp, double timestep, unsigned int numsubstep=1, bool reiterate=false, bool cache=true, bool interpolate=true);
bool setParam(SceneParam paramId, double value);
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
e_matrix m_A,m_B,m_Atemp,m_Wq,m_Jf,m_Jq,m_Ju,m_Cf,m_Cq,m_Jf_inv;
e_matrix6 m_Vf,m_Uf;
e_vector m_Wy,m_ydot,m_qdot,m_xdot;
e_vector6 m_Sf,m_tempf;
double m_minstep;
double m_maxstep;
unsigned int m_ncTotal,m_nqTotal,m_nuTotal,m_nsets;
std::vector<bool> m_ytask;
Solver* m_solver;
Cache* m_cache;
struct Object_struct{
Object* object;
UncontrolledObject* base;
unsigned int baseFrameIndex;
Range constraintrange;
Range jointrange;
Range coordinaterange; // Xu range of base when object is controlled
// Xu range of object when object is uncontrolled
Object_struct(Object* _object,UncontrolledObject* _base,unsigned int _baseFrameIndex,Range nq_range,Range nc_range,Range nu_range):
object(_object),base(_base),baseFrameIndex(_baseFrameIndex),constraintrange(nc_range),jointrange(nq_range),coordinaterange(nu_range)
{};
};
typedef std::map<std::string,Object_struct*> ObjectMap;
struct ConstraintSet_struct{
ConstraintSet* task;
ObjectMap::iterator object1;
ObjectMap::iterator object2;
Range constraintrange;
Range featurerange;
unsigned int ee1index;
unsigned int ee2index;
ConstraintSet_struct(ConstraintSet* _task,
ObjectMap::iterator _object1,unsigned int _ee1index,
ObjectMap::iterator _object2,unsigned int _ee2index,
Range nc_range,Range coord_range):
task(_task),
object1(_object1),object2(_object2),
constraintrange(nc_range),featurerange(coord_range),
ee1index(_ee1index), ee2index(_ee2index)
{};
};
typedef std::map<std::string,ConstraintSet_struct*> ConstraintMap;
ObjectMap objects;
ConstraintMap constraints;
static bool getConstraintPose(ConstraintSet* constraint, void *_param, KDL::Frame& _pose);
};
}
#endif /* SCENE_HPP_ */
|
