Visual Servoing Platform  version 3.0.1
moveBiclops.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Tests the control law
32  * Authors:
33  * Fabien Spindler
34  *
35  *****************************************************************************/
57 #include <visp3/io/vpParseArgv.h>
58 #include <visp3/core/vpConfig.h>
59 #include <visp3/core/vpDebug.h>
60 #include <visp3/core/vpColVector.h>
61 #include <visp3/core/vpTime.h>
62 #include <stdlib.h>
63 #ifdef VISP_HAVE_BICLOPS
64 
65 #include <visp3/robot/vpRobotBiclops.h>
66 
67 // List of allowed command line options
68 #define GETOPTARGS "c:h"
69 
70 /*
71 
72 Print the program options.
73 
74  \param name : Program name.
75  \param badparam : Bad parameter name.
76  \param conf : Biclops configuration file.
77 
78 */
79 void usage(const char *name, const char *badparam, std::string conf)
80 {
81  fprintf(stdout, "\n\
82 Move the biclops robot\n\
83 \n\
84 SYNOPSIS\n\
85  %s [-c <Biclops configuration file>] [-h]\n \
86 ", name);
87 
88  fprintf(stdout, "\n\
89 OPTIONS: Default\n\
90  -c <Biclops configuration file> %s\n\
91  Sets the biclops robot configuration file.\n\n",
92  conf.c_str());
93 
94  if (badparam) {
95  fprintf(stderr, "ERROR: \n" );
96  fprintf(stderr, "\nBad parameter [%s]\n", badparam);
97  }
98 }
99 
111 bool getOptions(int argc, const char **argv, std::string& conf)
112 {
113  const char *optarg_;
114  int c;
115  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
116 
117  switch (c) {
118  case 'c': conf = optarg_; break;
119  case 'h': usage(argv[0], NULL, conf); return false; break;
120 
121  default:
122  usage(argv[0], optarg_, conf); return false; break;
123  }
124  }
125 
126  if ((c == 1) || (c == -1)) {
127  // standalone param or error
128  usage(argv[0], NULL, conf);
129  std::cerr << "ERROR: " << std::endl;
130  std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
131  return false;
132  }
133 
134  return true;
135 }
136 
137 int
138 main(int argc, const char ** argv)
139 {
140  std::string opt_conf = "/usr/share/BiclopsDefault.cfg";
141 
142  // Read the command line options
143  if (getOptions(argc, argv, opt_conf) == false) {
144  exit (-1);
145  }
146  try {
147  vpRobotBiclops robot(opt_conf.c_str());
148 
149  vpColVector q (vpBiclops::ndof) ; // desired position
150  vpColVector qdot (vpBiclops::ndof) ; // desired velocity
151  vpColVector qm (vpBiclops::ndof) ; // measured position
152  vpColVector qm_dot(vpBiclops::ndof) ; // measured velocity
153 
155 
156  q = 0;
157  q[0] = vpMath::rad(-10);
158  q[1] = vpMath::rad(-20);
159  std::cout << "Set position in the articular frame: "
160  << " pan: " << vpMath::deg(q[0]) << " deg"
161  << " tilt: " << vpMath::deg(q[1]) << " deg" << std::endl ;
162  robot.setPositioningVelocity(30.) ;
164 
166  std::cout << "Position in the articular frame: "
167  << " pan: " << vpMath::deg(qm[0])
168  << " tilt: " << vpMath::deg(qm[1]) << std::endl ;
169  robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm) ;
170  std::cout << "Velocity in the articular frame: "
171  << " pan: " << vpMath::deg(qm[0])
172  << " tilt: " << vpMath::deg(qm[1]) << std::endl ;
173 
174  q[0] = vpMath::rad(10);
175  q[1] = vpMath::rad(20);
176  std::cout << "Set position in the articular frame: "
177  << " pan: " << vpMath::deg(q[0]) << " deg"
178  << " tilt: " << vpMath::deg(q[1]) << " deg" << std::endl ;
179  robot.setPositioningVelocity(10) ;
181 
183  std::cout << "Position in the articular frame: "
184  << " pan: " << vpMath::deg(qm[0])
185  << " tilt: " << vpMath::deg(qm[1]) << std::endl ;
186  robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm) ;
187  std::cout << "Velocity in the articular frame: "
188  << " pan: " << vpMath::deg(qm[0])
189  << " tilt: " << vpMath::deg(qm[1]) << std::endl ;
190 
191  std::cout << "Set STATE_VELOCITY_CONTROL" << std::endl;
193 
195  std::cout << "Position in the articular frame: "
196  << " pan: " << vpMath::deg(qm[0]) << " deg"
197  << " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl ;
198  robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm) ;
199  std::cout << "Velocity in the articular frame: "
200  << " pan: " << vpMath::deg(qm[0])
201  << " tilt: " << vpMath::deg(qm[1]) << std::endl ;
202 
203  qdot = 0 ;
204  // qdot[0] = vpMath::rad(0.1) ;
205  qdot[1] = vpMath::rad(25) ;
206  std::cout << "Set articular frame velocity "
207  << " pan: " << vpMath::deg(qdot[0]) << " deg/s"
208  << " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl ;
210 
211  //waits 5000ms
212  vpTime::wait(5000.0);
213 
215  std::cout << "Position in the articular frame: "
216  << " pan: " << vpMath::deg(qm[0]) << " deg"
217  << " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl ;
218  robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm) ;
219  std::cout << "Velocity in the articular frame: "
220  << " pan: " << vpMath::deg(qm[0])
221  << " tilt: " << vpMath::deg(qm[1]) << std::endl ;
222 
223  qdot = 0 ;
224  // qdot[0] = vpMath::rad(0.1) ;
225  qdot[1] = -vpMath::rad(25) ;
226  std::cout << "Set articular frame velocity "
227  << " pan: " << vpMath::deg(qdot[0]) << " deg/s"
228  << " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl ;
230 
231  //waits 3000 ms
232  vpTime::wait(3000.0);
233 
235  std::cout << "Position in the articular frame: "
236  << " pan: " << vpMath::deg(qm[0]) << " deg"
237  << " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl ;
238  robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm) ;
239  std::cout << "Velocity in the articular frame: "
240  << " pan: " << vpMath::deg(qm[0])
241  << " tilt: " << vpMath::deg(qm[1]) << std::endl ;
242 
243  qdot = 0 ;
244  // qdot[0] = vpMath::rad(0.1) ;
245  qdot[1] = vpMath::rad(10) ;
246  std::cout << "Set articular frame velocity "
247  << " pan: " << vpMath::deg(qdot[0]) << " deg/s"
248  << " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl ;
250 
251  //waits 2000 ms
252  vpTime::wait(2000.0);
253 
255  std::cout << "Position in the articular frame: "
256  << " pan: " << vpMath::deg(qm[0]) << " deg"
257  << " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl ;
258  robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm) ;
259  std::cout << "Velocity in the articular frame: "
260  << " pan: " << vpMath::deg(qm[0])
261  << " tilt: " << vpMath::deg(qm[1]) << std::endl ;
262 
263  qdot = 0 ;
264  qdot[0] = vpMath::rad(-5);
265  //qdot[1] = vpMath::rad(-5);
266 
267  std::cout << "Set articular frame velocity "
268  << " pan: " << vpMath::deg(qdot[0]) << " deg/s"
269  << " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl ;
271 
272  //waits 2000 ms
273  vpTime::wait(2000.0);
274 
276  std::cout << "Position in the articular frame: "
277  << " pan: " << vpMath::deg(qm[0]) << " deg"
278  << " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl ;
279  robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm) ;
280  std::cout << "Velocity in the articular frame: "
281  << " pan: " << vpMath::deg(qm[0])
282  << " tilt: " << vpMath::deg(qm[1]) << std::endl ;
283  }
284  catch(vpException &e) {
285  std::cout << "Catch an exception: " << e.getMessage() << std::endl;
286  }
287 
288 }
289 #else
290 int
291 main()
292 {
293  vpERROR_TRACE("You do not have a biclops robot connected to your computer...");
294  return 0;
295 }
296 
297 #endif
void setPosition(const vpHomogeneousMatrix &wMc)
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:157
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
static const unsigned int ndof
Definition: vpBiclops.h:123
#define vpERROR_TRACE
Definition: vpDebug.h:391
Initialize the position controller.
Definition: vpRobot.h:69
error that can be emited by ViSP classes.
Definition: vpException.h:73
vpHomogeneousMatrix getPosition() const
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:76
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
Initialize the velocity controller.
Definition: vpRobot.h:68
Interface for the biclops, pan, tilt head control.
static double rad(double deg)
Definition: vpMath.h:104
const char * getMessage(void) const
Definition: vpException.cpp:97
static double deg(double rad)
Definition: vpMath.h:97
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72