ViSP  2.6.2
moveAfma4.cpp
1 /****************************************************************************
2  *
3  * $Id: moveAfma4.cpp 3619 2012-03-09 17:28:57Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2012 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  * Test for Afma 4 dof robot.
36  *
37  * Authors:
38  * Fabien Spindler
39  *
40  *****************************************************************************/
59 #include <visp/vpConfig.h>
60 #include <visp/vpDebug.h>
61 #include <stdlib.h>
62 #ifdef VISP_HAVE_AFMA4
63 
64 #include <visp/vpParseArgv.h>
65 #include <visp/vpRobotAfma4.h>
66 
67 // List of allowed command line options
68 #define GETOPTARGS "mh"
69 
78 void usage(const char *name, const char *badparam)
79 {
80  fprintf(stdout, "\n\
81 Example of a positionning control followed by a velocity control \n\
82 of the Afma4 robot.\n \
83 \n\
84 SYNOPSIS\n\
85  %s [-m] [-h]\n \
86 ", name);
87 
88  fprintf(stdout, "\n\
89 OPTIONS: Default\n\
90  -m\n\
91  Turn off the control of the robot. This option is\n\
92  essentially useful for security reasons during nightly\n\
93  tests.\n\
94 \n\
95  -h\n\
96  Print the help.\n\n");
97 
98  if (badparam) {
99  fprintf(stderr, "ERROR: \n" );
100  fprintf(stderr, "\nBad parameter [%s]\n", badparam);
101  }
102 
103 }
104 
116 bool getOptions(int argc, const char **argv, bool &control)
117 {
118  const char *optarg;
119  int c;
120  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
121 
122  switch (c) {
123  case 'm': control = false; break;
124  case 'h': usage(argv[0], NULL); return false; break;
125 
126  default:
127  usage(argv[0], optarg); return false; break;
128  }
129  }
130 
131  if ((c == 1) || (c == -1)) {
132  // standalone param or error
133  usage(argv[0], NULL);
134  std::cerr << "ERROR: " << std::endl;
135  std::cerr << " Bad argument " << optarg << std::endl << std::endl;
136  return false;
137  }
138 
139  return true;
140 }
141 
142 int
143 main(int argc, const char ** argv)
144 {
145  try
146  {
147  bool control = true; // Turn on the robot control by applying positions
148  // and velocities to the robot.
149  // Read the command line options
150  if (getOptions(argc, argv, control) == false) {
151  exit (-1);
152  }
153 
154  vpRobotAfma4 robot ;
155 
156  vpColVector qd(robot.njoint) ;
157  vpColVector q(robot.njoint) ;
158 
159  //
160  // Position control in articular
161  //
162  qd[0] = vpMath::rad(10);
163  qd[1] = -0.1;
164  qd[2] = vpMath::rad(20);
165  qd[3] = vpMath::rad(-10);
166 
167  std::cout << "Position control: in articular..." << std::endl;
168  std::cout << " position to reach: " << qd.t() << std::endl;
170  if (control)
172  sleep(1) ;
173 
174 
176  std::cout << " measured position: " << q.t() ;
177  sleep(1) ;
178 
180 
181  //
182  // Velocity control in articular
183  //
184  std::cout << "Velocity control: in articular..." << std::endl;
185 
186  q = 0 ;
187  q[0] = vpMath::rad(2) ; // rotation arround vertical axis
188  std::cout << " rotation arround vertical axis: " << q[0] << std::endl;
189  if (control)
191  sleep(5) ;
192 
193  q = 0 ;
194  q[1] = 0.2 ; // Vertical translation
195  std::cout << " vertical translation: " << q[1] << std::endl;
196  if (control)
198  sleep(5) ;
199 
200  q = 0 ;
201  q[1] = -0.2 ; // Vertical translation
202  std::cout << " vertical translation: " << q[1] << std::endl;
203  if (control)
205  sleep(5) ;
206  q = 0 ;
207  q[2] = vpMath::rad(3) ; // pan
208  std::cout << " pan rotation: " << q[2] << std::endl;
209  if (control)
211  sleep(5) ;
212 
213  q = 0 ;
214  q[3] = vpMath::rad(2) ; // tilt
215  std::cout << " tilt rotation: " << q[3] << std::endl;
216  if (control)
218  sleep(5) ;
219 
220  //
221  // Velocity control in camera frame
222  //
224  std::cout << "Velocity control: in camera frame..." << std::endl;
225  q.resize(2) ;
226  q = 0.0;
227  q[0] = vpMath::rad(2) ; // rotation arround vertical axis
228  std::cout << " rx rotation: " << q[0] << std::endl;
229  if (control)
231  sleep(5) ;
232 
233  q.resize(2) ;
234  q = 0.0;
235  q[1] = vpMath::rad(2) ; // rotation arround vertical axis
236  std::cout << " ry rotation: " << q[1] << std::endl;
237  if (control)
239  sleep(5) ;
240 
241  std::cout << "The end" << std::endl;
242  }
243  catch (...) {
244  vpERROR_TRACE(" Test failed") ;
245  }
246  return 0;
247 }
248 #else
249 int
250 main()
251 {
252  vpERROR_TRACE("You do not have an afma4 robot connected to your computer...");
253  return 0;
254 }
255 
256 #endif
#define vpERROR_TRACE
Definition: vpDebug.h:379
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
Initialize the position controller.
Definition: vpRobot.h:71
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:79
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
Initialize the velocity controller.
Definition: vpRobot.h:70
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
static const unsigned int njoint
Number of joint.
Definition: vpAfma4.h:141
static double rad(double deg)
Definition: vpMath.h:100
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
Control of Irisa's cylindrical robot named Afma4.
Definition: vpRobotAfma4.h:181