ViSP  2.9.0
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 void display_trajectory(const vpImage<unsigned char> &I, std::vector<vpPoint> &point,
12  const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
13 {
14  unsigned int thickness = 3;
15  static std::vector<vpImagePoint> traj[4];
16  vpImagePoint cog;
17  for (unsigned int i=0; i<4; i++) {
18  // Project the point at the given camera position
19  point[i].project(cMo);
20  vpMeterPixelConversion::convertPoint(cam, point[i].get_x(), point[i].get_y(), cog);
21  traj[i].push_back(cog);
22  }
23  for (unsigned int i=0; i<4; i++) {
24  for (unsigned int j=1; j<traj[i].size(); j++) {
25  vpDisplay::displayLine(I, traj[i][j-1], traj[i][j], vpColor::green, thickness);
26  }
27  }
28 }
29 
30 int main()
31 {
32 #if defined(VISP_HAVE_PTHREAD)
33  try {
34  vpHomogeneousMatrix cdMo(0, 0, 0.75, 0, 0, 0);
35  vpHomogeneousMatrix cMo(0.15, -0.1, 1., vpMath::rad(10), vpMath::rad(-10), vpMath::rad(50));
36 
37  /*
38  Top view of the world frame, the camera frame and the object frame
39 
40  world, also robot base frame : --> w_y
41  |
42  \|/
43  w_x
44 
45  object :
46  o_y
47  /|\
48  |
49  o_x <--
50 
51 
52  camera :
53  c_y
54  /|\
55  |
56  c_x <--
57 
58  */
59  vpHomogeneousMatrix wMo(vpTranslationVector(0.40, 0, -0.15),
60  vpRotationMatrix(vpRxyzVector(-M_PI, 0, M_PI/2.)));
61 
62  std::vector<vpPoint> point(4) ;
63  point[0].setWorldCoordinates(-0.1,-0.1, 0);
64  point[1].setWorldCoordinates( 0.1,-0.1, 0);
65  point[2].setWorldCoordinates( 0.1, 0.1, 0);
66  point[3].setWorldCoordinates(-0.1, 0.1, 0);
67 
68  vpServo task ;
71  task.setLambda(0.5);
72 
73  vpFeaturePoint p[4], pd[4] ;
74  for (unsigned int i = 0 ; i < 4 ; i++) {
75  point[i].track(cdMo);
76  vpFeatureBuilder::create(pd[i], point[i]);
77  point[i].track(cMo);
78  vpFeatureBuilder::create(p[i], point[i]);
79  task.addFeature(p[i], pd[i]);
80  }
81 
82  vpSimulatorViper850 robot(true);
83  robot.setVerbose(true);
84 
85  // Enlarge the default joint limits
86  vpColVector qmin = robot.getJointMin();
87  vpColVector qmax = robot.getJointMax();
88  qmin[0] = -vpMath::rad(180);
89  qmax[0] = vpMath::rad(180);
90  qmax[1] = vpMath::rad(0);
91  qmax[2] = vpMath::rad(270);
92  qmin[4] = -vpMath::rad(180);
93  qmax[4] = vpMath::rad(180);
94 
95  robot.setJointLimit(qmin, qmax);
96 
97  std::cout << "Robot joint limits: " << std::endl;
98  for (unsigned int i=0; i< qmin.size(); i ++)
99  std::cout << "Joint " << i << ": min " << vpMath::deg(qmin[i]) << " max " << vpMath::deg(qmax[i]) << " (deg)" << std::endl;
100 
104  robot.set_fMo(wMo);
105  bool ret = true;
106 #if VISP_VERSION_INT > VP_VERSION_INT(2,7,0)
107  ret =
108  #endif
109  robot.initialiseCameraRelativeToObject(cMo);
110  if (ret == false)
111  return 0; // Not able to set the position
112  robot.setDesiredCameraPosition(cdMo);
113  // We modify the default external camera position
114  robot.setExternalCameraPosition(vpHomogeneousMatrix(vpTranslationVector(-0.4, 0.4, 2),
115  vpRotationMatrix(vpRxyzVector(M_PI/2,0,0))));
116 
117  vpImage<unsigned char> Iint(480, 640, 255);
118 #if defined(VISP_HAVE_X11)
119  vpDisplayX displayInt(Iint, 700, 0, "Internal view");
120 #elif defined(VISP_HAVE_GDI)
121  vpDisplayGDI displayInt(Iint, 700, 0, "Internal view");
122 #else
123  std::cout << "No image viewer is available..." << std::endl;
124 #endif
125 
126  vpCameraParameters cam(840, 840, Iint.getWidth()/2, Iint.getHeight()/2);
127  // Modify the camera parameters to match those used in the other simulations
128  robot.setCameraParameters(cam);
129 
130  bool start = true;
131  //for ( ; ; )
132  for (int iter =0; iter < 275; iter ++)
133  {
134  cMo = robot.get_cMo();
135 
136  for (unsigned int i = 0 ; i < 4 ; i++) {
137  point[i].track(cMo);
138  vpFeatureBuilder::create(p[i], point[i]);
139  }
140 
141  vpDisplay::display(Iint);
142  robot.getInternalView(Iint);
143  if (!start) {
144  display_trajectory(Iint, point, cMo, cam);
145  vpDisplay::displayCharString(Iint, 40, 120, "Click to stop the servo...", vpColor::red);
146  }
147  vpDisplay::flush(Iint);
148 
149  vpColVector v = task.computeControlLaw();
151 
152  // A click to exit
153  if (vpDisplay::getClick(Iint, false))
154  break;
155 
156  if (start) {
157  start = false;
158  v = 0;
160  vpDisplay::displayCharString(Iint, 40, 120, "Click to start the servo...", vpColor::blue);
161  vpDisplay::flush(Iint);
162  //vpDisplay::getClick(Iint);
163  }
164 
165  vpTime::wait(1000*robot.getSamplingTime());
166  }
167  task.kill();
168  }
169  catch(vpException e) {
170  std::cout << "Catch an exception: " << e << std::endl;
171  }
172 #endif
173 }
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:159
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
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:1994
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:190
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:199
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:206
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
virtual void displayCharString(const vpImagePoint &ip, const char *text, const vpColor &color=vpColor::green)=0
unsigned int getHeight() const
Definition: vpImage.h:150
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:92
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