ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
tutorial-ibvs-4pts-wireframe-robot-afma6.cpp
1 
2 #include <visp/vpDisplayGDI.h>
3 #include <visp/vpDisplayX.h>
4 #include <visp/vpFeatureBuilder.h>
5 #include <visp/vpServo.h>
6 #include <visp/vpSimulatorAfma6.h>
7 
8 void display_trajectory(const vpImage<unsigned char> &I, std::vector<vpPoint> &point,
9  const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
10 {
11  int thickness = 3;
12  static std::vector<vpImagePoint> traj[4];
13  vpImagePoint cog;
14  for (unsigned int i=0; i<4; i++) {
15  // Project the point at the given camera position
16  point[i].project(cMo);
17  vpMeterPixelConversion::convertPoint(cam, point[i].get_x(), point[i].get_y(), cog);
18  traj[i].push_back(cog);
19  }
20  for (unsigned int i=0; i<4; i++) {
21  for (unsigned int j=1; j<traj[i].size(); j++) {
22  vpDisplay::displayLine(I, traj[i][j-1], traj[i][j], vpColor::green, thickness);
23  }
24  }
25 }
26 
27 int main()
28 {
29 #if defined(VISP_HAVE_PTHREAD)
30  vpHomogeneousMatrix cdMo(0, 0, 0.75, 0, 0, 0);
31  vpHomogeneousMatrix cMo(-0.15, 0.1, 1., vpMath::rad(-10), vpMath::rad(10), vpMath::rad(50));
32 
33  /*
34  Top view of the world frame, the camera frame and the object frame
35 
36  world, also robot base frame :
37  w_y
38  /|\
39  |
40  w_x <--
41 
42  object :
43  o_y
44  /|\
45  |
46  o_x <--
47 
48 
49  camera :
50  c_y
51  /|\
52  |
53  c_x <--
54 
55  */
56  vpHomogeneousMatrix wMo(0, 0, 1., 0, 0, 0);
57 
58  std::vector<vpPoint> point(4) ;
59  point[0].setWorldCoordinates(-0.1,-0.1, 0);
60  point[1].setWorldCoordinates( 0.1,-0.1, 0);
61  point[2].setWorldCoordinates( 0.1, 0.1, 0);
62  point[3].setWorldCoordinates(-0.1, 0.1, 0);
63 
64  vpServo task ;
67  task.setLambda(0.5);
68 
69  vpFeaturePoint p[4], pd[4] ;
70  for (int i = 0 ; i < 4 ; i++) {
71  point[i].track(cdMo);
72  vpFeatureBuilder::create(pd[i], point[i]);
73  point[i].track(cMo);
74  vpFeatureBuilder::create(p[i], point[i]);
75  task.addFeature(p[i], pd[i]);
76  }
77 
78  vpSimulatorAfma6 robot(true);
79  robot.setVerbose(true);
80 
81  // Get the default joint limits
82  vpColVector qmin = robot.getJointMin();
83  vpColVector qmax = robot.getJointMax();
84 
85  std::cout << "Robot joint limits: " << std::endl;
86  for (unsigned int i=0; i< 3; i ++)
87  std::cout << "Joint " << i << ": min " << qmin[i] << " max " << qmax[i] << " (m)" << std::endl;
88  for (unsigned int i=3; i< qmin.size(); i ++)
89  std::cout << "Joint " << i << ": min " << vpMath::deg(qmin[i]) << " max " << vpMath::deg(qmax[i]) << " (deg)" << std::endl;
90 
94  robot.set_fMo(wMo);
95  bool ret = true;
96 #if VISP_VERSION_INT > VP_VERSION_INT(2,7,0)
97  ret =
98 #endif
99  robot.initialiseCameraRelativeToObject(cMo);
100  if (ret == false)
101  return 0; // Not able to set the position
102  robot.setDesiredCameraPosition(cdMo);
103 
104  vpImage<unsigned char> Iint(480, 640, 255);
105 #if defined(VISP_HAVE_X11)
106  vpDisplayX displayInt(Iint, 700, 0, "Internal view");
107 #elif defined(VISP_HAVE_GDI)
108  vpDisplayGDI displayInt(Iint, 700, 0, "Internal view");
109 #else
110  std::cout << "No image viewer is available..." << std::endl;
111 #endif
112 
113  vpCameraParameters cam(840, 840, Iint.getWidth()/2, Iint.getHeight()/2);
114  robot.setCameraParameters(cam);
115 
116  bool start = true;
117  for ( ; ; )
118  {
119  cMo = robot.get_cMo();
120 
121  for (int i = 0 ; i < 4 ; i++) {
122  point[i].track(cMo);
123  vpFeatureBuilder::create(p[i], point[i]);
124  }
125 
126  vpDisplay::display(Iint);
127  robot.getInternalView(Iint);
128  if (!start) {
129  display_trajectory(Iint, point, cMo, cam);
130  vpDisplay::displayCharString(Iint, 40, 120, "Click to stop the servo...", vpColor::red);
131  }
132  vpDisplay::flush(Iint);
133 
134  vpColVector v = task.computeControlLaw();
136 
137  // A click to exit
138  if (vpDisplay::getClick(Iint, false))
139  break;
140 
141  if (start) {
142  start = false;
143  v = 0;
145  vpDisplay::displayCharString(Iint, 40, 120, "Click to start the servo...", vpColor::blue);
146  vpDisplay::flush(Iint);
147  vpDisplay::getClick(Iint);
148  }
149 
150  vpTime::wait(1000*robot.getSamplingTime());
151  }
152  task.kill();
153 #endif
154 }