ViSP  2.10.0
tutorial-ibvs-4pts-wireframe-robot-viper.cpp
1 
2 #include <visp/vpDisplayGDI.h>
3 #include <visp/vpDisplayOpenCV.h>
4 #include <visp/vpDisplayX.h>
5 #include <visp/vpFeatureBuilder.h>
6 #include <visp/vpServo.h>
7 #include <visp/vpSimulatorViper850.h>
8 
9 void display_trajectory(const vpImage<unsigned char> &I, std::vector<vpPoint> &point,
10  const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam);
11 
12 void display_trajectory(const vpImage<unsigned char> &I, std::vector<vpPoint> &point,
13  const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
14 {
15  unsigned int thickness = 3;
16  static std::vector<vpImagePoint> traj[4];
17  vpImagePoint cog;
18  for (unsigned int i=0; i<4; i++) {
19  // Project the point at the given camera position
20  point[i].project(cMo);
21  vpMeterPixelConversion::convertPoint(cam, point[i].get_x(), point[i].get_y(), cog);
22  traj[i].push_back(cog);
23  }
24  for (unsigned int i=0; i<4; i++) {
25  for (unsigned int j=1; j<traj[i].size(); j++) {
26  vpDisplay::displayLine(I, traj[i][j-1], traj[i][j], vpColor::green, thickness);
27  }
28  }
29 }
30 
31 int main()
32 {
33 #if defined(VISP_HAVE_PTHREAD)
34  try {
35  vpHomogeneousMatrix cdMo(0, 0, 0.75, 0, 0, 0);
36  vpHomogeneousMatrix cMo(0.15, -0.1, 1., vpMath::rad(10), vpMath::rad(-10), vpMath::rad(50));
37 
38  /*
39  Top view of the world frame, the camera frame and the object frame
40 
41  world, also robot base frame : --> w_y
42  |
43  \|/
44  w_x
45 
46  object :
47  o_y
48  /|\
49  |
50  o_x <--
51 
52 
53  camera :
54  c_y
55  /|\
56  |
57  c_x <--
58 
59  */
60  vpHomogeneousMatrix wMo(vpTranslationVector(0.40, 0, -0.15),
61  vpRotationMatrix(vpRxyzVector(-M_PI, 0, M_PI/2.)));
62 
63  std::vector<vpPoint> point(4) ;
64  point[0].setWorldCoordinates(-0.1,-0.1, 0);
65  point[1].setWorldCoordinates( 0.1,-0.1, 0);
66  point[2].setWorldCoordinates( 0.1, 0.1, 0);
67  point[3].setWorldCoordinates(-0.1, 0.1, 0);
68 
69  vpServo task ;
72  task.setLambda(0.5);
73 
74  vpFeaturePoint p[4], pd[4] ;
75  for (unsigned int i = 0 ; i < 4 ; i++) {
76  point[i].track(cdMo);
77  vpFeatureBuilder::create(pd[i], point[i]);
78  point[i].track(cMo);
79  vpFeatureBuilder::create(p[i], point[i]);
80  task.addFeature(p[i], pd[i]);
81  }
82 
83  vpSimulatorViper850 robot(true);
84  robot.setVerbose(true);
85 
86  // Enlarge the default joint limits
87  vpColVector qmin = robot.getJointMin();
88  vpColVector qmax = robot.getJointMax();
89  qmin[0] = -vpMath::rad(180);
90  qmax[0] = vpMath::rad(180);
91  qmax[1] = vpMath::rad(0);
92  qmax[2] = vpMath::rad(270);
93  qmin[4] = -vpMath::rad(180);
94  qmax[4] = vpMath::rad(180);
95 
96  robot.setJointLimit(qmin, qmax);
97 
98  std::cout << "Robot joint limits: " << std::endl;
99  for (unsigned int i=0; i< qmin.size(); i ++)
100  std::cout << "Joint " << i << ": min " << vpMath::deg(qmin[i]) << " max " << vpMath::deg(qmax[i]) << " (deg)" << std::endl;
101 
105  robot.set_fMo(wMo);
106  bool ret = true;
107 #if VISP_VERSION_INT > VP_VERSION_INT(2,7,0)
108  ret =
109  #endif
110  robot.initialiseCameraRelativeToObject(cMo);
111  if (ret == false)
112  return 0; // Not able to set the position
113  robot.setDesiredCameraPosition(cdMo);
114  // We modify the default external camera position
115  robot.setExternalCameraPosition(vpHomogeneousMatrix(vpTranslationVector(-0.4, 0.4, 2),
116  vpRotationMatrix(vpRxyzVector(M_PI/2,0,0))));
117 
118  vpImage<unsigned char> Iint(480, 640, 255);
119 #if defined(VISP_HAVE_X11)
120  vpDisplayX displayInt(Iint, 700, 0, "Internal view");
121 #elif defined(VISP_HAVE_GDI)
122  vpDisplayGDI displayInt(Iint, 700, 0, "Internal view");
123 #elif defined(VISP_HAVE_OPENCV)
124  vpDisplayOpenCV displayInt(Iint, 700, 0, "Internal view");
125 #else
126  std::cout << "No image viewer is available..." << std::endl;
127 #endif
128 
129  vpCameraParameters cam(840, 840, Iint.getWidth()/2, Iint.getHeight()/2);
130  // Modify the camera parameters to match those used in the other simulations
131  robot.setCameraParameters(cam);
132 
133  bool start = true;
134  //for ( ; ; )
135  for (int iter =0; iter < 275; iter ++)
136  {
137  cMo = robot.get_cMo();
138 
139  for (unsigned int i = 0 ; i < 4 ; i++) {
140  point[i].track(cMo);
141  vpFeatureBuilder::create(p[i], point[i]);
142  }
143 
144  vpDisplay::display(Iint);
145  robot.getInternalView(Iint);
146  if (!start) {
147  display_trajectory(Iint, point, cMo, cam);
148  vpDisplay::displayText(Iint, 40, 120, "Click to stop the servo...", vpColor::red);
149  }
150  vpDisplay::flush(Iint);
151 
152  vpColVector v = task.computeControlLaw();
154 
155  // A click to exit
156  if (vpDisplay::getClick(Iint, false))
157  break;
158 
159  if (start) {
160  start = false;
161  v = 0;
163  vpDisplay::displayText(Iint, 40, 120, "Click to start the servo...", vpColor::blue);
164  vpDisplay::flush(Iint);
165  //vpDisplay::getClick(Iint);
166  }
167 
168  vpTime::wait(1000*robot.getSamplingTime());
169  }
170  task.kill();
171  }
172  catch(vpException e) {
173  std::cout << "Catch an exception: " << e << std::endl;
174  }
175 #endif
176 }
The object displayed at the desired position is the same than the scene object defined in vpSceneObje...
Perspective projection without distortion model.
unsigned int getWidth() const
Definition: vpImage.h:161
double getSamplingTime() const
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
void setVerbose(bool verbose)
Definition: vpRobot.h:158
A 40cm by 40cm plate with 4 points at coordinates (-0.1,-0.1,0), (0.1,-0.1,0), (0.1,0.1,0), (0.1,0.1,0). Each point is represented by a circle with 2cm radius.
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Point coordinates conversion from normalized coordinates in meter to pixel coordinates ...
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:132
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Definition: vpDisplay.cpp:887
Define the X11 console to display images.
Definition: vpDisplayX.h:152
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:449
error that can be emited by ViSP classes.
Definition: vpException.h:76
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
static int wait(double t0, double t)
Definition: vpTime.cpp:149
static const vpColor green
Definition: vpColor.h:170
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:2232
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:205
static const vpColor red
Definition: vpColor.h:167
The vpRotationMatrix considers the particular case of a rotation matrix.
unsigned int size() const
Definition: vpColVector.h:204
void kill()
Definition: vpServo.cpp:189
Initialize the velocity controller.
Definition: vpRobot.h:70
vpColVector computeControlLaw()
Definition: vpServo.cpp:902
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:210
The vpDisplayOpenCV allows to display image using the opencv library.
Generic class defining intrinsic camera parameters.
void setLambda(double c)
Definition: vpServo.h:370
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:522
static double rad(double deg)
Definition: vpMath.h:100
Simulator of Irisa's Viper S850 robot named Viper850.
static double deg(double rad)
Definition: vpMath.h:93
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
unsigned int getHeight() const
Definition: vpImage.h:152
Class that consider the case of the Euler angle using the x-y-z convention, where are respectively ...
Definition: vpRxyzVector.h:152
virtual bool getClick(bool blocking=true)=0
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:93
virtual void displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)=0
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:220
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v)
Class that consider the case of a translation vector.
static const vpColor blue
Definition: vpColor.h:173