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