ViSP  2.8.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  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 }
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:155
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:133
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)
create a new ste of two visual features
Definition: vpServo.cpp:444
void setLambda(double _lambda)
set the gain lambda
Definition: vpServo.h:253
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:1991
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:154
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()
destruction (memory deallocation if required)
Definition: vpServo.cpp:177
Initialize the velocity controller.
Definition: vpRobot.h:70
vpColVector computeControlLaw()
compute the desired control law
Definition: vpServo.cpp:883
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:203
Generic class defining intrinsic camera parameters.
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Set the type of the interaction matrix (current, mean, desired, user).
Definition: vpServo.cpp:509
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)
Class required to compute the visual servoing control law descbribed in and .
Definition: vpServo.h:153
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v)
Class that consider the case of a translation vector.
void setServo(vpServoType _servo_type)
Choice of the visual servoing control law.
Definition: vpServo.cpp:214
static const vpColor blue
Definition: vpColor.h:173