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