1 #ifndef ROBOT_SIM_VIEW_CALLBACKS_H 2 #define ROBOT_SIM_VIEW_CALLBACKS_H 4 #include <Robotics/IKFunctions.h> 5 #include <Robotics/SBLTree.h> 6 #include <Robotics/MotionPlanner.h> 7 #include <Graph/Callback.h> 9 #include <GLDraw/drawExtra.h> 14 :robot(_robot),goal(_goal)
17 virtual bool ForwardEdge(SBLTree::Node* i,SBLTree::Node* j)
20 edgeColor.setCurrentGL();
22 GLDraw::glVertex3v(EEPosition(*i));
23 GLDraw::glVertex3v(EEPosition(*j));
28 virtual void Visit(SBLTree::Node* node)
30 nodeColor.setCurrentGL();
32 GLDraw::glVertex3v(EEPosition(*node));
36 Vector3 EEPosition(
const Config& q)
const 38 robot.UpdateConfig(q);
39 return robot.links[goal.link].T_World*goal.localPosition;
44 GLDraw::GLColor nodeColor,edgeColor;
50 :robot(_robot),goal(_goal),prm(_prm)
54 void DrawEdge(
int i,
int j)
56 if(i <= 1 || j <= 1)
return;
58 edgeColor.setCurrentGL();
60 GLDraw::glVertex3v(EEPosition(prm->roadmap.nodes[i]));
61 GLDraw::glVertex3v(EEPosition(prm->roadmap.nodes[j]));
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); }
69 virtual void Visit(
int node)
72 nodeColor.setCurrentGL();
74 GLDraw::glVertex3v(EEPosition(prm->roadmap.nodes[node]));
78 Vector3 EEPosition(
const Config& q)
const 80 robot.UpdateConfig(q);
81 return robot.links[goal.link].T_World*goal.localPosition;
87 GLDraw::GLColor nodeColor,edgeColor;
93 :robot(_robot),goal(_goal)
96 virtual bool ForwardEdge(RRTPlanner::Node* i,RRTPlanner::Node* j)
99 edgeColor.setCurrentGL();
101 GLDraw::glVertex3v(EEPosition(i->x));
102 GLDraw::glVertex3v(EEPosition(j->x));
107 virtual void Visit(RRTPlanner::Node* node)
109 nodeColor.setCurrentGL();
111 GLDraw::glVertex3v(EEPosition(node->x));
115 Vector3 EEPosition(
const State& x)
const 118 q.setRef(x,0,1,robot.links.size());
119 robot.UpdateConfig(q);
120 return robot.links[goal.link].T_World*goal.localPosition;
125 GLDraw::GLColor nodeColor,edgeColor;
Definition: Callbacks.h:90
Definition: Callbacks.h:11
Definition: Callbacks.h:47