Klamp't  0.9.0
Callbacks.h
1 #ifndef ROBOT_SIM_VIEW_CALLBACKS_H
2 #define ROBOT_SIM_VIEW_CALLBACKS_H
3 
4 #include <Robotics/IKFunctions.h>
5 #include <Robotics/SBLTree.h>
6 #include <Robotics/MotionPlanner.h>
7 #include <Graph/Callback.h>
8 #include <GLDraw/GL.h>
9 #include <GLDraw/drawExtra.h>
10 
11 struct DrawEECallback : public Graph::CallbackBase<SBLTree::Node*>
12 {
13  DrawEECallback(Robot& _robot,const IKGoal& _goal)
14  :robot(_robot),goal(_goal)
15  {
16  }
17  virtual bool ForwardEdge(SBLTree::Node* i,SBLTree::Node* j)
18  {
19  //const EdgePlannerPtr& e=j->edgeFromParent();
20  edgeColor.setCurrentGL();
21  glBegin(GL_LINES);
22  GLDraw::glVertex3v(EEPosition(*i));
23  GLDraw::glVertex3v(EEPosition(*j));
24  glEnd();
25  return true;
26  }
27 
28  virtual void Visit(SBLTree::Node* node)
29  {
30  nodeColor.setCurrentGL();
31  glBegin(GL_POINTS);
32  GLDraw::glVertex3v(EEPosition(*node));
33  glEnd();
34  }
35 
36  Vector3 EEPosition(const Config& q) const
37  {
38  robot.UpdateConfig(q);
39  return robot.links[goal.link].T_World*goal.localPosition;
40  }
41 
42  Robot& robot;
43  IKGoal goal;
44  GLDraw::GLColor nodeColor,edgeColor;
45 };
46 
47 struct DrawEECallback2 : public Graph::CallbackBase<int>
48 {
49  DrawEECallback2(Robot& _robot,const IKGoal& _goal,RoadmapPlanner* _prm)
50  :robot(_robot),goal(_goal),prm(_prm)
51  {
52  }
53 
54  void DrawEdge(int i,int j)
55  {
56  if(i <= 1 || j <= 1) return;
57  //const EdgePlannerPtr& e=j->edgeFromParent();
58  edgeColor.setCurrentGL();
59  glBegin(GL_LINES);
60  GLDraw::glVertex3v(EEPosition(prm->roadmap.nodes[i]));
61  GLDraw::glVertex3v(EEPosition(prm->roadmap.nodes[j]));
62  glEnd();
63  }
64 
65  virtual bool ForwardEdge(int i,int j) { DrawEdge(i,j); return true; }
66  virtual void CrossEdge(int i,int j) { DrawEdge(i,j); }
67  virtual void BackEdge(int i,int j) { DrawEdge(i,j); }
68 
69  virtual void Visit(int node)
70  {
71  if(node <= 1) return;
72  nodeColor.setCurrentGL();
73  glBegin(GL_POINTS);
74  GLDraw::glVertex3v(EEPosition(prm->roadmap.nodes[node]));
75  glEnd();
76  }
77 
78  Vector3 EEPosition(const Config& q) const
79  {
80  robot.UpdateConfig(q);
81  return robot.links[goal.link].T_World*goal.localPosition;
82  }
83 
84  Robot& robot;
85  IKGoal goal;
86  RoadmapPlanner* prm;
87  GLDraw::GLColor nodeColor,edgeColor;
88 };
89 
90 struct DrawEECallback3 : public Graph::CallbackBase<RRTPlanner::Node*>
91 {
92  DrawEECallback3(Robot& _robot,const IKGoal& _goal)
93  :robot(_robot),goal(_goal)
94  {
95  }
96  virtual bool ForwardEdge(RRTPlanner::Node* i,RRTPlanner::Node* j)
97  {
98  //const EdgePlannerPtr& e=j->edgeFromParent();
99  edgeColor.setCurrentGL();
100  glBegin(GL_LINES);
101  GLDraw::glVertex3v(EEPosition(i->x));
102  GLDraw::glVertex3v(EEPosition(j->x));
103  glEnd();
104  return true;
105  }
106 
107  virtual void Visit(RRTPlanner::Node* node)
108  {
109  nodeColor.setCurrentGL();
110  glBegin(GL_POINTS);
111  GLDraw::glVertex3v(EEPosition(node->x));
112  glEnd();
113  }
114 
115  Vector3 EEPosition(const State& x) const
116  {
117  Vector q;
118  q.setRef(x,0,1,robot.links.size());
119  robot.UpdateConfig(q);
120  return robot.links[goal.link].T_World*goal.localPosition;
121  }
122 
123  Robot& robot;
124  IKGoal goal;
125  GLDraw::GLColor nodeColor,edgeColor;
126 };
127 
128 #endif
Definition: Callbacks.h:90
Definition: Callbacks.h:11
Definition: Callbacks.h:47