Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
moveAfma4.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 Afma 4 dof robot.
33  *
34  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
55 #include <visp3/core/vpConfig.h>
56 #include <visp3/core/vpDebug.h>
57 
58 #ifdef VISP_HAVE_AFMA4
59 
60 #include <stdlib.h>
61 #include <unistd.h>
62 
63 #include <visp3/io/vpParseArgv.h>
64 #include <visp3/robot/vpRobotAfma4.h>
65 
66 // List of allowed command line options
67 #define GETOPTARGS "mh"
68 
77 void usage(const char *name, const char *badparam)
78 {
79  fprintf(stdout, "\n\
80 Example of a positionning control followed by a velocity control \n\
81 of the Afma4 robot.\n \
82 \n\
83 SYNOPSIS\n\
84  %s [-m] [-h]\n \
85 ", name);
86 
87  fprintf(stdout, "\n\
88 OPTIONS: Default\n\
89  -m\n\
90  Turn off the control of the robot. This option is\n\
91  essentially useful for security reasons during nightly\n\
92  tests.\n\
93 \n\
94  -h\n\
95  Print the help.\n\n");
96 
97  if (badparam) {
98  fprintf(stderr, "ERROR: \n");
99  fprintf(stderr, "\nBad parameter [%s]\n", badparam);
100  }
101 }
102 
114 bool getOptions(int argc, const char **argv, bool &control)
115 {
116  const char *optarg;
117  int c;
118  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
119 
120  switch (c) {
121  case 'm':
122  control = false;
123  break;
124  case 'h':
125  usage(argv[0], NULL);
126  return false;
127  break;
128 
129  default:
130  usage(argv[0], optarg);
131  return false;
132  break;
133  }
134  }
135 
136  if ((c == 1) || (c == -1)) {
137  // standalone param or error
138  usage(argv[0], NULL);
139  std::cerr << "ERROR: " << std::endl;
140  std::cerr << " Bad argument " << optarg << std::endl << std::endl;
141  return false;
142  }
143 
144  return true;
145 }
146 
147 int main(int argc, const char **argv)
148 {
149  try {
150  bool control = true; // Turn on the robot control by applying positions
151  // and velocities to the robot.
152  // Read the command line options
153  if (getOptions(argc, argv, control) == false) {
154  exit(-1);
155  }
156 
157  vpRobotAfma4 robot;
158 
159  vpColVector qd(robot.njoint);
160  vpColVector q(robot.njoint);
161 
162  //
163  // Position control in articular
164  //
165  qd[0] = vpMath::rad(10);
166  qd[1] = -0.1;
167  qd[2] = vpMath::rad(20);
168  qd[3] = vpMath::rad(-10);
169 
170  std::cout << "Position control: in articular..." << std::endl;
171  std::cout << " position to reach: " << qd.t() << std::endl;
173  if (control)
175  sleep(1);
176 
178  std::cout << " measured position: " << q.t();
179  sleep(1);
180 
182 
183 #if 0
184  //
185  // Velocity control in articular
186  //
187  std::cout << "Velocity control: in articular..." << std::endl;
188 
189  q = 0 ;
190  q[0] = vpMath::rad(2) ; // rotation arround vertical axis
191  std::cout << " rotation arround vertical axis: " << q[0] << std::endl;
192  if (control)
194  sleep(5) ;
195 
196  q = 0 ;
197  q[1] = 0.2 ; // Vertical translation
198  std::cout << " vertical translation: " << q[1] << std::endl;
199  if (control)
201  sleep(5) ;
202 
203  q = 0 ;
204  q[1] = -0.2 ; // Vertical translation
205  std::cout << " vertical translation: " << q[1] << std::endl;
206  if (control)
208  sleep(5) ;
209  q = 0 ;
210  q[2] = vpMath::rad(3) ; // pan
211  std::cout << " pan rotation: " << q[2] << std::endl;
212  if (control)
214  sleep(5) ;
215 
216  q = 0 ;
217  q[3] = vpMath::rad(2) ; // tilt
218  std::cout << " tilt rotation: " << q[3] << std::endl;
219  if (control)
221  sleep(5) ;
222 #endif
223  //
224  // Velocity control in camera frame
225  //
227  std::cout << "Velocity control: in camera frame..." << std::endl;
228  q.resize(6);
229  q = 0.0;
230  q[0] = vpMath::rad(2); // rotation arround vertical axis
231  std::cout << " rx rotation: " << q[0] << std::endl;
232  if (control)
234  sleep(5);
235 
236  q.resize(6);
237  q = 0.0;
238  q[1] = vpMath::rad(2); // rotation arround vertical axis
239  std::cout << " ry rotation: " << q[1] << std::endl;
240  if (control)
242  sleep(5);
243 
244  std::cout << "The end" << std::endl;
245  return EXIT_SUCCESS;
246  } catch (const vpException &e) {
247  std::cout << "Catch a ViSP exception: " << e << std::endl;
248  return EXIT_FAILURE;
249  }
250 }
251 #else
252 int main()
253 {
254  std::cout << "You do not have an afma4 robot connected to your computer..." << std::endl;
255  return EXIT_SUCCESS;
256 }
257 
258 #endif
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
Initialize the position controller.
Definition: vpRobot.h:68
error that can be emited by ViSP classes.
Definition: vpException.h:71
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
Initialize the velocity controller.
Definition: vpRobot.h:67
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
static const unsigned int njoint
Number of joint.
Definition: vpAfma4.h:142
static double rad(double deg)
Definition: vpMath.h:102
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
Control of Irisa&#39;s cylindrical robot named Afma4.
Definition: vpRobotAfma4.h:178