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