Visual Servoing Platform  version 3.3.0
testVirtuoseWithGlove.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Test for Virtuose + Glove SDK wrapper.
33  *
34  * Author:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
46 #include <stdlib.h>
47 
48 #include <iostream>
49 #include <sstream>
50 #include <vector>
51 
52 #include <visp3/robot/vpVirtuose.h>
53 #include <visp3/core/vpImage.h>
54 #include <visp3/core/vpCameraParameters.h>
55 #include <visp3/core/vpMeterPixelConversion.h>
56 #include <visp3/gui/vpDisplayX.h>
57 #include <visp3/gui/vpDisplayGDI.h>
58 
59 #if defined(VISP_HAVE_VIRTUOSE) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
60 
61 int main()
62 {
63  int port = 53210;
64 
65  std::vector<vpVirtuose> virtuose(4); // 0: virtuose, 1: thumb, 2: index, 3: middle
66  std::vector<vpHomogeneousMatrix> wMd(4);
67 
68  // Position of the external camera with respect of the virtuose world frame
69  vpHomogeneousMatrix wMc(0.3, 0, 1.2, vpMath::rad(180), 0, 0);
70 
71  // Open device
72  for (size_t device=0; device < virtuose.size(); device ++) {
73  std::stringstream ss; ss << port + device;
74  std::string ip = "localhost#" + ss.str();
75  std::cout << "Connect to: " << ip << std::endl;
76 
77  virtuose[device].setIpAddress(ip);
78  virtuose[device].init();
79  }
80 
81  std::cout << "After init" << std::endl;
82 #if 0
83  // Get joint position
84  for (size_t device=0; device < virtuose.size(); device ++) {
85  std::cout << "Number of joints: " << virtuose[device].getJointsNumber()
86  << " Joint position: " << virtuose[device].getArticularPosition().t() << std::endl
87  << " Cartesian position: " << virtuose[device].getAvatarPosition().t() << std::endl;
88  // std::cout << "Joint velocity: " << virtuose.getArticularVelocity().t() << std::endl;
89  }
90 #endif
91  bool end = false;
92  vpImage<unsigned char> I(480, 640, 0);
94 
95 #if defined(VISP_HAVE_X11)
96  vpDisplayX d(I);
97 #elif defined(VISP_HAVE_GDI)
98  vpDisplayGDI d(I);
99 #endif
100 
101  while(!end) {
103  // Get joint position
104  for (size_t device=0; device < virtuose.size(); device ++) {
105  vpPoseVector wpd = virtuose[device].getAvatarPosition();
106  std::cout << "Device #" << device << " has position: " << wpd.t() << std::endl;
107  wMd[device].buildFrom(wpd);
108 
109  vpHomogeneousMatrix cMd = wMc.inverse() * wMd[device];
110  vpDisplay::displayFrame(I, cMd, cam, 0.1);
111 
112  std::stringstream ss;
113  ss << device;
114  double X = cMd[0][3];
115  double Y = cMd[1][3];
116  double Z = cMd[2][3];
117  vpImagePoint pos;
118  vpMeterPixelConversion::convertPoint(cam, X/Z, Y/Z, pos);
119  vpDisplay::displayText(I, pos + vpImagePoint(10, 10), ss.str(), vpColor::white);
120  vpDisplay::displayText(I, 10, 10, "Click to quit...", vpColor::red);
121  }
122  if(vpDisplay::getClick(I, false)) {
123  end = true;
124  }
125  vpDisplay::flush(I);
126 
127  }
128 
129  // Close device
130  for (size_t device=0; device < virtuose.size(); device ++) {
131  virtuose[device].close();
132  }
133  std::cout << "The end" << std::endl;
134 }
135 
136 #else
137 int main() { std::cout << "You should install Virtuose API to use this binary..." << std::endl; }
138 #endif
vpDisplayX
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:149
vpMath::rad
static double rad(double deg)
Definition: vpMath.h:107
vpCameraParameters
Generic class defining intrinsic camera parameters.
Definition: vpCameraParameters.h:232
vpDisplayGDI
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:127
vpDisplay::displayFrame
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1, const vpImagePoint &offset=vpImagePoint(0, 0))
Definition: vpDisplay_uchar.cpp:383
vpDisplay::display
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay_uchar.cpp:739
vpDisplay::displayText
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Definition: vpDisplay_uchar.cpp:663
vpPoseVector
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:150
vpImagePoint
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:87
vpColor::white
static const vpColor white
Definition: vpColor.h:173
vpDisplay::flush
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay_uchar.cpp:715
vpHomogeneousMatrix::inverse
vpHomogeneousMatrix inverse() const
Definition: vpHomogeneousMatrix.cpp:640
vpImage< unsigned char >
vpDisplay::getClick
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
Definition: vpDisplay_uchar.cpp:764
vpHomogeneousMatrix
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition: vpHomogeneousMatrix.h:148
vpPoseVector::t
vpRowVector t() const
Definition: vpPoseVector.cpp:392
vpMeterPixelConversion::convertPoint
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Definition: vpMeterPixelConversion.h:106
vpColor::red
static const vpColor red
Definition: vpColor.h:178