ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
tutorial-ibvs-4pts-wireframe-robot-viper.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/vpSimulatorViper850.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 
31  vpHomogeneousMatrix cdMo(0, 0, 0.75, 0, 0, 0);
32  vpHomogeneousMatrix cMo(0.15, -0.1, 1., vpMath::rad(10), vpMath::rad(-10), vpMath::rad(50));
33 
34  /*
35  Top view of the world frame, the camera frame and the object frame
36 
37  world, also robot base frame : --> 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(vpTranslationVector(0.40, 0, -0.15),
57  vpRotationMatrix(vpRxyzVector(-M_PI, 0, M_PI/2.)));
58 
59  std::vector<vpPoint> point(4) ;
60  point[0].setWorldCoordinates(-0.1,-0.1, 0);
61  point[1].setWorldCoordinates( 0.1,-0.1, 0);
62  point[2].setWorldCoordinates( 0.1, 0.1, 0);
63  point[3].setWorldCoordinates(-0.1, 0.1, 0);
64 
65  vpServo task ;
68  task.setLambda(0.5);
69 
70  vpFeaturePoint p[4], pd[4] ;
71  for (int i = 0 ; i < 4 ; i++) {
72  point[i].track(cdMo);
73  vpFeatureBuilder::create(pd[i], point[i]);
74  point[i].track(cMo);
75  vpFeatureBuilder::create(p[i], point[i]);
76  task.addFeature(p[i], pd[i]);
77  }
78 
79  vpSimulatorViper850 robot(true);
80  robot.setVerbose(true);
81 
82  // Enlarge the default joint limits
83  vpColVector qmin = robot.getJointMin();
84  vpColVector qmax = robot.getJointMax();
85  qmin[0] = -vpMath::rad(180);
86  qmax[0] = vpMath::rad(180);
87  qmax[1] = vpMath::rad(0);
88  qmax[2] = vpMath::rad(270);
89  qmin[4] = -vpMath::rad(180);
90  qmax[4] = vpMath::rad(180);
91 
92  robot.setJointLimit(qmin, qmax);
93 
94  std::cout << "Robot joint limits: " << std::endl;
95  for (unsigned int i=0; i< qmin.size(); i ++)
96  std::cout << "Joint " << i << ": min " << vpMath::deg(qmin[i]) << " max " << vpMath::deg(qmax[i]) << " (deg)" << std::endl;
97 
101  robot.set_fMo(wMo);
102  bool ret = true;
103 #if VISP_VERSION_INT > VP_VERSION_INT(2,7,0)
104  ret =
105 #endif
106  robot.initialiseCameraRelativeToObject(cMo);
107  if (ret == false)
108  return 0; // Not able to set the position
109  robot.setDesiredCameraPosition(cdMo);
110  // We modify the default external camera position
111  robot.setExternalCameraPosition(vpHomogeneousMatrix(vpTranslationVector(-0.4, 0.4, 2),
112  vpRotationMatrix(vpRxyzVector(M_PI/2,0,0))));
113 
114  vpImage<unsigned char> Iint(480, 640, 255);
115 #if defined(VISP_HAVE_X11)
116  vpDisplayX displayInt(Iint, 700, 0, "Internal view");
117 #elif defined(VISP_HAVE_GDI)
118  vpDisplayGDI displayInt(Iint, 700, 0, "Internal view");
119 #else
120  std::cout << "No image viewer is available..." << std::endl;
121 #endif
122 
123  vpCameraParameters cam(840, 840, Iint.getWidth()/2, Iint.getHeight()/2);
124  // Modify the camera parameters to match those used in the other simulations
125  robot.setCameraParameters(cam);
126 
127  bool start = true;
128  //for ( ; ; )
129  for (int iter =0; iter < 275; iter ++)
130  {
131  cMo = robot.get_cMo();
132 
133  for (int i = 0 ; i < 4 ; i++) {
134  point[i].track(cMo);
135  vpFeatureBuilder::create(p[i], point[i]);
136  }
137 
138  vpDisplay::display(Iint);
139  robot.getInternalView(Iint);
140  if (!start) {
141  display_trajectory(Iint, point, cMo, cam);
142  vpDisplay::displayCharString(Iint, 40, 120, "Click to stop the servo...", vpColor::red);
143  }
144  vpDisplay::flush(Iint);
145 
146  vpColVector v = task.computeControlLaw();
148 
149  // A click to exit
150  if (vpDisplay::getClick(Iint, false))
151  break;
152 
153  if (start) {
154  start = false;
155  v = 0;
157  vpDisplay::displayCharString(Iint, 40, 120, "Click to start the servo...", vpColor::blue);
158  vpDisplay::flush(Iint);
159  //vpDisplay::getClick(Iint);
160  }
161 
162  vpTime::wait(1000*robot.getSamplingTime());
163  }
164  task.kill();
165 #endif
166 }