Visual Servoing Platform version 3.5.0
moveBiclops.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 * Tests the control law
33 * Authors:
34 * Fabien Spindler
35 *
36 *****************************************************************************/
55#include <stdlib.h>
56#include <visp3/core/vpColVector.h>
57#include <visp3/core/vpDebug.h>
58#include <visp3/core/vpTime.h>
59#include <visp3/io/vpParseArgv.h>
60#ifdef VISP_HAVE_BICLOPS
61
62#include <visp3/robot/vpRobotBiclops.h>
63
64// List of allowed command line options
65#define GETOPTARGS "c:h"
66
67/*
68
69Print the program options.
70
71 \param name : Program name.
72 \param badparam : Bad parameter name.
73 \param conf : Biclops configuration file.
74
75*/
76void usage(const char *name, const char *badparam, std::string conf)
77{
78 fprintf(stdout, "\n\
79Move the biclops robot\n\
80\n\
81SYNOPSIS\n\
82 %s [-c <Biclops configuration file>] [-h]\n \
83", name);
84
85 fprintf(stdout, "\n\
86OPTIONS: Default\n\
87 -c <Biclops configuration file> %s\n\
88 Sets the biclops robot configuration file.\n\n", conf.c_str());
89
90 if (badparam) {
91 fprintf(stderr, "ERROR: \n");
92 fprintf(stderr, "\nBad parameter [%s]\n", badparam);
93 }
94}
95
107bool getOptions(int argc, const char **argv, std::string &conf)
108{
109 const char *optarg_;
110 int c;
111 while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
112
113 switch (c) {
114 case 'c':
115 conf = optarg_;
116 break;
117 case 'h':
118 usage(argv[0], NULL, conf);
119 return false;
120 break;
121
122 default:
123 usage(argv[0], optarg_, conf);
124 return false;
125 break;
126 }
127 }
128
129 if ((c == 1) || (c == -1)) {
130 // standalone param or error
131 usage(argv[0], NULL, conf);
132 std::cerr << "ERROR: " << std::endl;
133 std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
134 return false;
135 }
136
137 return true;
138}
139
140int main(int argc, const char **argv)
141{
142 std::string opt_conf = "/usr/share/BiclopsDefault.cfg";
143
144 // Read the command line options
145 if (getOptions(argc, argv, opt_conf) == false) {
146 exit(-1);
147 }
148 try {
149 vpRobotBiclops robot(opt_conf.c_str());
150
151 vpColVector q(vpBiclops::ndof); // desired position
152 vpColVector qdot(vpBiclops::ndof); // desired velocity
153 vpColVector qm(vpBiclops::ndof); // measured position
154 vpColVector qm_dot(vpBiclops::ndof); // measured velocity
155
157
158 q = 0;
159 q[0] = vpMath::rad(-10);
160 q[1] = vpMath::rad(-20);
161 std::cout << "Set position in the articular frame: "
162 << " pan: " << vpMath::deg(q[0]) << " deg"
163 << " tilt: " << vpMath::deg(q[1]) << " deg" << std::endl;
164 robot.setPositioningVelocity(30.);
165 robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
166
167 robot.getPosition(vpRobot::ARTICULAR_FRAME, qm);
168 std::cout << "Position in the articular frame: "
169 << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl;
171 std::cout << "Velocity in the articular frame: "
172 << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl;
173
174 q[0] = vpMath::rad(10);
175 q[1] = vpMath::rad(20);
176 std::cout << "Set position in the articular frame: "
177 << " pan: " << vpMath::deg(q[0]) << " deg"
178 << " tilt: " << vpMath::deg(q[1]) << " deg" << std::endl;
179 robot.setPositioningVelocity(10);
180 robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
181
182 robot.getPosition(vpRobot::ARTICULAR_FRAME, qm);
183 std::cout << "Position in the articular frame: "
184 << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl;
186 std::cout << "Velocity in the articular frame: "
187 << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl;
188
189 std::cout << "Set STATE_VELOCITY_CONTROL" << std::endl;
191
192 robot.getPosition(vpRobot::ARTICULAR_FRAME, qm);
193 std::cout << "Position in the articular frame: "
194 << " pan: " << vpMath::deg(qm[0]) << " deg"
195 << " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl;
197 std::cout << "Velocity in the articular frame: "
198 << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl;
199
200 qdot = 0;
201 // qdot[0] = vpMath::rad(0.1) ;
202 qdot[1] = vpMath::rad(25);
203 std::cout << "Set articular frame velocity "
204 << " pan: " << vpMath::deg(qdot[0]) << " deg/s"
205 << " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl;
207
208 // waits 5000ms
209 vpTime::wait(5000.0);
210
211 robot.getPosition(vpRobot::ARTICULAR_FRAME, qm);
212 std::cout << "Position in the articular frame: "
213 << " pan: " << vpMath::deg(qm[0]) << " deg"
214 << " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl;
216 std::cout << "Velocity in the articular frame: "
217 << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl;
218
219 qdot = 0;
220 // qdot[0] = vpMath::rad(0.1) ;
221 qdot[1] = -vpMath::rad(25);
222 std::cout << "Set articular frame velocity "
223 << " pan: " << vpMath::deg(qdot[0]) << " deg/s"
224 << " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl;
226
227 // waits 3000 ms
228 vpTime::wait(3000.0);
229
230 robot.getPosition(vpRobot::ARTICULAR_FRAME, qm);
231 std::cout << "Position in the articular frame: "
232 << " pan: " << vpMath::deg(qm[0]) << " deg"
233 << " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl;
235 std::cout << "Velocity in the articular frame: "
236 << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl;
237
238 qdot = 0;
239 // qdot[0] = vpMath::rad(0.1) ;
240 qdot[1] = vpMath::rad(10);
241 std::cout << "Set articular frame velocity "
242 << " pan: " << vpMath::deg(qdot[0]) << " deg/s"
243 << " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl;
245
246 // waits 2000 ms
247 vpTime::wait(2000.0);
248
249 robot.getPosition(vpRobot::ARTICULAR_FRAME, qm);
250 std::cout << "Position in the articular frame: "
251 << " pan: " << vpMath::deg(qm[0]) << " deg"
252 << " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl;
254 std::cout << "Velocity in the articular frame: "
255 << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl;
256
257 qdot = 0;
258 qdot[0] = vpMath::rad(-5);
259 // qdot[1] = vpMath::rad(-5);
260
261 std::cout << "Set articular frame velocity "
262 << " pan: " << vpMath::deg(qdot[0]) << " deg/s"
263 << " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl;
265
266 // waits 2000 ms
267 vpTime::wait(2000.0);
268
269 robot.getPosition(vpRobot::ARTICULAR_FRAME, qm);
270 std::cout << "Position in the articular frame: "
271 << " pan: " << vpMath::deg(qm[0]) << " deg"
272 << " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl;
274 std::cout << "Velocity in the articular frame: "
275 << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl;
276 return EXIT_SUCCESS;
277 }
278 catch (const vpException &e) {
279 std::cout << "Catch an exception: " << e.getMessage() << std::endl;
280 return EXIT_FAILURE;
281 }
282}
283#else
284int main()
285{
286 std::cout << "You do not have an biclops PT robot connected to your computer..." << std::endl;
287 return EXIT_SUCCESS;
288}
289
290#endif
static const unsigned int ndof
Definition: vpBiclops.h:126
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
error that can be emited by ViSP classes.
Definition: vpException.h:72
const char * getMessage() const
Definition: vpException.cpp:90
static double rad(double deg)
Definition: vpMath.h:110
static double deg(double rad)
Definition: vpMath.h:103
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
Interface for the biclops, pan, tilt head control.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
@ ARTICULAR_FRAME
Definition: vpRobot.h:78
@ 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
VISP_EXPORT int wait(double t0, double t)