ViSP  3.0.0
moveBiclops.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2015 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  vpCTRACE << "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  vpCTRACE << "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  vpCTRACE << "Velocity in the articular frame: "
171  << " pan: " << vpMath::deg(qm[0])
172  << " tilt: " << vpMath::deg(qm[1]) << std::endl ;
173 
174  vpCTRACE << "---------------------------------------- " << std::endl;
175 
176  q[0] = vpMath::rad(10);
177  q[1] = vpMath::rad(20);
178  vpCTRACE << "Set position in the articular frame: "
179  << " pan: " << vpMath::deg(q[0]) << " deg"
180  << " tilt: " << vpMath::deg(q[1]) << " deg" << std::endl ;
181  robot.setPositioningVelocity(10) ;
183 
185  vpCTRACE << "Position in the articular frame: "
186  << " pan: " << vpMath::deg(qm[0])
187  << " tilt: " << vpMath::deg(qm[1]) << std::endl ;
188  robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm) ;
189  vpCTRACE << "Velocity in the articular frame: "
190  << " pan: " << vpMath::deg(qm[0])
191  << " tilt: " << vpMath::deg(qm[1]) << std::endl ;
192 
193  vpCTRACE << "---------------------------------------- " << std::endl;
194 
195  vpCTRACE << "Set STATE_VELOCITY_CONTROL" << std::endl;
197 
199  vpCTRACE << "Position in the articular frame: "
200  << " pan: " << vpMath::deg(qm[0]) << " deg"
201  << " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl ;
202  robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm) ;
203  vpCTRACE << "Velocity in the articular frame: "
204  << " pan: " << vpMath::deg(qm[0])
205  << " tilt: " << vpMath::deg(qm[1]) << std::endl ;
206 
207  vpCTRACE << "---------------------------------------- " << std::endl;
208  qdot = 0 ;
209  // qdot[0] = vpMath::rad(0.1) ;
210  qdot[1] = vpMath::rad(25) ;
211  vpCTRACE << "Set articular frame velocity "
212  << " pan: " << vpMath::deg(qdot[0]) << " deg/s"
213  << " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl ;
215 
216  //waits 5000ms
217  vpTime::wait(5000.0);
218 
220  vpCTRACE << "Position in the articular frame: "
221  << " pan: " << vpMath::deg(qm[0]) << " deg"
222  << " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl ;
223  robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm) ;
224  vpCTRACE << "Velocity in the articular frame: "
225  << " pan: " << vpMath::deg(qm[0])
226  << " tilt: " << vpMath::deg(qm[1]) << std::endl ;
227 
228 
229  vpCTRACE << "---------------------------------------- " << std::endl;
230  qdot = 0 ;
231  // qdot[0] = vpMath::rad(0.1) ;
232  qdot[1] = -vpMath::rad(25) ;
233  vpCTRACE << "Set articular frame velocity "
234  << " pan: " << vpMath::deg(qdot[0]) << " deg/s"
235  << " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl ;
237 
238  //waits 3000 ms
239  vpTime::wait(3000.0);
240 
242  vpCTRACE << "Position in the articular frame: "
243  << " pan: " << vpMath::deg(qm[0]) << " deg"
244  << " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl ;
245  robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm) ;
246  vpCTRACE << "Velocity in the articular frame: "
247  << " pan: " << vpMath::deg(qm[0])
248  << " tilt: " << vpMath::deg(qm[1]) << std::endl ;
249 
250 
251  vpCTRACE << "---------------------------------------- " << std::endl;
252 
253 
254  qdot = 0 ;
255  // qdot[0] = vpMath::rad(0.1) ;
256  qdot[1] = vpMath::rad(10) ;
257  vpCTRACE << "Set articular frame velocity "
258  << " pan: " << vpMath::deg(qdot[0]) << " deg/s"
259  << " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl ;
261 
262  //waits 2000 ms
263  vpTime::wait(2000.0);
264 
266  vpCTRACE << "Position in the articular frame: "
267  << " pan: " << vpMath::deg(qm[0]) << " deg"
268  << " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl ;
269  robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm) ;
270  vpCTRACE << "Velocity in the articular frame: "
271  << " pan: " << vpMath::deg(qm[0])
272  << " tilt: " << vpMath::deg(qm[1]) << std::endl ;
273 
274  vpCTRACE << "---------------------------------------- " << std::endl;
275 
276  qdot = 0 ;
277  qdot[0] = vpMath::rad(-5);
278  //qdot[1] = vpMath::rad(-5);
279 
280  vpCTRACE << "Set articular frame velocity "
281  << " pan: " << vpMath::deg(qdot[0]) << " deg/s"
282  << " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl ;
284 
285  //waits 2000 ms
286  vpTime::wait(2000.0);
287 
289  vpCTRACE << "Position in the articular frame: "
290  << " pan: " << vpMath::deg(qm[0]) << " deg"
291  << " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl ;
292  robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm) ;
293  vpCTRACE << "Velocity in the articular frame: "
294  << " pan: " << vpMath::deg(qm[0])
295  << " tilt: " << vpMath::deg(qm[1]) << std::endl ;
296  }
297  catch(...) {
298 
299  }
300 
301 }
302 #else
303 int
304 main()
305 {
306  vpERROR_TRACE("You do not have a biclops robot connected to your computer...");
307  return 0;
308 }
309 
310 #endif
void setPosition(const vpHomogeneousMatrix &wMc)
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:150
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
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
vpHomogeneousMatrix getPosition() const
Interface for the biclops, pan, tilt head control.
static double rad(double deg)
Definition: vpMath.h:104
static double deg(double rad)
Definition: vpMath.h:97
#define vpCTRACE
Definition: vpDebug.h:337
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72