Visual Servoing Platform version 3.5.0
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
77void usage(const char *name, const char *badparam)
78{
79 fprintf(stdout, "\n\
80Example of a positionning control followed by a velocity control \n\
81of the Afma4 robot.\n \
82\n\
83SYNOPSIS\n\
84 %s [-m] [-h]\n \
85", name);
86
87 fprintf(stdout, "\n\
88OPTIONS: 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
114bool 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
147int 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)
174 robot.setPosition(vpRobot::ARTICULAR_FRAME, qd);
175 sleep(1);
176
177 robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
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
252int 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
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
error that can be emited by ViSP classes.
Definition: vpException.h:72
static double rad(double deg)
Definition: vpMath.h:110
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
Control of Irisa's cylindrical robot named Afma4.
Definition: vpRobotAfma4.h:179
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
@ ARTICULAR_FRAME
Definition: vpRobot.h:78
@ CAMERA_FRAME
Definition: vpRobot.h:82
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:67
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:66
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201