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 * Class which provides a simulator for the robot Afma6.
33 *
34 * Authors:
35 * Nicolas Melchior
36 *
37 *****************************************************************************/
38
39 #include <visp3/core/vpConfig.h>
40 #if defined(VISP_HAVE_MODULE_GUI) && ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD))
41 #include <cmath> // std::fabs
42 #include <limits> // numeric_limits
43 #include <string>
44 #include <visp3/core/vpImagePoint.h>
45 #include <visp3/core/vpIoTools.h>
46 #include <visp3/core/vpMeterPixelConversion.h>
47 #include <visp3/core/vpPoint.h>
48 #include <visp3/core/vpTime.h>
49 #include <visp3/robot/vpRobotException.h>
50 #include <visp3/robot/vpSimulatorAfma6.h>
51
52 #include "../wireframe-simulator/vpBound.h"
53 #include "../wireframe-simulator/vpRfstack.h"
54 #include "../wireframe-simulator/vpScene.h"
55 #include "../wireframe-simulator/vpVwstack.h"
56
57 const double vpSimulatorAfma6::defaultPositioningVelocity = 25.0;
58
59 /*!
60 Basic constructor
61 */
vpSimulatorAfma6()62 vpSimulatorAfma6::vpSimulatorAfma6()
63 : vpRobotWireFrameSimulator(), vpAfma6(), q_prev_getdis(), first_time_getdis(true),
64 positioningVelocity(defaultPositioningVelocity), zeroPos(), reposPos(), toolCustom(false), arm_dir()
65 {
66 init();
67 initDisplay();
68
69 tcur = vpTime::measureTimeMs();
70
71 #if defined(_WIN32)
72 #ifdef WINRT_8_1
73 mutex_fMi = CreateMutexEx(NULL, NULL, 0, NULL);
74 mutex_artVel = CreateMutexEx(NULL, NULL, 0, NULL);
75 mutex_artCoord = CreateMutexEx(NULL, NULL, 0, NULL);
76 mutex_velocity = CreateMutexEx(NULL, NULL, 0, NULL);
77 mutex_display = CreateMutexEx(NULL, NULL, 0, NULL);
78 #else
79 mutex_fMi = CreateMutex(NULL, FALSE, NULL);
80 mutex_artVel = CreateMutex(NULL, FALSE, NULL);
81 mutex_artCoord = CreateMutex(NULL, FALSE, NULL);
82 mutex_velocity = CreateMutex(NULL, FALSE, NULL);
83 mutex_display = CreateMutex(NULL, FALSE, NULL);
84 #endif
85
86 DWORD dwThreadIdArray;
87 hThread = CreateThread(NULL, // default security attributes
88 0, // use default stack size
89 launcher, // thread function name
90 this, // argument to thread function
91 0, // use default creation flags
92 &dwThreadIdArray); // returns the thread identifier
93 #elif defined(VISP_HAVE_PTHREAD)
94 pthread_mutex_init(&mutex_fMi, NULL);
95 pthread_mutex_init(&mutex_artVel, NULL);
96 pthread_mutex_init(&mutex_artCoord, NULL);
97 pthread_mutex_init(&mutex_velocity, NULL);
98 pthread_mutex_init(&mutex_display, NULL);
99
100 pthread_attr_init(&attr);
101 pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
102
103 pthread_create(&thread, NULL, launcher, (void *)this);
104 #endif
105
106 compute_fMi();
107 }
108
109 /*!
110 Constructor used to enable or disable the external view of the robot.
111
112 \param do_display : When true, enables the display of the external view.
113
114 */
vpSimulatorAfma6(bool do_display)115 vpSimulatorAfma6::vpSimulatorAfma6(bool do_display)
116 : vpRobotWireFrameSimulator(do_display), q_prev_getdis(), first_time_getdis(true),
117 positioningVelocity(defaultPositioningVelocity), zeroPos(), reposPos(), toolCustom(false), arm_dir()
118 {
119 init();
120 initDisplay();
121
122 tcur = vpTime::measureTimeMs();
123
124 #if defined(_WIN32)
125 #ifdef WINRT_8_1
126 mutex_fMi = CreateMutexEx(NULL, NULL, 0, NULL);
127 mutex_artVel = CreateMutexEx(NULL, NULL, 0, NULL);
128 mutex_artCoord = CreateMutexEx(NULL, NULL, 0, NULL);
129 mutex_velocity = CreateMutexEx(NULL, NULL, 0, NULL);
130 mutex_display = CreateMutexEx(NULL, NULL, 0, NULL);
131 #else
132 mutex_fMi = CreateMutex(NULL, FALSE, NULL);
133 mutex_artVel = CreateMutex(NULL, FALSE, NULL);
134 mutex_artCoord = CreateMutex(NULL, FALSE, NULL);
135 mutex_velocity = CreateMutex(NULL, FALSE, NULL);
136 mutex_display = CreateMutex(NULL, FALSE, NULL);
137 #endif
138
139 DWORD dwThreadIdArray;
140 hThread = CreateThread(NULL, // default security attributes
141 0, // use default stack size
142 launcher, // thread function name
143 this, // argument to thread function
144 0, // use default creation flags
145 &dwThreadIdArray); // returns the thread identifier
146 #elif defined(VISP_HAVE_PTHREAD)
147 pthread_mutex_init(&mutex_fMi, NULL);
148 pthread_mutex_init(&mutex_artVel, NULL);
149 pthread_mutex_init(&mutex_artCoord, NULL);
150 pthread_mutex_init(&mutex_velocity, NULL);
151 pthread_mutex_init(&mutex_display, NULL);
152
153 pthread_attr_init(&attr);
154 pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
155
156 pthread_create(&thread, NULL, launcher, (void *)this);
157 #endif
158
159 compute_fMi();
160 }
161
162 /*!
163 Basic destructor
164 */
~vpSimulatorAfma6()165 vpSimulatorAfma6::~vpSimulatorAfma6()
166 {
167 robotStop = true;
168
169 #if defined(_WIN32)
170 #if defined(WINRT_8_1)
171 WaitForSingleObjectEx(hThread, INFINITE, FALSE);
172 #else // pure win32
173 WaitForSingleObject(hThread, INFINITE);
174 #endif
175 CloseHandle(hThread);
176 CloseHandle(mutex_fMi);
177 CloseHandle(mutex_artVel);
178 CloseHandle(mutex_artCoord);
179 CloseHandle(mutex_velocity);
180 CloseHandle(mutex_display);
181 #elif defined(VISP_HAVE_PTHREAD)
182 pthread_attr_destroy(&attr);
183 pthread_join(thread, NULL);
184 pthread_mutex_destroy(&mutex_fMi);
185 pthread_mutex_destroy(&mutex_artVel);
186 pthread_mutex_destroy(&mutex_artCoord);
187 pthread_mutex_destroy(&mutex_velocity);
188 pthread_mutex_destroy(&mutex_display);
189 #endif
190
191 if (robotArms != NULL) {
192 for (int i = 0; i < 6; i++)
193 free_Bound_scene(&(robotArms[i]));
194 }
195
196 delete[] robotArms;
197 delete[] fMi;
198 }
199
200 /*!
201 Method which initialises the parameters linked to the robot caracteristics.
202
203 Set the path to the arm files (*.bnd and *.sln) used by the
204 simulator. If the path set in vpConfig.h in VISP_ROBOT_ARMS_DIR macro is
205 not valid, the path is set from the VISP_ROBOT_ARMS_DIR environment
206 variable that the user has to set.
207 */
init()208 void vpSimulatorAfma6::init()
209 {
210 // set arm_dir from #define VISP_ROBOT_ARMS_DIR if it exists
211 // VISP_ROBOT_ARMS_DIR may contain multiple locations separated by ";"
212 std::vector<std::string> arm_dirs = vpIoTools::splitChain(std::string(VISP_ROBOT_ARMS_DIR), std::string(";"));
213 bool armDirExists = false;
214 for (size_t i = 0; i < arm_dirs.size(); i++)
215 if (vpIoTools::checkDirectory(arm_dirs[i]) == true) { // directory exists
216 arm_dir = arm_dirs[i];
217 armDirExists = true;
218 break;
219 }
220 if (!armDirExists) {
221 try {
222 arm_dir = vpIoTools::getenv("VISP_ROBOT_ARMS_DIR");
223 std::cout << "The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
224 } catch (...) {
225 std::cout << "Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
226 }
227 }
228
229 this->init(vpAfma6::TOOL_CCMOP);
230 toolCustom = false;
231
232 size_fMi = 8;
233 fMi = new vpHomogeneousMatrix[8];
234 artCoord.resize(njoint);
235 artVel.resize(njoint);
236
237 zeroPos.resize(njoint);
238 zeroPos = 0;
239 reposPos.resize(njoint);
240 reposPos = 0;
241 reposPos[1] = -M_PI / 2;
242 reposPos[2] = M_PI;
243 reposPos[4] = M_PI / 2;
244
245 artCoord = zeroPos;
246 artVel = 0;
247
248 q_prev_getdis.resize(njoint);
249 q_prev_getdis = 0;
250 first_time_getdis = true;
251
252 positioningVelocity = defaultPositioningVelocity;
253
254 setRobotFrame(vpRobot::ARTICULAR_FRAME);
255 this->setRobotState(vpRobot::STATE_STOP);
256
257 // Software joint limits in radians
258 //_joint_min.resize(njoint);
259 _joint_min[0] = -0.6501;
260 _joint_min[1] = -0.6001;
261 _joint_min[2] = -0.5001;
262 _joint_min[3] = -2.7301;
263 _joint_min[4] = -0.1001;
264 _joint_min[5] = -1.5901;
265 //_joint_max.resize(njoint);
266 _joint_max[0] = 0.7001;
267 _joint_max[1] = 0.5201;
268 _joint_max[2] = 0.4601;
269 _joint_max[3] = 2.7301;
270 _joint_max[4] = 2.4801;
271 _joint_max[5] = 1.5901;
272 }
273
274 /*!
275 Method which initialises the parameters linked to the display part.
276 */
initDisplay()277 void vpSimulatorAfma6::initDisplay()
278 {
279 robotArms = NULL;
280 robotArms = new Bound_scene[6];
281 initArms();
282 setExternalCameraPosition(vpHomogeneousMatrix(0, 0, 0, 0, 0, vpMath::rad(180)) *
283 vpHomogeneousMatrix(-0.1, 0, 4, vpMath::rad(90), 0, 0));
284 cameraParam.initPersProjWithoutDistortion(558.5309599, 556.055053, 320, 240);
285 setExternalCameraParameters(cameraParam);
286 vpCameraParameters tmp;
287 getCameraParameters(tmp, 640, 480);
288 px_int = tmp.get_px();
289 py_int = tmp.get_py();
290 sceneInitialized = true;
291 }
292
293 /*!
294
295 Initialize the robot kinematics with the extrinsic calibration
296 parameters associated to a specific camera.
297
298 The eMc parameters depend on the camera.
299
300 \warning Only perspective projection without distortion is available!
301
302 \param tool : Tool to use. Note that the generic camera is not handled.
303
304 \param proj_model : Projection model associated to the camera.
305
306 \sa vpCameraParameters, init()
307 */
init(vpAfma6::vpAfma6ToolType tool,vpCameraParameters::vpCameraParametersProjType proj_model)308 void vpSimulatorAfma6::init(vpAfma6::vpAfma6ToolType tool, vpCameraParameters::vpCameraParametersProjType proj_model)
309 {
310 this->projModel = proj_model;
311 unsigned int name_length = 30; // the size of this kind of string "/afma6_tool_vacuum.bnd"
312 if (arm_dir.size() > FILENAME_MAX)
313 throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
314 unsigned int full_length = (unsigned int)arm_dir.size() + name_length;
315 if (full_length > FILENAME_MAX)
316 throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
317
318 // Use here default values of the robot constant parameters.
319 switch (tool) {
320 case vpAfma6::TOOL_CCMOP: {
321 _erc[0] = vpMath::rad(164.35); // rx
322 _erc[1] = vpMath::rad(89.64); // ry
323 _erc[2] = vpMath::rad(-73.05); // rz
324 _etc[0] = 0.0117; // tx
325 _etc[1] = 0.0033; // ty
326 _etc[2] = 0.2272; // tz
327
328 setCameraParameters(vpCameraParameters(1109.5735473989, 1112.1520168160, 320, 240));
329
330 if (robotArms != NULL) {
331 while (get_displayBusy())
332 vpTime::wait(2);
333 free_Bound_scene(&(robotArms[5]));
334 char *name_arm = new char[full_length];
335 strcpy(name_arm, arm_dir.c_str());
336 strcat(name_arm, "/afma6_tool_ccmop.bnd");
337 set_scene(name_arm, robotArms + 5, 1.0);
338 set_displayBusy(false);
339 delete[] name_arm;
340 }
341 break;
342 }
343 case vpAfma6::TOOL_GRIPPER: {
344 _erc[0] = vpMath::rad(88.33); // rx
345 _erc[1] = vpMath::rad(72.07); // ry
346 _erc[2] = vpMath::rad(2.53); // rz
347 _etc[0] = 0.0783; // tx
348 _etc[1] = 0.1234; // ty
349 _etc[2] = 0.1638; // tz
350
351 setCameraParameters(vpCameraParameters(852.6583228197, 854.8084224761, 320, 240));
352
353 if (robotArms != NULL) {
354 while (get_displayBusy())
355 vpTime::wait(2);
356 free_Bound_scene(&(robotArms[5]));
357 char *name_arm = new char[full_length];
358 strcpy(name_arm, arm_dir.c_str());
359 strcat(name_arm, "/afma6_tool_gripper.bnd");
360 set_scene(name_arm, robotArms + 5, 1.0);
361 set_displayBusy(false);
362 delete[] name_arm;
363 }
364 break;
365 }
366 case vpAfma6::TOOL_VACUUM: {
367 _erc[0] = vpMath::rad(90.40); // rx
368 _erc[1] = vpMath::rad(75.11); // ry
369 _erc[2] = vpMath::rad(0.18); // rz
370 _etc[0] = 0.0038; // tx
371 _etc[1] = 0.1281; // ty
372 _etc[2] = 0.1658; // tz
373
374 setCameraParameters(vpCameraParameters(853.4876600807, 856.0339170706, 320, 240));
375
376 if (robotArms != NULL) {
377 while (get_displayBusy())
378 vpTime::wait(2);
379 free_Bound_scene(&(robotArms[5]));
380
381 char *name_arm = new char[full_length];
382
383 strcpy(name_arm, arm_dir.c_str());
384 strcat(name_arm, "/afma6_tool_vacuum.bnd");
385 set_scene(name_arm, robotArms + 5, 1.0);
386 set_displayBusy(false);
387 delete[] name_arm;
388 }
389 break;
390 }
391 case vpAfma6::TOOL_CUSTOM: {
392 std::cout << "The custom tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
393 break;
394 }
395 case vpAfma6::TOOL_INTEL_D435_CAMERA: {
396 std::cout << "The Intel D435 camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
397 break;
398 }
399 case vpAfma6::TOOL_GENERIC_CAMERA: {
400 std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
401 break;
402 }
403 }
404
405 vpRotationMatrix eRc(_erc);
406 this->_eMc.buildFrom(_etc, eRc);
407
408 setToolType(tool);
409 return;
410 }
411
412 /*!
413 Get the current intrinsic camera parameters obtained by calibration.
414
415 \param cam : In output, camera parameters to fill.
416 \param image_width : Image width used to compute camera calibration.
417 \param image_height : Image height used to compute camera calibration.
418
419 \warning The image size must be : 640x480 !
420 */
421
getCameraParameters(vpCameraParameters & cam,const unsigned int & image_width,const unsigned int & image_height)422 void vpSimulatorAfma6::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
423 const unsigned int &image_height)
424 {
425 if (toolCustom) {
426 cam.initPersProjWithoutDistortion(px_int, py_int, image_width / 2, image_height / 2);
427 }
428 // Set default parameters
429 switch (getToolType()) {
430 case vpAfma6::TOOL_CCMOP: {
431 // Set default intrinsic camera parameters for 640x480 images
432 if (image_width == 640 && image_height == 480) {
433 std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\""
434 << std::endl;
435 cam.initPersProjWithoutDistortion(1109.5735473989, 1112.1520168160, 320, 240);
436 } else {
437 vpTRACE("Cannot get default intrinsic camera parameters for this image "
438 "resolution");
439 }
440 break;
441 }
442 case vpAfma6::TOOL_GRIPPER: {
443 // Set default intrinsic camera parameters for 640x480 images
444 if (image_width == 640 && image_height == 480) {
445 std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\""
446 << std::endl;
447 cam.initPersProjWithoutDistortion(852.6583228197, 854.8084224761, 320, 240);
448 } else {
449 vpTRACE("Cannot get default intrinsic camera parameters for this image "
450 "resolution");
451 }
452 break;
453 }
454 case vpAfma6::TOOL_CUSTOM: {
455 std::cout << "The generic tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
456 break;
457 }
458 case vpAfma6::TOOL_INTEL_D435_CAMERA: {
459 std::cout << "The Intel D435 camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
460 break;
461 }
462 case vpAfma6::TOOL_GENERIC_CAMERA: {
463 std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
464 break;
465 }
466 case vpAfma6::TOOL_VACUUM: {
467 std::cout << "The vacuum tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
468 break;
469 }
470 default:
471 vpERROR_TRACE("This error should not occur!");
472 break;
473 }
474 return;
475 }
476
477 /*!
478 Get the current intrinsic camera parameters obtained by calibration.
479
480 \param cam : In output, camera parameters to fill.
481 \param I_ : A B&W image send by the current camera in use.
482
483 \warning The image size must be : 640x480 !
484 */
getCameraParameters(vpCameraParameters & cam,const vpImage<unsigned char> & I_)485 void vpSimulatorAfma6::getCameraParameters(vpCameraParameters &cam, const vpImage<unsigned char> &I_)
486 {
487 getCameraParameters(cam, I_.getWidth(), I_.getHeight());
488 }
489
490 /*!
491 Get the current intrinsic camera parameters obtained by calibration.
492
493 \param cam : In output, camera parameters to fill.
494 \param I_ : A B&W image send by the current camera in use.
495
496 \warning The image size must be : 640x480 !
497 */
getCameraParameters(vpCameraParameters & cam,const vpImage<vpRGBa> & I_)498 void vpSimulatorAfma6::getCameraParameters(vpCameraParameters &cam, const vpImage<vpRGBa> &I_)
499 {
500 getCameraParameters(cam, I_.getWidth(), I_.getHeight());
501 }
502
503 /*!
504 Set the intrinsic camera parameters.
505
506 \param cam : The desired camera parameters.
507 */
setCameraParameters(const vpCameraParameters & cam)508 void vpSimulatorAfma6::setCameraParameters(const vpCameraParameters &cam)
509 {
510 px_int = cam.get_px();
511 py_int = cam.get_py();
512 toolCustom = true;
513 }
514
515 /*!
516 Method lauched by the thread to compute the position of the robot in the
517 articular frame.
518 */
updateArticularPosition()519 void vpSimulatorAfma6::updateArticularPosition()
520 {
521 double tcur_1 = tcur; // temporary variable used to store the last time
522 // since the last command
523
524 while (!robotStop) {
525 // Get current time
526 tprev = tcur_1;
527 tcur = vpTime::measureTimeMs();
528
529 if (setVelocityCalled || !constantSamplingTimeMode) {
530 setVelocityCalled = false;
531
532 computeArticularVelocity();
533
534 double ellapsedTime = (tcur - tprev) * 1e-3;
535 if (constantSamplingTimeMode) { // if we want a constant velocity, we
536 // force the ellapsed time to the given
537 // samplingTime
538 ellapsedTime = getSamplingTime(); // in second
539 }
540
541 vpColVector articularCoordinates = get_artCoord();
542 vpColVector articularVelocities = get_artVel();
543
544 if (jointLimit) {
545 double art = articularCoordinates[jointLimitArt - 1] + ellapsedTime * articularVelocities[jointLimitArt - 1];
546 if (art <= _joint_min[jointLimitArt - 1] || art >= _joint_max[jointLimitArt - 1]) {
547 if (verbose_) {
548 std::cout << "Joint " << jointLimitArt - 1
549 << " reaches a limit: " << vpMath::deg(_joint_min[jointLimitArt - 1]) << " < " << vpMath::deg(art)
550 << " < " << vpMath::deg(_joint_max[jointLimitArt - 1]) << std::endl;
551 }
552
553 articularVelocities = 0.0;
554 } else
555 jointLimit = false;
556 }
557
558 articularCoordinates[0] = articularCoordinates[0] + ellapsedTime * articularVelocities[0];
559 articularCoordinates[1] = articularCoordinates[1] + ellapsedTime * articularVelocities[1];
560 articularCoordinates[2] = articularCoordinates[2] + ellapsedTime * articularVelocities[2];
561 articularCoordinates[3] = articularCoordinates[3] + ellapsedTime * articularVelocities[3];
562 articularCoordinates[4] = articularCoordinates[4] + ellapsedTime * articularVelocities[4];
563 articularCoordinates[5] = articularCoordinates[5] + ellapsedTime * articularVelocities[5];
564
565 int jl = isInJointLimit();
566
567 if (jl != 0 && jointLimit == false) {
568 if (jl < 0)
569 ellapsedTime = (_joint_min[(unsigned int)(-jl - 1)] - articularCoordinates[(unsigned int)(-jl - 1)]) /
570 (articularVelocities[(unsigned int)(-jl - 1)]);
571 else
572 ellapsedTime = (_joint_max[(unsigned int)(jl - 1)] - articularCoordinates[(unsigned int)(jl - 1)]) /
573 (articularVelocities[(unsigned int)(jl - 1)]);
574
575 for (unsigned int i = 0; i < 6; i++)
576 articularCoordinates[i] = articularCoordinates[i] + ellapsedTime * articularVelocities[i];
577
578 jointLimit = true;
579 jointLimitArt = (unsigned int)fabs((double)jl);
580 }
581
582 set_artCoord(articularCoordinates);
583 set_artVel(articularVelocities);
584
585 compute_fMi();
586
587 if (displayAllowed) {
588 vpDisplay::display(I);
589 vpDisplay::displayFrame(I, getExternalCameraPosition(), cameraParam, 0.2, vpColor::none, thickness_);
590 vpDisplay::displayFrame(I, getExternalCameraPosition() * fMi[7], cameraParam, 0.1, vpColor::none, thickness_);
591 }
592
593 if (displayType == MODEL_3D && displayAllowed) {
594 while (get_displayBusy())
595 vpTime::wait(2);
596 vpSimulatorAfma6::getExternalImage(I);
597 set_displayBusy(false);
598 }
599
600 if (0 /*displayType == MODEL_DH && displayAllowed*/) {
601 vpHomogeneousMatrix fMit[8];
602 get_fMi(fMit);
603
604 // vpDisplay::displayFrame(I,getExternalCameraPosition
605 // ()*fMi[6],cameraParam,0.2,vpColor::none);
606
607 vpImagePoint iP, iP_1;
608 vpPoint pt(0, 0, 0);
609
610 pt.track(getExternalCameraPosition());
611 vpMeterPixelConversion::convertPoint(cameraParam, pt.get_x(), pt.get_y(), iP_1);
612 pt.track(getExternalCameraPosition() * fMit[0]);
613 vpMeterPixelConversion::convertPoint(cameraParam, pt.get_x(), pt.get_y(), iP);
614 vpDisplay::displayLine(I, iP_1, iP, vpColor::green, thickness_);
615 for (unsigned int k = 1; k < 7; k++) {
616 pt.track(getExternalCameraPosition() * fMit[k - 1]);
617 vpMeterPixelConversion::convertPoint(cameraParam, pt.get_x(), pt.get_y(), iP_1);
618
619 pt.track(getExternalCameraPosition() * fMit[k]);
620 vpMeterPixelConversion::convertPoint(cameraParam, pt.get_x(), pt.get_y(), iP);
621
622 vpDisplay::displayLine(I, iP_1, iP, vpColor::green, thickness_);
623 }
624 vpDisplay::displayCamera(I, getExternalCameraPosition() * fMit[7], cameraParam, 0.1, vpColor::green,
625 thickness_);
626 }
627
628 vpDisplay::flush(I);
629
630 vpTime::wait(tcur, 1000 * getSamplingTime());
631 tcur_1 = tcur;
632 } else {
633 vpTime::wait(tcur, vpTime::getMinTimeForUsleepCall());
634 }
635 }
636 }
637
638 /*!
639 Compute the pose between the robot reference frame and the frames used used
640 to compute the Denavit-Hartenberg representation. The last element of the
641 table corresponds to the pose between the reference frame and the camera
642 frame.
643
644 To compute the diferent homogeneous matrices, this function needs the
645 articular coordinates as input.
646
647 Finally the output is a table of 8 elements : \f$ fM1 \f$,\f$ fM2 \f$,\f$
648 fM3 \f$,\f$ fM4 \f$,\f$ fM5 \f$,\f$ fM6 = fMw \f$,\f$ fM7 = fMe \f$ and \f$
649 fMc \f$ - where w is for wrist and e for effector-.
650 */
compute_fMi()651 void vpSimulatorAfma6::compute_fMi()
652 {
653 // vpColVector q = get_artCoord();
654 vpColVector q(6); //; = get_artCoord();
655 q = get_artCoord();
656
657 vpHomogeneousMatrix fMit[8];
658
659 double q1 = q[0];
660 double q2 = q[1];
661 double q3 = q[2];
662 double q4 = q[3];
663 double q5 = q[4];
664 double q6 = q[5];
665
666 double c4 = cos(q4);
667 double s4 = sin(q4);
668 double c5 = cos(q5);
669 double s5 = sin(q5);
670 double c6 = cos(q6);
671 double s6 = sin(q6);
672
673 fMit[0][0][0] = 1;
674 fMit[0][1][0] = 0;
675 fMit[0][2][0] = 0;
676 fMit[0][0][1] = 0;
677 fMit[0][1][1] = 1;
678 fMit[0][2][1] = 0;
679 fMit[0][0][2] = 0;
680 fMit[0][1][2] = 0;
681 fMit[0][2][2] = 1;
682 fMit[0][0][3] = q1;
683 fMit[0][1][3] = 0;
684 fMit[0][2][3] = 0;
685
686 fMit[1][0][0] = 1;
687 fMit[1][1][0] = 0;
688 fMit[1][2][0] = 0;
689 fMit[1][0][1] = 0;
690 fMit[1][1][1] = 1;
691 fMit[1][2][1] = 0;
692 fMit[1][0][2] = 0;
693 fMit[1][1][2] = 0;
694 fMit[1][2][2] = 1;
695 fMit[1][0][3] = q1;
696 fMit[1][1][3] = q2;
697 fMit[1][2][3] = 0;
698
699 fMit[2][0][0] = 1;
700 fMit[2][1][0] = 0;
701 fMit[2][2][0] = 0;
702 fMit[2][0][1] = 0;
703 fMit[2][1][1] = 1;
704 fMit[2][2][1] = 0;
705 fMit[2][0][2] = 0;
706 fMit[2][1][2] = 0;
707 fMit[2][2][2] = 1;
708 fMit[2][0][3] = q1;
709 fMit[2][1][3] = q2;
710 fMit[2][2][3] = q3;
711
712 fMit[3][0][0] = s4;
713 fMit[3][1][0] = -c4;
714 fMit[3][2][0] = 0;
715 fMit[3][0][1] = c4;
716 fMit[3][1][1] = s4;
717 fMit[3][2][1] = 0;
718 fMit[3][0][2] = 0;
719 fMit[3][1][2] = 0;
720 fMit[3][2][2] = 1;
721 fMit[3][0][3] = q1;
722 fMit[3][1][3] = q2;
723 fMit[3][2][3] = q3;
724
725 fMit[4][0][0] = s4 * s5;
726 fMit[4][1][0] = -c4 * s5;
727 fMit[4][2][0] = c5;
728 fMit[4][0][1] = s4 * c5;
729 fMit[4][1][1] = -c4 * c5;
730 fMit[4][2][1] = -s5;
731 fMit[4][0][2] = c4;
732 fMit[4][1][2] = s4;
733 fMit[4][2][2] = 0;
734 fMit[4][0][3] = c4 * this->_long_56 + q1;
735 fMit[4][1][3] = s4 * this->_long_56 + q2;
736 fMit[4][2][3] = q3;
737
738 fMit[5][0][0] = s4 * s5 * c6 + c4 * s6;
739 fMit[5][1][0] = -c4 * s5 * c6 + s4 * s6;
740 fMit[5][2][0] = c5 * c6;
741 fMit[5][0][1] = -s4 * s5 * s6 + c4 * c6;
742 fMit[5][1][1] = c4 * s5 * s6 + s4 * c6;
743 fMit[5][2][1] = -c5 * s6;
744 fMit[5][0][2] = -s4 * c5;
745 fMit[5][1][2] = c4 * c5;
746 fMit[5][2][2] = s5;
747 fMit[5][0][3] = c4 * this->_long_56 + q1;
748 fMit[5][1][3] = s4 * this->_long_56 + q2;
749 fMit[5][2][3] = q3;
750
751 fMit[6][0][0] = fMit[5][0][0];
752 fMit[6][1][0] = fMit[5][1][0];
753 fMit[6][2][0] = fMit[5][2][0];
754 fMit[6][0][1] = fMit[5][0][1];
755 fMit[6][1][1] = fMit[5][1][1];
756 fMit[6][2][1] = fMit[5][2][1];
757 fMit[6][0][2] = fMit[5][0][2];
758 fMit[6][1][2] = fMit[5][1][2];
759 fMit[6][2][2] = fMit[5][2][2];
760 fMit[6][0][3] = fMit[5][0][3];
761 fMit[6][1][3] = fMit[5][1][3];
762 fMit[6][2][3] = fMit[5][2][3];
763
764 // vpHomogeneousMatrix cMe;
765 // get_cMe(cMe);
766 // cMe = cMe.inverse();
767 // fMit[7] = fMit[6] * cMe;
768 vpAfma6::get_fMc(q, fMit[7]);
769
770 #if defined(_WIN32)
771 #if defined(WINRT_8_1)
772 WaitForSingleObjectEx(mutex_fMi, INFINITE, FALSE);
773 #else // pure win32
774 WaitForSingleObject(mutex_fMi, INFINITE);
775 #endif
776 for (int i = 0; i < 8; i++)
777 fMi[i] = fMit[i];
778 ReleaseMutex(mutex_fMi);
779 #elif defined(VISP_HAVE_PTHREAD)
780 pthread_mutex_lock(&mutex_fMi);
781 for (int i = 0; i < 8; i++)
782 fMi[i] = fMit[i];
783 pthread_mutex_unlock(&mutex_fMi);
784 #endif
785 }
786
787 /*!
788 Change the robot state.
789
790 \param newState : New requested robot state.
791 */
setRobotState(vpRobot::vpRobotStateType newState)792 vpRobot::vpRobotStateType vpSimulatorAfma6::setRobotState(vpRobot::vpRobotStateType newState)
793 {
794 switch (newState) {
795 case vpRobot::STATE_STOP: {
796 // Start primitive STOP only if the current state is Velocity
797 if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState()) {
798 stopMotion();
799 }
800 break;
801 }
802 case vpRobot::STATE_POSITION_CONTROL: {
803 if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState()) {
804 std::cout << "Change the control mode from velocity to position control.\n";
805 stopMotion();
806 } else {
807 // std::cout << "Change the control mode from stop to position
808 // control.\n";
809 }
810 break;
811 }
812 case vpRobot::STATE_VELOCITY_CONTROL: {
813 if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState()) {
814 std::cout << "Change the control mode from stop to velocity control.\n";
815 }
816 break;
817 }
818 case vpRobot::STATE_ACCELERATION_CONTROL:
819 default:
820 break;
821 }
822
823 return vpRobot::setRobotState(newState);
824 }
825
826 /*!
827 Apply a velocity to the robot.
828
829 \param frame : Control frame in which the velocity is expressed. Velocities
830 could be expressed in articular, camera frame, reference frame or mixt
831 frame.
832
833 \param vel : Velocity vector. The size of this vector
834 is always 6.
835
836 - In articular, \f$ vel = [\dot{q}_1, \dot{q}_2, \dot{q}_3, \dot{q}_4,
837 \dot{q}_5, \dot{q}_6]^t \f$ correspond to joint velocities in rad/s.
838
839 - In camera frame, \f$ vel = [^{c} v_x, ^{c} v_y, ^{c} v_z, ^{c}
840 \omega_x, ^{c} \omega_y, ^{c} \omega_z]^t \f$ is a velocity twist vector
841 expressed in the camera frame, with translations velocities \f$ ^{c} v_x, ^{c}
842 v_y, ^{c} v_z \f$ in m/s and rotation velocities \f$ ^{c}\omega_x, ^{c}
843 \omega_y, ^{c} \omega_z \f$ in rad/s.
844
845 - In reference frame, \f$ vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{r}
846 \omega_x, ^{r} \omega_y, ^{r} \omega_z]^t \f$ is a velocity twist vector
847 expressed in the reference frame, with translations velocities \f$ ^{c} v_x,
848 ^{c} v_y, ^{c} v_z \f$ in m/s and rotation velocities \f$ ^{c}\omega_x, ^{c}
849 \omega_y, ^{c} \omega_z \f$ in rad/s.
850
851 - In mixt frame, \f$ vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{c} \omega_x,
852 ^{c} \omega_y, ^{c} \omega_z]^t \f$ is a velocity twist vector where,
853 translations \f$ ^{r} v_x, ^{r} v_y, ^{r} v_z \f$ are expressed in the
854 reference frame in m/s and rotations \f$ ^{c} \omega_x, ^{c} \omega_y, ^{c}
855 \omega_z \f$ in the camera frame in rad/s.
856
857 \exception vpRobotException::wrongStateError : If a the robot is not
858 configured to handle a velocity. The robot can handle a velocity only if the
859 velocity control mode is set. For that, call setRobotState(
860 vpRobot::STATE_VELOCITY_CONTROL) before setVelocity().
861
862 \warning Velocities could be saturated if one of them exceed the
863 maximal autorized speed (see vpRobot::maxTranslationVelocity and
864 vpRobot::maxRotationVelocity). To change these values use
865 setMaxTranslationVelocity() and setMaxRotationVelocity().
866
867 \code
868 #include <visp3/core/vpColVector.h>
869 #include <visp3/core/vpMath.h>
870 #include <visp3/robot/vpSimulatorAfma6.h>
871
872 int main()
873 {
874 vpSimulatorAfma6 robot;
875
876 vpColVector qvel(6);
877 // Set a joint velocity
878 qvel[0] = 0.1; // Joint 1 velocity in m/s
879 qvel[1] = 0.1; // Joint 2 velocity in m/s
880 qvel[2] = 0.1; // Joint 3 velocity in m/s
881 qvel[3] = M_PI/8; // Joint 4 velocity in rad/s
882 qvel[4] = 0; // Joint 5 velocity in rad/s
883 qvel[5] = 0; // Joint 6 velocity in rad/s
884
885 // Initialize the controller to position control
886 robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
887
888 for ( ; ; ) {
889 // Apply a velocity in the joint space
890 robot.setVelocity(vpRobot::ARTICULAR_FRAME, qvel);
891
892 // Compute new velocities qvel...
893 }
894 // Stop the robot
895 robot.setRobotState(vpRobot::STATE_STOP);
896 }
897 \endcode
898 */
setVelocity(const vpRobot::vpControlFrameType frame,const vpColVector & vel)899 void vpSimulatorAfma6::setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
900 {
901 if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState()) {
902 vpERROR_TRACE("Cannot send a velocity to the robot "
903 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
904 throw vpRobotException(vpRobotException::wrongStateError,
905 "Cannot send a velocity to the robot "
906 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
907 }
908
909 vpColVector vel_sat(6);
910
911 double scale_sat = 1;
912 double vel_trans_max = getMaxTranslationVelocity();
913 double vel_rot_max = getMaxRotationVelocity();
914
915 double vel_abs; // Absolute value
916
917 // Velocity saturation
918 switch (frame) {
919 // saturation in cartesian space
920 case vpRobot::CAMERA_FRAME:
921 case vpRobot::REFERENCE_FRAME: {
922 if (vel.getRows() != 6) {
923 vpERROR_TRACE("The velocity vector must have a size of 6 !!!!");
924 throw;
925 }
926
927 for (unsigned int i = 0; i < 3; ++i) {
928 vel_abs = fabs(vel[i]);
929 if (vel_abs > vel_trans_max && !jointLimit) {
930 vel_trans_max = vel_abs;
931 vpERROR_TRACE("Excess velocity %g m/s in TRANSLATION "
932 "(axis nr. %d).",
933 vel[i], i + 1);
934 }
935
936 vel_abs = fabs(vel[i + 3]);
937 if (vel_abs > vel_rot_max && !jointLimit) {
938 vel_rot_max = vel_abs;
939 vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
940 "(axis nr. %d).",
941 vel[i + 3], i + 4);
942 }
943 }
944
945 double scale_trans_sat = 1;
946 double scale_rot_sat = 1;
947 if (vel_trans_max > getMaxTranslationVelocity())
948 scale_trans_sat = getMaxTranslationVelocity() / vel_trans_max;
949
950 if (vel_rot_max > getMaxRotationVelocity())
951 scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
952
953 if ((scale_trans_sat < 1) || (scale_rot_sat < 1)) {
954 if (scale_trans_sat < scale_rot_sat)
955 scale_sat = scale_trans_sat;
956 else
957 scale_sat = scale_rot_sat;
958 }
959 break;
960 }
961
962 // saturation in joint space
963 case vpRobot::ARTICULAR_FRAME: {
964 if (vel.getRows() != 6) {
965 vpERROR_TRACE("The velocity vector must have a size of 6 !!!!");
966 throw;
967 }
968 for (unsigned int i = 0; i < 6; ++i) {
969 vel_abs = fabs(vel[i]);
970 if (vel_abs > vel_rot_max && !jointLimit) {
971 vel_rot_max = vel_abs;
972 vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
973 "(axis nr. %d).",
974 vel[i], i + 1);
975 }
976 }
977 double scale_rot_sat = 1;
978 if (vel_rot_max > getMaxRotationVelocity())
979 scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
980 if (scale_rot_sat < 1)
981 scale_sat = scale_rot_sat;
982 break;
983 }
984 case vpRobot::MIXT_FRAME: {
985 throw vpRobotException(vpRobotException::wrongStateError, "Cannot set a velocity in MIXT_FRAME frame:"
986 "functionality not implemented");
987 }
988 case vpRobot::END_EFFECTOR_FRAME: {
989 throw vpRobotException(vpRobotException::wrongStateError, "Cannot set a velocity in END_EFFECTOT_FRAME frame:"
990 "functionality not implemented");
991 }
992 }
993
994 set_velocity(vel * scale_sat);
995 setRobotFrame(frame);
996 setVelocityCalled = true;
997 }
998
999 /*!
1000 Compute the articular velocity relative to the velocity in another frame.
1001 */
computeArticularVelocity()1002 void vpSimulatorAfma6::computeArticularVelocity()
1003 {
1004 vpRobot::vpControlFrameType frame = getRobotFrame();
1005
1006 double vel_rot_max = getMaxRotationVelocity();
1007
1008 vpColVector articularCoordinates = get_artCoord();
1009 vpColVector velocityframe = get_velocity();
1010 vpColVector articularVelocity;
1011
1012 switch (frame) {
1013 case vpRobot::CAMERA_FRAME: {
1014 vpMatrix eJe_;
1015 vpVelocityTwistMatrix eVc(_eMc);
1016 vpAfma6::get_eJe(articularCoordinates, eJe_);
1017 eJe_ = eJe_.pseudoInverse();
1018 if (singularityManagement)
1019 singularityTest(articularCoordinates, eJe_);
1020 articularVelocity = eJe_ * eVc * velocityframe;
1021 set_artVel(articularVelocity);
1022 break;
1023 }
1024 case vpRobot::REFERENCE_FRAME: {
1025 vpMatrix fJe_;
1026 vpAfma6::get_fJe(articularCoordinates, fJe_);
1027 fJe_ = fJe_.pseudoInverse();
1028 if (singularityManagement)
1029 singularityTest(articularCoordinates, fJe_);
1030 articularVelocity = fJe_ * velocityframe;
1031 set_artVel(articularVelocity);
1032 break;
1033 }
1034 case vpRobot::ARTICULAR_FRAME: {
1035 articularVelocity = velocityframe;
1036 set_artVel(articularVelocity);
1037 break;
1038 }
1039 case vpRobot::END_EFFECTOR_FRAME:
1040 case vpRobot::MIXT_FRAME: {
1041 break;
1042 }
1043 }
1044
1045 switch (frame) {
1046 case vpRobot::CAMERA_FRAME:
1047 case vpRobot::REFERENCE_FRAME: {
1048 for (unsigned int i = 0; i < 6; ++i) {
1049 double vel_abs = fabs(articularVelocity[i]);
1050 if (vel_abs > vel_rot_max && !jointLimit) {
1051 vel_rot_max = vel_abs;
1052 vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
1053 "(axis nr. %d).",
1054 articularVelocity[i], i + 1);
1055 }
1056 }
1057 double scale_rot_sat = 1;
1058 double scale_sat = 1;
1059 if (vel_rot_max > getMaxRotationVelocity())
1060 scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
1061 if (scale_rot_sat < 1)
1062 scale_sat = scale_rot_sat;
1063
1064 set_artVel(articularVelocity * scale_sat);
1065 break;
1066 }
1067 case vpRobot::ARTICULAR_FRAME:
1068 case vpRobot::END_EFFECTOR_FRAME:
1069 case vpRobot::MIXT_FRAME: {
1070 break;
1071 }
1072 }
1073 }
1074
1075 /*!
1076 Get the robot velocities.
1077
1078 \param frame : Frame in wich velocities are mesured.
1079
1080 \param vel : Measured velocities. Translations are expressed in m/s
1081 and rotations in rad/s.
1082
1083 \warning In camera frame, reference frame and mixt frame, the representation
1084 of the rotation is ThetaU. In that cases, \f$velocity = [\dot x, \dot y,
1085 \dot z, \dot {\theta U}_x, \dot {\theta U}_y, \dot {\theta U}_z]\f$.
1086
1087 \code
1088 #include <visp3/core/vpColVector.h>
1089 #include <visp3/robot/vpSimulatorAfma6.h>
1090
1091 int main()
1092 {
1093 // Set requested joint velocities
1094 vpColVector q_dot(6);
1095 q_dot[0] = 0.1; // Joint 1 velocity in m/s
1096 q_dot[1] = 0.2; // Joint 2 velocity in m/s
1097 q_dot[2] = 0.3; // Joint 3 velocity in m/s
1098 q_dot[3] = M_PI/8; // Joint 4 velocity in rad/s
1099 q_dot[4] = M_PI/4; // Joint 5 velocity in rad/s
1100 q_dot[5] = M_PI/16;// Joint 6 velocity in rad/s
1101
1102 vpSimulatorAfma6 robot;
1103
1104 robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
1105
1106 // Moves the joint in velocity
1107 robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
1108
1109 // Initialisation of the velocity measurement
1110 vpColVector q_dot_mes; // Measured velocities
1111
1112 for ( ; ; ) {
1113 robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes);
1114 vpTime::wait(40); // wait 40 ms
1115 // here q_dot_mes is equal to [0.1, 0.2, 0.3, M_PI/8, M_PI/4, M_PI/16]
1116 }
1117 }
1118 \endcode
1119 */
getVelocity(const vpRobot::vpControlFrameType frame,vpColVector & vel)1120 void vpSimulatorAfma6::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &vel)
1121 {
1122 vel.resize(6);
1123
1124 vpColVector articularCoordinates = get_artCoord();
1125 vpColVector articularVelocity = get_artVel();
1126
1127 switch (frame) {
1128 case vpRobot::CAMERA_FRAME: {
1129 vpMatrix eJe_;
1130 vpVelocityTwistMatrix cVe(_eMc);
1131 vpAfma6::get_eJe(articularCoordinates, eJe_);
1132 vel = cVe * eJe_ * articularVelocity;
1133 break;
1134 }
1135 case vpRobot::ARTICULAR_FRAME: {
1136 vel = articularVelocity;
1137 break;
1138 }
1139 case vpRobot::REFERENCE_FRAME: {
1140 vpMatrix fJe_;
1141 vpAfma6::get_fJe(articularCoordinates, fJe_);
1142 vel = fJe_ * articularVelocity;
1143 break;
1144 }
1145 case vpRobot::END_EFFECTOR_FRAME:
1146 case vpRobot::MIXT_FRAME: {
1147 break;
1148 }
1149 default: {
1150 vpERROR_TRACE("Error in spec of vpRobot. "
1151 "Case not taken in account.");
1152 return;
1153 }
1154 }
1155 }
1156
1157 /*!
1158 Get the robot time stamped velocities.
1159
1160 \param frame : Frame in wich velocities are mesured.
1161
1162 \param vel : Measured velocities. Translations are expressed in m/s
1163 and rotations in rad/s.
1164
1165 \param timestamp : Unix time in second since January 1st 1970.
1166
1167 \warning In camera frame, reference frame and mixt frame, the representation
1168 of the rotation is ThetaU. In that cases, \f$velocity = [\dot x, \dot y,
1169 \dot z, \dot {\theta U}_x, \dot {\theta U}_y, \dot {\theta U}_z]\f$.
1170
1171 \sa getVelocity(const vpRobot::vpControlFrameType frame, vpColVector & vel)
1172 */
getVelocity(const vpRobot::vpControlFrameType frame,vpColVector & vel,double & timestamp)1173 void vpSimulatorAfma6::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &vel, double ×tamp)
1174 {
1175 timestamp = vpTime::measureTimeSecond();
1176 getVelocity(frame, vel);
1177 }
1178
1179 /*!
1180 Get the robot velocities.
1181
1182 \param frame : Frame in wich velocities are mesured.
1183
1184 \return Measured velocities. Translations are expressed in m/s
1185 and rotations in rad/s.
1186
1187 \code
1188 #include <visp3/core/vpColVector.h>
1189 #include <visp3/robot/vpSimulatorAfma6.h>
1190
1191 int main()
1192 {
1193 // Set requested joint velocities
1194 vpColVector q_dot(6);
1195 q_dot[0] = 0.1; // Joint 1 velocity in rad/s
1196 q_dot[1] = 0.2; // Joint 2 velocity in rad/s
1197 q_dot[2] = 0.3; // Joint 3 velocity in rad/s
1198 q_dot[3] = M_PI/8; // Joint 4 velocity in rad/s
1199 q_dot[4] = M_PI/4; // Joint 5 velocity in rad/s
1200 q_dot[5] = M_PI/16;// Joint 6 velocity in rad/s
1201
1202 vpSimulatorAfma6 robot;
1203
1204 robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
1205
1206 // Moves the joint in velocity
1207 robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
1208
1209 // Initialisation of the velocity measurement
1210 vpColVector q_dot_mes; // Measured velocities
1211
1212 for ( ; ; ) {
1213 q_dot_mes = robot.getVelocity(vpRobot::ARTICULAR_FRAME);
1214 vpTime::wait(40); // wait 40 ms
1215 // here q_dot_mes is equal to [0.1, 0.2, 0.3, M_PI/8, M_PI/4, M_PI/16]
1216 }
1217 }
1218 \endcode
1219 */
getVelocity(vpRobot::vpControlFrameType frame)1220 vpColVector vpSimulatorAfma6::getVelocity(vpRobot::vpControlFrameType frame)
1221 {
1222 vpColVector vel(6);
1223 getVelocity(frame, vel);
1224
1225 return vel;
1226 }
1227
1228 /*!
1229 Get the time stamped robot velocities.
1230
1231 \param frame : Frame in wich velocities are mesured.
1232
1233 \param timestamp : Unix time in second since January 1st 1970.
1234
1235 \return Measured velocities. Translations are expressed in m/s
1236 and rotations in rad/s.
1237
1238 \sa getVelocity(vpRobot::vpControlFrameType frame)
1239 */
getVelocity(vpRobot::vpControlFrameType frame,double & timestamp)1240 vpColVector vpSimulatorAfma6::getVelocity(vpRobot::vpControlFrameType frame, double ×tamp)
1241 {
1242 timestamp = vpTime::measureTimeSecond();
1243 vpColVector vel(6);
1244 getVelocity(frame, vel);
1245
1246 return vel;
1247 }
1248
findHighestPositioningSpeed(vpColVector & q)1249 void vpSimulatorAfma6::findHighestPositioningSpeed(vpColVector &q)
1250 {
1251 double vel_rot_max = getMaxRotationVelocity();
1252 double velmax = fabs(q[0]);
1253 for (unsigned int i = 1; i < 6; i++) {
1254 if (velmax < fabs(q[i]))
1255 velmax = fabs(q[i]);
1256 }
1257
1258 double alpha = (getPositioningVelocity() * vel_rot_max) / (velmax * 100);
1259 q = q * alpha;
1260 }
1261
1262 /*!
1263 Move to an absolute position with a given percent of max velocity.
1264 The percent of max velocity is to set with setPositioningVelocity().
1265 The position to reach can be specified in joint coordinates, in the
1266 camera frame or in the reference frame.
1267
1268 \warning This method is blocking. It returns only when the position
1269 is reached by the robot.
1270
1271 \param q : A six dimension vector corresponding to the
1272 position to reach. All the positions are expressed in meters for the
1273 translations and radians for the rotations. If the position is out
1274 of range, an exception is provided.
1275
1276 \param frame : Frame in which the position is expressed.
1277
1278 - In the joint space, positions are respectively X, Y, Z, A, B, C,
1279 with X,Y,Z the translations, and A,B,C the rotations of the
1280 end-effector.
1281
1282 - In the camera and the reference frame, rotations are
1283 represented by a vpRxyzVector.
1284
1285 - Mixt frame is not implemented. By mixt frame we mean, translations
1286 expressed in the reference frame, and rotations in the camera
1287 frame.
1288
1289 \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
1290 implemented.
1291
1292 \exception vpRobotException::positionOutOfRangeError : The requested
1293 position is out of range.
1294
1295 \code
1296 #include <visp3/core/vpColVector.h>
1297 #include <visp3/robot/vpSimulatorAfma6.h>
1298
1299 int main()
1300 {
1301 vpColVector position(6);
1302 // Set positions in the camera frame
1303 position[0] = 0.1; // x axis, in meter
1304 position[1] = 0.2; // y axis, in meter
1305 position[2] = 0.3; // z axis, in meter
1306 position[3] = M_PI/8; // rotation around x axis, in rad
1307 position[4] = M_PI/4; // rotation around y axis, in rad
1308 position[5] = M_PI; // rotation around z axis, in rad
1309
1310 vpSimulatorAfma6 robot;
1311
1312 robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
1313
1314 // Set the max velocity to 20%
1315 robot.setPositioningVelocity(20);
1316
1317 // Moves the robot in the camera frame
1318 robot.setPosition(vpRobot::CAMERA_FRAME, position);
1319 }
1320 \endcode
1321
1322 To catch the exception if the position is out of range, modify the code
1323 like:
1324
1325 \code
1326 try {
1327 robot.setPosition(vpRobot::CAMERA_FRAME, position);
1328 }
1329 catch (vpRobotException &e) {
1330 if (e.getCode() == vpRobotException::positionOutOfRangeError) {
1331 std::cout << "The position is out of range" << std::endl;
1332 }
1333 \endcode
1334
1335 */
setPosition(const vpRobot::vpControlFrameType frame,const vpColVector & q)1336 void vpSimulatorAfma6::setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
1337 {
1338 if (vpRobot::STATE_POSITION_CONTROL != getRobotState()) {
1339 vpERROR_TRACE("Robot was not in position-based control\n"
1340 "Modification of the robot state");
1341 // setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
1342 }
1343
1344 vpColVector articularCoordinates = get_artCoord();
1345
1346 vpColVector error(6);
1347 double errsqr = 0;
1348 switch (frame) {
1349 case vpRobot::CAMERA_FRAME: {
1350 int nbSol;
1351 vpColVector qdes(6);
1352
1353 vpTranslationVector txyz;
1354 vpRxyzVector rxyz;
1355 for (unsigned int i = 0; i < 3; i++) {
1356 txyz[i] = q[i];
1357 rxyz[i] = q[i + 3];
1358 }
1359
1360 vpRotationMatrix cRc2(rxyz);
1361 vpHomogeneousMatrix cMc2(txyz, cRc2);
1362
1363 vpHomogeneousMatrix fMc_;
1364 vpAfma6::get_fMc(articularCoordinates, fMc_);
1365
1366 vpHomogeneousMatrix fMc2 = fMc_ * cMc2;
1367
1368 do {
1369 articularCoordinates = get_artCoord();
1370 qdes = articularCoordinates;
1371 nbSol = getInverseKinematics(fMc2, qdes, true, verbose_);
1372 setVelocityCalled = true;
1373 if (nbSol > 0) {
1374 error = qdes - articularCoordinates;
1375 errsqr = error.sumSquare();
1376 // findHighestPositioningSpeed(error);
1377 set_artVel(error);
1378 if (errsqr < 1e-4) {
1379 set_artCoord(qdes);
1380 error = 0;
1381 set_artVel(error);
1382 set_velocity(error);
1383 break;
1384 }
1385 } else {
1386 vpERROR_TRACE("Positionning error.");
1387 throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
1388 }
1389 } while (errsqr > 1e-8 && nbSol > 0);
1390
1391 break;
1392 }
1393
1394 case vpRobot::ARTICULAR_FRAME: {
1395 do {
1396 articularCoordinates = get_artCoord();
1397 error = q - articularCoordinates;
1398 errsqr = error.sumSquare();
1399 // findHighestPositioningSpeed(error);
1400 set_artVel(error);
1401 setVelocityCalled = true;
1402 if (errsqr < 1e-4) {
1403 set_artCoord(q);
1404 error = 0;
1405 set_artVel(error);
1406 set_velocity(error);
1407 break;
1408 }
1409 } while (errsqr > 1e-8);
1410 break;
1411 }
1412
1413 case vpRobot::REFERENCE_FRAME: {
1414 int nbSol;
1415 vpColVector qdes(6);
1416
1417 vpTranslationVector txyz;
1418 vpRxyzVector rxyz;
1419 for (unsigned int i = 0; i < 3; i++) {
1420 txyz[i] = q[i];
1421 rxyz[i] = q[i + 3];
1422 }
1423
1424 vpRotationMatrix fRc(rxyz);
1425 vpHomogeneousMatrix fMc_(txyz, fRc);
1426
1427 do {
1428 articularCoordinates = get_artCoord();
1429 qdes = articularCoordinates;
1430 nbSol = getInverseKinematics(fMc_, qdes, true, verbose_);
1431 setVelocityCalled = true;
1432 if (nbSol > 0) {
1433 error = qdes - articularCoordinates;
1434 errsqr = error.sumSquare();
1435 // findHighestPositioningSpeed(error);
1436 set_artVel(error);
1437 if (errsqr < 1e-4) {
1438 set_artCoord(qdes);
1439 error = 0;
1440 set_artVel(error);
1441 set_velocity(error);
1442 break;
1443 }
1444 } else
1445 vpERROR_TRACE("Positionning error. Position unreachable");
1446 } while (errsqr > 1e-8 && nbSol > 0);
1447 break;
1448 }
1449 case vpRobot::MIXT_FRAME: {
1450 vpERROR_TRACE("Positionning error. Mixt frame not implemented");
1451 throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1452 "MIXT_FRAME not implemented.");
1453 }
1454 case vpRobot::END_EFFECTOR_FRAME: {
1455 vpERROR_TRACE("Positionning error. Mixt frame not implemented");
1456 throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1457 "END_EFFECTOR_FRAME not implemented.");
1458 }
1459 }
1460 }
1461
1462 /*!
1463 Move to an absolute position with a given percent of max velocity.
1464 The percent of max velocity is to set with setPositioningVelocity().
1465 The position to reach can be specified in joint coordinates, in the
1466 camera frame or in the reference frame.
1467
1468 This method overloads setPosition(const
1469 vpRobot::vpControlFrameType, const vpColVector &).
1470
1471 \warning This method is blocking. It returns only when the position
1472 is reached by the robot.
1473
1474 \param pos1, pos2, pos3, pos4, pos5, pos6 : The six coordinates of
1475 the position to reach. All the positions are expressed in meters for
1476 the translations and radians for the rotations.
1477
1478 \param frame : Frame in which the position is expressed.
1479
1480 - In the joint space, positions are respectively X (pos1), Y (pos2),
1481 Z (pos3), A (pos4), B (pos5), C (pos6), with X,Y,Z the
1482 translations, and A,B,C the rotations of the end-effector.
1483
1484 - In the camera and the reference frame, rotations [pos4, pos5, pos6] are
1485 represented by a vpRxyzVector.
1486
1487 - Mixt frame is not implemented. By mixt frame we mean, translations
1488 expressed in the reference frame, and rotations in the camera
1489 frame.
1490
1491 \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
1492 implemented.
1493
1494 \exception vpRobotException::positionOutOfRangeError : The requested
1495 position is out of range.
1496
1497 \code
1498 #include <visp3/robot/vpSimulatorAfma6.h>
1499
1500 int main()
1501 {
1502 // Set positions in the camera frame
1503 double pos1 = 0.1; // x axis, in meter
1504 double pos2 = 0.2; // y axis, in meter
1505 double pos3 = 0.3; // z axis, in meter
1506 double pos4 = M_PI/8; // rotation around x axis, in rad
1507 double pos5 = M_PI/4; // rotation around y axis, in rad
1508 double pos6 = M_PI; // rotation around z axis, in rad
1509
1510 vpSimulatorAfma6 robot;
1511
1512 robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
1513
1514 // Set the max velocity to 20%
1515 robot.setPositioningVelocity(20);
1516
1517 // Moves the robot in the camera frame
1518 robot.setPosition(vpRobot::CAMERA_FRAME, pos1, pos2, pos3, pos4, pos5, pos6);
1519 }
1520 \endcode
1521
1522 \sa setPosition()
1523 */
setPosition(const vpRobot::vpControlFrameType frame,double pos1,double pos2,double pos3,double pos4,double pos5,double pos6)1524 void vpSimulatorAfma6::setPosition(const vpRobot::vpControlFrameType frame, double pos1, double pos2,
1525 double pos3, double pos4, double pos5, double pos6)
1526 {
1527 try {
1528 vpColVector position(6);
1529 position[0] = pos1;
1530 position[1] = pos2;
1531 position[2] = pos3;
1532 position[3] = pos4;
1533 position[4] = pos5;
1534 position[5] = pos6;
1535
1536 setPosition(frame, position);
1537 } catch (...) {
1538 vpERROR_TRACE("Error caught");
1539 throw;
1540 }
1541 }
1542
1543 /*!
1544 Move to an absolute joint position with a given percent of max
1545 velocity. The robot state is set to position control. The percent
1546 of max velocity is to set with setPositioningVelocity(). The
1547 position to reach is defined in the position file.
1548
1549 \param filename : Name of the position file to read. The
1550 readPosFile() documentation shows a typical content of such a
1551 position file.
1552
1553 This method has the same behavior than the sample code given below;
1554 \code
1555 #include <visp3/core/vpColVector.h>
1556 #include <visp3/robot/vpSimulatorAfma6.h>
1557
1558 int main()
1559 {
1560 vpColVector q;
1561 vpSimulatorAfma6 robot;
1562
1563 robot.readPosFile("MyPositionFilename.pos", q);
1564 robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
1565 robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
1566 }
1567 \endcode
1568
1569 \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
1570 implemented.
1571
1572 \exception vpRobotException::positionOutOfRangeError : The requested
1573 position is out of range.
1574
1575 \sa setPositioningVelocity()
1576 */
setPosition(const char * filename)1577 void vpSimulatorAfma6::setPosition(const char *filename)
1578 {
1579 vpColVector q;
1580 bool ret;
1581
1582 ret = this->readPosFile(filename, q);
1583
1584 if (ret == false) {
1585 vpERROR_TRACE("Bad position in \"%s\"", filename);
1586 throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
1587 }
1588 this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
1589 this->setPosition(vpRobot::ARTICULAR_FRAME, q);
1590 }
1591
1592 /*!
1593 Get the current position of the robot.
1594
1595 \param frame : Control frame type in which to get the position, either :
1596 - in the camera cartesien frame,
1597 - joint (articular) coordinates of each axes
1598 - in a reference or fixed cartesien frame attached to the robot base
1599 - in a mixt cartesien frame (translation in reference
1600 frame, and rotation in camera frame)
1601
1602 \param q : Measured position of the robot:
1603 - in camera cartesien frame, a 6 dimension vector, set to 0.
1604
1605 - in articular, a 6 dimension vector corresponding to the joint
1606 position of each dof in radians.
1607
1608 - in reference frame, a 6 dimension vector, the first 3 values correspond to
1609 the translation tx, ty, tz in meters (like a vpTranslationVector), and the
1610 last 3 values to the rx, ry, rz rotation (like a vpRxyzVector). The code
1611 below show how to convert this position into a vpHomogeneousMatrix:
1612
1613 \code
1614 #include <visp3/core/vpColVector.h>
1615 #include <visp3/core/vpHomogeneousMatrix.h>
1616 #include <visp3/core/vpRotationMatrix.h>
1617 #include <visp3/core/vpRxyzVector.h>
1618 #include <visp3/core/vpTranslationVector.h>
1619 #include <visp3/robot/vpSimulatorAfma6.h>
1620
1621 int main()
1622 {
1623 vpSimulatorAfma6 robot;
1624
1625 vpColVector position;
1626 robot.getPosition(vpRobot::REFERENCE_FRAME, position);
1627
1628 vpTranslationVector ftc; // reference frame to camera frame translations
1629 vpRxyzVector frc; // reference frame to camera frame rotations
1630
1631 // Update the transformation between reference frame and camera frame
1632 for (int i=0; i < 3; i++) {
1633 ftc[i] = position[i]; // tx, ty, tz
1634 frc[i] = position[i+3]; // ry, ry, rz
1635 }
1636
1637 // Create a rotation matrix from the Rxyz rotation angles
1638 vpRotationMatrix fRc(frc); // reference frame to camera frame rotation matrix
1639
1640 // Create the camera to fix frame transformation in terms of a
1641 // homogeneous matrix
1642 vpHomogeneousMatrix fMc(fRc, ftc);
1643 }
1644 \endcode
1645
1646 \sa getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q,
1647 double ×tamp) \sa setPosition(const vpRobot::vpControlFrameType frame,
1648 const vpColVector & r)
1649
1650 */
getPosition(const vpRobot::vpControlFrameType frame,vpColVector & q)1651 void vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
1652 {
1653 q.resize(6);
1654
1655 switch (frame) {
1656 case vpRobot::CAMERA_FRAME: {
1657 q = 0;
1658 break;
1659 }
1660
1661 case vpRobot::ARTICULAR_FRAME: {
1662 q = get_artCoord();
1663 break;
1664 }
1665
1666 case vpRobot::REFERENCE_FRAME: {
1667 vpHomogeneousMatrix fMc_;
1668 vpAfma6::get_fMc(get_artCoord(), fMc_);
1669
1670 vpRotationMatrix fRc;
1671 fMc_.extract(fRc);
1672 vpRxyzVector rxyz(fRc);
1673
1674 vpTranslationVector txyz;
1675 fMc_.extract(txyz);
1676
1677 for (unsigned int i = 0; i < 3; i++) {
1678 q[i] = txyz[i];
1679 q[i + 3] = rxyz[i];
1680 }
1681 break;
1682 }
1683
1684 case vpRobot::MIXT_FRAME: {
1685 throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1686 "Mixt frame not implemented.");
1687 }
1688 case vpRobot::END_EFFECTOR_FRAME: {
1689 throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1690 "End-effector frame not implemented.");
1691 }
1692 }
1693 }
1694
1695 /*!
1696
1697 Get the current time stamped position of the robot.
1698
1699 \param frame : Control frame type in which to get the position, either :
1700 - in the camera cartesien frame,
1701 - joint (articular) coordinates of each axes
1702 - in a reference or fixed cartesien frame attached to the robot base
1703 - in a mixt cartesien frame (translation in reference
1704 frame, and rotation in camera frame)
1705
1706 \param q : Measured position of the robot:
1707 - in camera cartesien frame, a 6 dimension vector, set to 0.
1708
1709 - in articular, a 6 dimension vector corresponding to the joint
1710 position of each dof in radians.
1711
1712 - in reference frame, a 6 dimension vector, the first 3 values correspond to
1713 the translation tx, ty, tz in meters (like a vpTranslationVector), and the
1714 last 3 values to the rx, ry, rz rotation (like a vpRxyzVector). The code
1715 below show how to convert this position into a vpHomogeneousMatrix:
1716
1717 \param timestamp : Unix time in second since January 1st 1970.
1718
1719 \sa getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
1720 */
getPosition(const vpRobot::vpControlFrameType frame,vpColVector & q,double & timestamp)1721 void vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q, double ×tamp)
1722 {
1723 timestamp = vpTime::measureTimeSecond();
1724 getPosition(frame, q);
1725 }
1726
1727 /*!
1728 Get the current position of the robot.
1729
1730 Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector
1731 &)
1732
1733 The difference is here that the position is returned using a ThetaU
1734 representation.
1735
1736 \sa getPosition(const vpRobot::vpControlFrameType frame, vpColVector &)
1737 */
getPosition(const vpRobot::vpControlFrameType frame,vpPoseVector & position)1738 void vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position)
1739 {
1740 vpColVector posRxyz;
1741 // recupere position en Rxyz
1742 this->getPosition(frame, posRxyz);
1743
1744 // recupere le vecteur thetaU correspondant
1745 vpThetaUVector RtuVect(vpRxyzVector(posRxyz[3], posRxyz[4], posRxyz[5]));
1746
1747 // remplit le vpPoseVector avec translation et rotation ThetaU
1748 for (unsigned int j = 0; j < 3; j++) {
1749 position[j] = posRxyz[j];
1750 position[j + 3] = RtuVect[j];
1751 }
1752 }
1753
1754 /*!
1755
1756 Get the current time stamped position of the robot.
1757
1758 Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector
1759 &, double &)
1760
1761 The difference is here that the position is returned using a ThetaU
1762 representation.
1763
1764 */
getPosition(const vpRobot::vpControlFrameType frame,vpPoseVector & position,double & timestamp)1765 void vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double ×tamp)
1766 {
1767 timestamp = vpTime::measureTimeSecond();
1768 getPosition(frame, position);
1769 }
1770
1771 /*!
1772 This method enables to set the minimum and maximum joint limits for all the
1773 six axis of the robot. The three first values have to be given in meter and
1774 the others in radian.
1775
1776 \param limitMin : The minimum joint limits are given in a vector of size 6.
1777 The three first values have to be given in meter and the others in radian.
1778 \param limitMax : The maximum joint limits are given in a vector of size 6.
1779 The three first values have to be given in meter and the others in radian.
1780 */
setJointLimit(const vpColVector & limitMin,const vpColVector & limitMax)1781 void vpSimulatorAfma6::setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
1782 {
1783 if (limitMin.getRows() != 6 || limitMax.getRows() != 6) {
1784 vpTRACE("Joint limit vector has not a size of 6 !");
1785 return;
1786 }
1787
1788 _joint_min[0] = limitMin[0];
1789 _joint_min[1] = limitMin[1];
1790 _joint_min[2] = limitMin[2];
1791 _joint_min[3] = limitMin[3];
1792 _joint_min[4] = limitMin[4];
1793 _joint_min[5] = limitMin[5];
1794
1795 _joint_max[0] = limitMax[0];
1796 _joint_max[1] = limitMax[1];
1797 _joint_max[2] = limitMax[2];
1798 _joint_max[3] = limitMax[3];
1799 _joint_max[4] = limitMax[4];
1800 _joint_max[5] = limitMax[5];
1801 }
1802
1803 /*!
1804 Test to detect if the robot is near one of its singularities.
1805
1806 The goal is to avoid the problems du to such configurations.
1807 */
singularityTest(const vpColVector & q,vpMatrix & J)1808 bool vpSimulatorAfma6::singularityTest(const vpColVector &q, vpMatrix &J)
1809 {
1810 double q5 = q[4];
1811
1812 bool cond = fabs(q5 - M_PI / 2) < 1e-1;
1813
1814 if (cond) {
1815 J[0][3] = 0;
1816 J[0][4] = 0;
1817 J[1][3] = 0;
1818 J[1][4] = 0;
1819 J[3][3] = 0;
1820 J[3][4] = 0;
1821 J[5][3] = 0;
1822 J[5][4] = 0;
1823 return true;
1824 }
1825
1826 return false;
1827 }
1828
1829 /*!
1830 Method used to check if the robot reached a joint limit.
1831 */
isInJointLimit()1832 int vpSimulatorAfma6::isInJointLimit()
1833 {
1834 int artNumb = 0;
1835 double diff = 0;
1836 double difft = 0;
1837
1838 vpColVector articularCoordinates = get_artCoord();
1839
1840 for (unsigned int i = 0; i < 6; i++) {
1841 if (articularCoordinates[i] <= _joint_min[i]) {
1842 difft = _joint_min[i] - articularCoordinates[i];
1843 if (difft > diff) {
1844 diff = difft;
1845 artNumb = -(int)i - 1;
1846 }
1847 }
1848 }
1849
1850 for (unsigned int i = 0; i < 6; i++) {
1851 if (articularCoordinates[i] >= _joint_max[i]) {
1852 difft = articularCoordinates[i] - _joint_max[i];
1853 if (difft > diff) {
1854 diff = difft;
1855 artNumb = (int)(i + 1);
1856 }
1857 }
1858 }
1859
1860 if (artNumb != 0)
1861 std::cout << "\nWarning: Velocity control stopped: axis " << fabs((float)artNumb) << " on joint limit!"
1862 << std::endl;
1863
1864 return artNumb;
1865 }
1866
1867 /*!
1868 Get the robot displacement since the last call of this method.
1869
1870 \warning This functionnality is not implemented for the moment in the
1871 cartesian space. It is only available in the joint space
1872 (vpRobot::ARTICULAR_FRAME).
1873
1874 \param frame : The frame in which the measured displacement is expressed.
1875
1876 \param displacement : The measured displacement since the last call
1877 of this method. The dimension of \e displacement is always
1878 6. Translations are expressed in meters, rotations in radians.
1879
1880 In camera or reference frame, rotations are expressed with the
1881 Euler Rxyz representation.
1882
1883 */
getDisplacement(vpRobot::vpControlFrameType frame,vpColVector & displacement)1884 void vpSimulatorAfma6::getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
1885 {
1886 displacement.resize(6);
1887 displacement = 0;
1888 vpColVector q_cur(6);
1889
1890 q_cur = get_artCoord();
1891
1892 if (!first_time_getdis) {
1893 switch (frame) {
1894 case vpRobot::CAMERA_FRAME: {
1895 std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
1896 return;
1897 }
1898 case vpRobot::ARTICULAR_FRAME: {
1899 displacement = q_cur - q_prev_getdis;
1900 break;
1901 }
1902 case vpRobot::REFERENCE_FRAME: {
1903 std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
1904 return;
1905 }
1906 case vpRobot::MIXT_FRAME: {
1907 std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
1908 return;
1909 }
1910 case vpRobot::END_EFFECTOR_FRAME: {
1911 std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
1912 return;
1913 }
1914 }
1915 } else {
1916 first_time_getdis = false;
1917 }
1918
1919 // Memorize the joint position for the next call
1920 q_prev_getdis = q_cur;
1921 }
1922
1923 /*!
1924 Read joint positions in a specific Afma6 position file.
1925
1926 This position file has to start with a header. The six joint positions
1927 are given after the "R:" keyword. The first 3 values correspond to the
1928 joint translations X,Y,Z expressed in meters. The 3 last values
1929 correspond to the joint rotations A,B,C expressed in degres to be more
1930 representative for the user. Theses values are then converted in
1931 radians in \e q. The character "#" starting a line indicates a
1932 comment.
1933
1934 A typical content of such a file is given below:
1935
1936 \code
1937 #AFMA6 - Position - Version 2.01
1938 # file: "myposition.pos "
1939 #
1940 # R: X Y Z A B C
1941 # Joint position: X, Y, Z: translations in meters
1942 # A, B, C: rotations in degrees
1943 #
1944
1945 R: 0.1 0.3 -0.25 -80.5 80 0
1946 \endcode
1947
1948 \param filename : Name of the position file to read.
1949
1950 \param q : Joint positions: X,Y,Z,A,B,C. Translations X,Y,Z are
1951 expressed in meters, while joint rotations A,B,C in radians.
1952
1953 \return true if the positions were successfully readen in the file. false, if
1954 an error occurs.
1955
1956 The code below shows how to read a position from a file and move the robot to
1957 this position.
1958 \code
1959 vpSimulatorAfma6 robot;
1960 vpColVector q; // Joint position
1961 robot.readPosFile("myposition.pos", q); // Set the joint position from the file
1962 robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
1963
1964 robot.setPositioningVelocity(5); // Positioning velocity set to 5%
1965 robot.setPosition(vpRobot::ARTICULAR_FRAME, q); // Move to the joint position
1966 \endcode
1967
1968 \sa savePosFile()
1969 */
readPosFile(const std::string & filename,vpColVector & q)1970 bool vpSimulatorAfma6::readPosFile(const std::string &filename, vpColVector &q)
1971 {
1972 std::ifstream fd(filename.c_str(), std::ios::in);
1973
1974 if (!fd.is_open()) {
1975 return false;
1976 }
1977
1978 std::string line;
1979 std::string key("R:");
1980 std::string id("#AFMA6 - Position");
1981 bool pos_found = false;
1982 int lineNum = 0;
1983
1984 q.resize(njoint);
1985
1986 while (std::getline(fd, line)) {
1987 lineNum++;
1988 if (lineNum == 1) {
1989 if (!(line.compare(0, id.size(), id) == 0)) { // check if Afma6 position file
1990 std::cout << "Error: this position file " << filename << " is not for Afma6 robot" << std::endl;
1991 return false;
1992 }
1993 }
1994 if ((line.compare(0, 1, "#") == 0)) { // skip comment
1995 continue;
1996 }
1997 if ((line.compare(0, key.size(), key) == 0)) { // decode position
1998 // check if there are at least njoint values in the line
1999 std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
2000 if (chain.size() < njoint + 1) // try to split with tab separator
2001 chain = vpIoTools::splitChain(line, std::string("\t"));
2002 if (chain.size() < njoint + 1)
2003 continue;
2004
2005 std::istringstream ss(line);
2006 std::string key_;
2007 ss >> key_;
2008 for (unsigned int i = 0; i < njoint; i++)
2009 ss >> q[i];
2010 pos_found = true;
2011 break;
2012 }
2013 }
2014
2015 // converts rotations from degrees into radians
2016 q[3] = vpMath::rad(q[3]);
2017 q[4] = vpMath::rad(q[4]);
2018 q[5] = vpMath::rad(q[5]);
2019
2020 fd.close();
2021
2022 if (!pos_found) {
2023 std::cout << "Error: unable to find a position for Afma6 robot in " << filename << std::endl;
2024 return false;
2025 }
2026
2027 return true;
2028 }
2029
2030 /*!
2031 Save joint (articular) positions in a specific Afma6 position file.
2032
2033 This position file starts with a header on the first line. After
2034 convertion of the rotations in degrees, the joint position \e q is
2035 written on a line starting with the keyword "R: ". See readPosFile()
2036 documentation for an example of such a file.
2037
2038 \param filename : Name of the position file to create.
2039
2040 \param q : Joint positions [X,Y,Z,A,B,C] to save in the
2041 filename. Translations X,Y,Z are expressed in meters, while
2042 rotations A,B,C in radians.
2043
2044 \warning The joint rotations A,B,C written in the file are converted
2045 in degrees to be more representative for the user.
2046
2047 \return true if the positions were successfully saved in the file. false, if
2048 an error occurs.
2049
2050 \sa readPosFile()
2051 */
savePosFile(const std::string & filename,const vpColVector & q)2052 bool vpSimulatorAfma6::savePosFile(const std::string &filename, const vpColVector &q)
2053 {
2054 FILE *fd;
2055 fd = fopen(filename.c_str(), "w");
2056 if (fd == NULL)
2057 return false;
2058
2059 fprintf(fd, "\
2060 #AFMA6 - Position - Version 2.01\n\
2061 #\n\
2062 # R: X Y Z A B C\n\
2063 # Joint position: X, Y, Z: translations in meters\n\
2064 # A, B, C: rotations in degrees\n\
2065 #\n\
2066 #\n\n");
2067
2068 // Save positions in mm and deg
2069 fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n", q[0], q[1], q[2], vpMath::deg(q[3]), vpMath::deg(q[4]),
2070 vpMath::deg(q[5]));
2071
2072 fclose(fd);
2073 return (true);
2074 }
2075
2076 /*!
2077 Moves the robot to the joint position specified in the filename.
2078
2079 \param filename: File containing a joint position.
2080
2081 \sa readPosFile
2082 */
move(const char * filename)2083 void vpSimulatorAfma6::move(const char *filename)
2084 {
2085 vpColVector q;
2086
2087 try {
2088 this->readPosFile(filename, q);
2089 this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
2090 this->setPosition(vpRobot::ARTICULAR_FRAME, q);
2091 } catch (...) {
2092 throw;
2093 }
2094 }
2095
2096 /*!
2097 Get the geometric transformation \f$^c{\bf M}_e\f$ between the
2098 camera frame and the end-effector frame. This transformation is
2099 constant and correspond to the extrinsic camera parameters estimated
2100 by calibration.
2101
2102 \param cMe : Transformation between the camera frame and the
2103 end-effector frame.
2104 */
get_cMe(vpHomogeneousMatrix & cMe)2105 void vpSimulatorAfma6::get_cMe(vpHomogeneousMatrix &cMe) { vpAfma6::get_cMe(cMe); }
2106
2107 /*!
2108 Get the twist transformation \f$^c{\bf V}_e\f$ from camera frame to
2109 end-effector frame. This transformation allows to compute a
2110 velocity expressed in the end-effector frame into the camera frame.
2111
2112 \param cVe : Twist transformation.
2113 */
get_cVe(vpVelocityTwistMatrix & cVe)2114 void vpSimulatorAfma6::get_cVe(vpVelocityTwistMatrix &cVe)
2115 {
2116 vpHomogeneousMatrix cMe;
2117 vpAfma6::get_cMe(cMe);
2118
2119 cVe.buildFrom(cMe);
2120 }
2121
2122 /*!
2123 Get the robot jacobian expressed in the end-effector frame.
2124
2125 To compute \f$^e{\bf J}_e\f$, we communicate with the low level
2126 controller to get the joint position of the robot.
2127
2128 \param eJe_ : Robot jacobian \f$^e{\bf J}_e\f$ expressed in the
2129 end-effector frame.
2130 */
get_eJe(vpMatrix & eJe_)2131 void vpSimulatorAfma6::get_eJe(vpMatrix &eJe_)
2132 {
2133 try {
2134 vpAfma6::get_eJe(get_artCoord(), eJe_);
2135 } catch (...) {
2136 vpERROR_TRACE("catch exception ");
2137 throw;
2138 }
2139 }
2140
2141 /*!
2142 Get the robot jacobian expressed in the robot reference frame also
2143 called fix frame.
2144
2145 To compute \f$^f{\bf J}_e\f$, we communicate with the low level
2146 controller to get the joint position of the robot.
2147
2148 \param fJe_ : Robot jacobian \f$^f{\bf J}_e\f$ expressed in the
2149 reference frame.
2150 */
get_fJe(vpMatrix & fJe_)2151 void vpSimulatorAfma6::get_fJe(vpMatrix &fJe_)
2152 {
2153 try {
2154 vpColVector articularCoordinates = get_artCoord();
2155 vpAfma6::get_fJe(articularCoordinates, fJe_);
2156 } catch (...) {
2157 vpERROR_TRACE("Error caught");
2158 throw;
2159 }
2160 }
2161
2162 /*!
2163 Stop the robot.
2164 */
stopMotion()2165 void vpSimulatorAfma6::stopMotion()
2166 {
2167 if (getRobotState() != vpRobot::STATE_VELOCITY_CONTROL)
2168 return;
2169
2170 vpColVector stop(6);
2171 stop = 0;
2172 set_artVel(stop);
2173 set_velocity(stop);
2174 vpRobot::setRobotState(vpRobot::STATE_STOP);
2175 }
2176
2177 /**********************************************************************************/
2178 /**********************************************************************************/
2179 /**********************************************************************************/
2180 /**********************************************************************************/
2181
2182 /*!
2183 Initialise the display of the robot's arms.
2184
2185 Set the path to the scene files (*.bnd and *.sln) used by the
2186 simulator. If the path set in vpConfig.h in VISP_SCENES_DIR macro is
2187 not valid, the path is set from the VISP_SCENES_DIR environment
2188 variable that the user has to set.
2189 */
initArms()2190 void vpSimulatorAfma6::initArms()
2191 {
2192 // set scene_dir from #define VISP_SCENE_DIR if it exists
2193 // VISP_SCENES_DIR may contain multiple locations separated by ";"
2194 std::string scene_dir_;
2195 std::vector<std::string> scene_dirs = vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(";"));
2196 bool sceneDirExists = false;
2197 for (size_t i = 0; i < scene_dirs.size(); i++)
2198 if (vpIoTools::checkDirectory(scene_dirs[i]) == true) { // directory exists
2199 scene_dir_ = scene_dirs[i];
2200 sceneDirExists = true;
2201 break;
2202 }
2203 if (!sceneDirExists) {
2204 try {
2205 scene_dir_ = vpIoTools::getenv("VISP_SCENES_DIR");
2206 std::cout << "The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
2207 } catch (...) {
2208 std::cout << "Cannot get VISP_SCENES_DIR environment variable" << std::endl;
2209 }
2210 }
2211
2212 unsigned int name_length = 30; // the size of this kind of string "/afma6_arm2.bnd"
2213 if (scene_dir_.size() > FILENAME_MAX)
2214 throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
2215 unsigned int full_length = (unsigned int)scene_dir_.size() + name_length;
2216 if (full_length > FILENAME_MAX)
2217 throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
2218
2219 char *name_cam = new char[full_length];
2220
2221 strcpy(name_cam, scene_dir_.c_str());
2222 strcat(name_cam, "/camera.bnd");
2223 set_scene(name_cam, &camera, cameraFactor);
2224
2225 if (arm_dir.size() > FILENAME_MAX)
2226 throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
2227 full_length = (unsigned int)arm_dir.size() + name_length;
2228 if (full_length > FILENAME_MAX)
2229 throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
2230
2231 char *name_arm = new char[full_length];
2232 strcpy(name_arm, arm_dir.c_str());
2233 strcat(name_arm, "/afma6_gate.bnd");
2234 std::cout << "name arm: " << name_arm << std::endl;
2235 set_scene(name_arm, robotArms, 1.0);
2236 strcpy(name_arm, arm_dir.c_str());
2237 strcat(name_arm, "/afma6_arm1.bnd");
2238 set_scene(name_arm, robotArms + 1, 1.0);
2239 strcpy(name_arm, arm_dir.c_str());
2240 strcat(name_arm, "/afma6_arm2.bnd");
2241 set_scene(name_arm, robotArms + 2, 1.0);
2242 strcpy(name_arm, arm_dir.c_str());
2243 strcat(name_arm, "/afma6_arm3.bnd");
2244 set_scene(name_arm, robotArms + 3, 1.0);
2245 strcpy(name_arm, arm_dir.c_str());
2246 strcat(name_arm, "/afma6_arm4.bnd");
2247 set_scene(name_arm, robotArms + 4, 1.0);
2248
2249 vpAfma6::vpAfma6ToolType tool;
2250 tool = getToolType();
2251 strcpy(name_arm, arm_dir.c_str());
2252 switch (tool) {
2253 case vpAfma6::TOOL_CCMOP: {
2254 strcat(name_arm, "/afma6_tool_ccmop.bnd");
2255 break;
2256 }
2257 case vpAfma6::TOOL_GRIPPER: {
2258 strcat(name_arm, "/afma6_tool_gripper.bnd");
2259 break;
2260 }
2261 case vpAfma6::TOOL_VACUUM: {
2262 strcat(name_arm, "/afma6_tool_vacuum.bnd");
2263 break;
2264 }
2265 case vpAfma6::TOOL_CUSTOM: {
2266 std::cout << "The custom tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
2267 break;
2268 }
2269 case vpAfma6::TOOL_INTEL_D435_CAMERA: {
2270 std::cout << "The Intel D435 camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
2271 break;
2272 }
2273 case vpAfma6::TOOL_GENERIC_CAMERA: {
2274 std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
2275 break;
2276 }
2277 }
2278 set_scene(name_arm, robotArms + 5, 1.0);
2279
2280 add_rfstack(IS_BACK);
2281
2282 add_vwstack("start", "depth", 0.0, 100.0);
2283 add_vwstack("start", "window", -0.1, 0.1, -0.1, 0.1);
2284 add_vwstack("start", "type", PERSPECTIVE);
2285 //
2286 // sceneInitialized = true;
2287 // displayObject = true;
2288 displayCamera = true;
2289
2290 delete[] name_cam;
2291 delete[] name_arm;
2292 }
2293
getExternalImage(vpImage<vpRGBa> & I_)2294 void vpSimulatorAfma6::getExternalImage(vpImage<vpRGBa> &I_)
2295 {
2296 bool changed = false;
2297 vpHomogeneousMatrix displacement = navigation(I_, changed);
2298
2299 // if (displacement[2][3] != 0)
2300 if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
2301 camMf2 = camMf2 * displacement;
2302
2303 f2Mf = camMf2.inverse() * camMf;
2304
2305 camMf = camMf2 * displacement * f2Mf;
2306
2307 double u;
2308 double v;
2309 // if(px_ext != 1 && py_ext != 1)
2310 // we assume px_ext and py_ext > 0
2311 if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
2312 (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon())) {
2313 u = (double)I_.getWidth() / (2 * px_ext);
2314 v = (double)I_.getHeight() / (2 * py_ext);
2315 } else {
2316 u = (double)I_.getWidth() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
2317 v = (double)I_.getHeight() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
2318 }
2319
2320 float w44o[4][4], w44cext[4][4], x, y, z;
2321
2322 vp2jlc_matrix(camMf.inverse(), w44cext);
2323
2324 add_vwstack("start", "cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
2325 x = w44cext[2][0] + w44cext[3][0];
2326 y = w44cext[2][1] + w44cext[3][1];
2327 z = w44cext[2][2] + w44cext[3][2];
2328 add_vwstack("start", "vrp", x, y, z);
2329 add_vwstack("start", "vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
2330 add_vwstack("start", "vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
2331 add_vwstack("start", "window", -u, u, -v, v);
2332
2333 vpHomogeneousMatrix fMit[8];
2334 get_fMi(fMit);
2335
2336 vp2jlc_matrix(vpHomogeneousMatrix(0, 0, 0, 0, 0, 0), w44o);
2337 display_scene(w44o, robotArms[0], I_, curColor);
2338
2339 vp2jlc_matrix(fMit[0], w44o);
2340 display_scene(w44o, robotArms[1], I_, curColor);
2341
2342 vp2jlc_matrix(fMit[2], w44o);
2343 display_scene(w44o, robotArms[2], I_, curColor);
2344
2345 vp2jlc_matrix(fMit[3], w44o);
2346 display_scene(w44o, robotArms[3], I_, curColor);
2347
2348 vp2jlc_matrix(fMit[4], w44o);
2349 display_scene(w44o, robotArms[4], I_, curColor);
2350
2351 vp2jlc_matrix(fMit[5], w44o);
2352 display_scene(w44o, robotArms[5], I_, curColor);
2353
2354 if (displayCamera) {
2355 vpHomogeneousMatrix cMe;
2356 get_cMe(cMe);
2357 cMe = cMe.inverse();
2358 cMe = fMit[6] * cMe;
2359 vp2jlc_matrix(cMe, w44o);
2360 display_scene(w44o, camera, I_, camColor);
2361 }
2362
2363 if (displayObject) {
2364 vp2jlc_matrix(fMo, w44o);
2365 display_scene(w44o, scene, I_, curColor);
2366 }
2367 }
2368
2369 /*!
2370 This method enables to initialise the joint coordinates of the robot in
2371 order to position the camera relative to the object.
2372
2373 Before using this method it is advised to set the position of the object
2374 thanks to the set_fMo() method.
2375
2376 In other terms, set the world to camera transformation
2377 \f${^f}{\bf M}_c = {^f}{\bf M}_o \; ({^c}{\bf M}_o)^{-1}\f$, and from the
2378 inverse kinematics set the joint positions \f${\bf q}\f$ that corresponds to
2379 the \f${^f}{\bf M}_c\f$ transformation.
2380
2381 \param cMo_ : the desired pose of the camera.
2382
2383 \return false if the robot kinematics is not able to reach the cMo position.
2384
2385 */
initialiseCameraRelativeToObject(const vpHomogeneousMatrix & cMo_)2386 bool vpSimulatorAfma6::initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo_)
2387 {
2388 vpColVector stop(6);
2389 bool status = true;
2390 stop = 0;
2391 set_artVel(stop);
2392 set_velocity(stop);
2393 vpHomogeneousMatrix fMc_;
2394 fMc_ = fMo * cMo_.inverse();
2395
2396 vpColVector articularCoordinates = get_artCoord();
2397 int nbSol = getInverseKinematics(fMc_, articularCoordinates, true, verbose_);
2398
2399 if (nbSol == 0) {
2400 status = false;
2401 vpERROR_TRACE("Positionning error. Position unreachable");
2402 }
2403
2404 if (verbose_)
2405 std::cout << "Used joint coordinates (rad): " << articularCoordinates.t() << std::endl;
2406
2407 set_artCoord(articularCoordinates);
2408
2409 compute_fMi();
2410
2411 return status;
2412 }
2413
2414 /*!
2415 This method enables to initialise the pose between the object and the
2416 reference frame, in order to position the object relative to the camera.
2417
2418 Before using this method it is advised to set the articular coordinates of
2419 the robot.
2420
2421 In other terms, set the world to object transformation
2422 \f${^f}{\bf M}_o = {^f}{\bf M}_c \; {^c}{\bf M}_o\f$ where \f$ {^f}{\bf M}_c
2423 = f({\bf q})\f$ with \f${\bf q}\f$ the robot joint position
2424
2425 \param cMo_ : the desired pose of the camera.
2426 */
initialiseObjectRelativeToCamera(const vpHomogeneousMatrix & cMo_)2427 void vpSimulatorAfma6::initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo_)
2428 {
2429 vpColVector stop(6);
2430 stop = 0;
2431 set_artVel(stop);
2432 set_velocity(stop);
2433 vpHomogeneousMatrix fMit[8];
2434 get_fMi(fMit);
2435 fMo = fMit[7] * cMo_;
2436 }
2437
2438 /*!
2439 This method enable to move the robot with respect to the initialized object.
2440 The robot trajectory is a straight line from the current position to the one
2441 corresponding to the desired pose (3D visual servoing).
2442
2443 \param cdMo_ : the desired pose of the camera wrt. the object
2444 \param Iint : pointer to the image where the internal view is displayed
2445 \param errMax : maximum error to consider the pose is reached
2446
2447 \return True is the pose is reached, False else
2448 */
setPosition(const vpHomogeneousMatrix & cdMo_,vpImage<unsigned char> * Iint,const double & errMax)2449 bool vpSimulatorAfma6::setPosition(const vpHomogeneousMatrix &cdMo_, vpImage<unsigned char> *Iint, const double &errMax)
2450 {
2451 // get rid of max velocity
2452 double vMax = getMaxTranslationVelocity();
2453 double wMax = getMaxRotationVelocity();
2454 setMaxTranslationVelocity(10. * vMax);
2455 setMaxRotationVelocity(10. * wMax);
2456
2457 vpColVector v(3), w(3), vel(6);
2458 vpHomogeneousMatrix cdMc;
2459 vpTranslationVector cdTc;
2460 vpRotationMatrix cdRc;
2461 vpThetaUVector cdTUc;
2462 vpColVector err(6);
2463 err = 1.;
2464 const double lambda = 5.;
2465
2466 vpVelocityTwistMatrix cVe;
2467
2468 unsigned int i, iter = 0;
2469 while ((iter++ < 300) & (err.frobeniusNorm() > errMax)) {
2470 double t = vpTime::measureTimeMs();
2471
2472 // update image
2473 if (Iint != NULL) {
2474 vpDisplay::display(*Iint);
2475 getInternalView(*Iint);
2476 vpDisplay::flush(*Iint);
2477 }
2478
2479 // update pose error
2480 cdMc = cdMo_ * get_cMo().inverse();
2481 cdMc.extract(cdRc);
2482 cdMc.extract(cdTc);
2483 cdTUc.buildFrom(cdRc);
2484
2485 // compute v,w and velocity
2486 v = -lambda * cdRc.t() * cdTc;
2487 w = -lambda * cdTUc;
2488 for (i = 0; i < 3; ++i) {
2489 vel[i] = v[i];
2490 vel[i + 3] = w[i];
2491 err[i] = cdTc[i];
2492 err[i + 3] = cdTUc[i];
2493 }
2494
2495 // update feat
2496 setVelocity(vpRobot::CAMERA_FRAME, vel);
2497
2498 // wait for it
2499 vpTime::wait(t, 10);
2500 }
2501 vel = 0.;
2502 set_velocity(vel);
2503 set_artVel(vel);
2504 setMaxTranslationVelocity(vMax);
2505 setMaxRotationVelocity(wMax);
2506
2507 // std::cout << "setPosition: final error " << err.t() << std::endl;
2508 return (err.frobeniusNorm() <= errMax);
2509 }
2510
2511 #elif !defined(VISP_BUILD_SHARED_LIBS)
2512 // Work arround to avoid warning: libvisp_robot.a(vpSimulatorAfma6.cpp.o) has
2513 // no symbols
dummy_vpSimulatorAfma6()2514 void dummy_vpSimulatorAfma6(){};
2515 #endif
2516