Visual Servoing Platform version 3.5.0
vpSimulatorAfma6.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See http://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * 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
58
63 : vpRobotWireFrameSimulator(), vpAfma6(), q_prev_getdis(), first_time_getdis(true),
64 positioningVelocity(defaultPositioningVelocity), zeroPos(), reposPos(), toolCustom(false), arm_dir()
65{
66 init();
68
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
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
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
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
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
230 toolCustom = false;
231
232 size_fMi = 8;
233 fMi = new vpHomogeneousMatrix[8];
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
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
278{
279 robotArms = NULL;
280 robotArms = new Bound_scene[6];
281 initArms();
283 vpHomogeneousMatrix(-0.1, 0, 4, vpMath::rad(90), 0, 0));
284 cameraParam.initPersProjWithoutDistortion(558.5309599, 556.055053, 320, 240);
287 getCameraParameters(tmp, 640, 480);
288 px_int = tmp.get_px();
289 py_int = tmp.get_py();
290 sceneInitialized = true;
291}
292
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 }
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 }
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 }
392 std::cout << "The custom tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
393 break;
394 }
396 std::cout << "The Intel D435 camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
397 break;
398 }
400 std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
401 break;
402 }
403 }
404
406 this->_eMc.buildFrom(_etc, eRc);
407
408 setToolType(tool);
409 return;
410}
411
422void 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 }
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 }
455 std::cout << "The generic tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
456 break;
457 }
459 std::cout << "The Intel D435 camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
460 break;
461 }
463 std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
464 break;
465 }
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
486{
487 getCameraParameters(cam, I_.getWidth(), I_.getHeight());
488}
489
499{
500 getCameraParameters(cam, I_.getWidth(), I_.getHeight());
501}
502
509{
510 px_int = cam.get_px();
511 py_int = cam.get_py();
512 toolCustom = true;
513}
514
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;
528
530 setVelocityCalled = false;
531
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) {
591 }
592
594 while (get_displayBusy())
595 vpTime::wait(2);
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
612 pt.track(getExternalCameraPosition() * fMit[0]);
615 for (unsigned int k = 1; k < 7; k++) {
616 pt.track(getExternalCameraPosition() * fMit[k - 1]);
618
619 pt.track(getExternalCameraPosition() * fMit[k]);
621
623 }
625 thickness_);
626 }
627
629
631 tcur_1 = tcur;
632 } else {
634 }
635 }
636}
637
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
793{
794 switch (newState) {
795 case vpRobot::STATE_STOP: {
796 // Start primitive STOP only if the current state is Velocity
798 stopMotion();
799 }
800 break;
801 }
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 }
814 std::cout << "Change the control mode from stop to velocity control.\n";
815 }
816 break;
817 }
819 default:
820 break;
821 }
822
823 return vpRobot::setRobotState(newState);
824}
825
900{
902 vpERROR_TRACE("Cannot send a velocity to the robot "
903 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
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
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
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 }
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
1003{
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_;
1016 vpAfma6::get_eJe(articularCoordinates, eJe_);
1017 eJe_ = eJe_.pseudoInverse();
1019 singularityTest(articularCoordinates, eJe_);
1020 articularVelocity = eJe_ * eVc * velocityframe;
1021 set_artVel(articularVelocity);
1022 break;
1023 }
1025 vpMatrix fJe_;
1026 vpAfma6::get_fJe(articularCoordinates, fJe_);
1027 fJe_ = fJe_.pseudoInverse();
1029 singularityTest(articularCoordinates, fJe_);
1030 articularVelocity = fJe_ * velocityframe;
1031 set_artVel(articularVelocity);
1032 break;
1033 }
1035 articularVelocity = velocityframe;
1036 set_artVel(articularVelocity);
1037 break;
1038 }
1040 case vpRobot::MIXT_FRAME: {
1041 break;
1042 }
1043 }
1044
1045 switch (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 }
1069 case vpRobot::MIXT_FRAME: {
1070 break;
1071 }
1072 }
1073}
1074
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_;
1131 vpAfma6::get_eJe(articularCoordinates, eJe_);
1132 vel = cVe * eJe_ * articularVelocity;
1133 break;
1134 }
1136 vel = articularVelocity;
1137 break;
1138 }
1140 vpMatrix fJe_;
1141 vpAfma6::get_fJe(articularCoordinates, fJe_);
1142 vel = fJe_ * articularVelocity;
1143 break;
1144 }
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
1174{
1175 timestamp = vpTime::measureTimeSecond();
1176 getVelocity(frame, vel);
1177}
1178
1221{
1222 vpColVector vel(6);
1223 getVelocity(frame, vel);
1224
1225 return vel;
1226}
1227
1241{
1242 timestamp = vpTime::measureTimeSecond();
1243 vpColVector vel(6);
1244 getVelocity(frame, vel);
1245
1246 return vel;
1247}
1248
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
1337{
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
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
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
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
1414 int nbSol;
1415 vpColVector qdes(6);
1416
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 }
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
1524void 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
1577void 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 }
1590}
1591
1652{
1653 q.resize(6);
1654
1655 switch (frame) {
1656 case vpRobot::CAMERA_FRAME: {
1657 q = 0;
1658 break;
1659 }
1660
1662 q = get_artCoord();
1663 break;
1664 }
1665
1669
1670 vpRotationMatrix fRc;
1671 fMc_.extract(fRc);
1672 vpRxyzVector rxyz(fRc);
1673
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 }
1689 throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1690 "End-effector frame not implemented.");
1691 }
1692 }
1693}
1694
1722{
1723 timestamp = vpTime::measureTimeSecond();
1724 getPosition(frame, q);
1725}
1726
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
1765void vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double &timestamp)
1766{
1767 timestamp = vpTime::measureTimeSecond();
1768 getPosition(frame, position);
1769}
1770
1781void 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
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
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
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 }
1899 displacement = q_cur - q_prev_getdis;
1900 break;
1901 }
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 }
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
1970bool 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
2052bool 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
2083void vpSimulatorAfma6::move(const char *filename)
2084{
2085 vpColVector q;
2086
2087 try {
2088 this->readPosFile(filename, q);
2091 } catch (...) {
2092 throw;
2093 }
2094}
2095
2106
2115{
2117 vpAfma6::get_cMe(cMe);
2118
2119 cVe.buildFrom(cMe);
2120}
2121
2132{
2133 try {
2135 } catch (...) {
2136 vpERROR_TRACE("catch exception ");
2137 throw;
2138 }
2139}
2140
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
2166{
2168 return;
2169
2170 vpColVector stop(6);
2171 stop = 0;
2172 set_artVel(stop);
2173 set_velocity(stop);
2175}
2176
2177/**********************************************************************************/
2178/**********************************************************************************/
2179/**********************************************************************************/
2180/**********************************************************************************/
2181
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
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 }
2270 std::cout << "The Intel D435 camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
2271 break;
2272 }
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
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) {
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
2387{
2388 vpColVector stop(6);
2389 bool status = true;
2390 stop = 0;
2391 set_artVel(stop);
2392 set_velocity(stop);
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
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
2449bool 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);
2460 vpRotationMatrix cdRc;
2461 vpThetaUVector cdTUc;
2462 vpColVector err(6);
2463 err = 1.;
2464 const double lambda = 5.;
2465
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
2497
2498 // wait for it
2499 vpTime::wait(t, 10);
2500 }
2501 vel = 0.;
2502 set_velocity(vel);
2503 set_artVel(vel);
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
2514void dummy_vpSimulatorAfma6(){};
2515#endif
Modelisation of Irisa's gantry robot named Afma6.
Definition: vpAfma6.h:79
static const char *const CONST_CCMOP_CAMERA_NAME
Definition: vpAfma6.h:102
double _joint_min[6]
Definition: vpAfma6.h:204
static const unsigned int njoint
Number of joint.
Definition: vpAfma6.h:198
vpRxyzVector _erc
Definition: vpAfma6.h:207
int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest=true, const bool &verbose=false) const
Definition: vpAfma6.cpp:600
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma6.cpp:888
void setToolType(vpAfma6::vpAfma6ToolType tool)
Set the current tool type.
Definition: vpAfma6.h:194
vpAfma6ToolType getToolType() const
Get the current tool type.
Definition: vpAfma6.h:169
static const char *const CONST_GRIPPER_CAMERA_NAME
Definition: vpAfma6.h:107
vpHomogeneousMatrix _eMc
Definition: vpAfma6.h:209
double _long_56
Definition: vpAfma6.h:202
vpTranslationVector _etc
Definition: vpAfma6.h:206
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpAfma6.cpp:775
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpAfma6.h:215
double _joint_max[6]
Definition: vpAfma6.h:203
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpAfma6.cpp:932
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpAfma6.cpp:1002
vpAfma6ToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpAfma6.h:126
@ TOOL_CCMOP
Definition: vpAfma6.h:127
@ TOOL_GENERIC_CAMERA
Definition: vpAfma6.h:130
@ TOOL_CUSTOM
Definition: vpAfma6.h:132
@ TOOL_VACUUM
Definition: vpAfma6.h:129
@ TOOL_INTEL_D435_CAMERA
Definition: vpAfma6.h:131
@ TOOL_GRIPPER
Definition: vpAfma6.h:128
unsigned int getRows() const
Definition: vpArray2D.h:289
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
double sumSquare() const
vpRowVector t() const
double frobeniusNorm() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
static const vpColor none
Definition: vpColor.h:229
static const vpColor green
Definition: vpColor.h:220
static void display(const vpImage< unsigned char > &I)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static void flush(const vpImage< unsigned char > &I)
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1, const vpImagePoint &offset=vpImagePoint(0, 0))
static void displayCamera(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness)
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ dimensionError
Bad dimension.
Definition: vpException.h:95
void track(const vpHomogeneousMatrix &cMo)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void extract(vpRotationMatrix &R) const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
unsigned int getWidth() const
Definition: vpImage.h:246
unsigned int getHeight() const
Definition: vpImage.h:188
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1900
static bool checkDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:420
static std::string getenv(const std::string &env)
Definition: vpIoTools.cpp:353
static double rad(double deg)
Definition: vpMath.h:110
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:145
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:153
static double deg(double rad)
Definition: vpMath.h:103
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2241
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:472
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:470
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:152
Error that can be emited by the vpRobot class and its derivates.
double getSamplingTime() const
This class aims to be a basis used to create all the simulators of robots.
void set_velocity(const vpColVector &vel)
void set_displayBusy(const bool &status)
void getInternalView(vpImage< vpRGBa > &I)
vpHomogeneousMatrix getExternalCameraPosition() const
void set_artCoord(const vpColVector &coord)
static void * launcher(void *arg)
void setExternalCameraPosition(const vpHomogeneousMatrix &camMf_)
void set_artVel(const vpColVector &vel)
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:144
vpControlFrameType
Definition: vpRobot.h:75
@ REFERENCE_FRAME
Definition: vpRobot.h:76
@ ARTICULAR_FRAME
Definition: vpRobot.h:78
@ MIXT_FRAME
Definition: vpRobot.h:86
@ CAMERA_FRAME
Definition: vpRobot.h:82
@ END_EFFECTOR_FRAME
Definition: vpRobot.h:81
vpControlFrameType getRobotFrame(void) const
Definition: vpRobot.h:172
vpRobotStateType
Definition: vpRobot.h:64
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:67
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:66
@ STATE_ACCELERATION_CONTROL
Initialize the acceleration controller.
Definition: vpRobot.h:68
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:65
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:273
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:251
void setMaxRotationVelocity(double maxVr)
Definition: vpRobot.cpp:260
vpControlFrameType setRobotFrame(vpRobot::vpControlFrameType newFrame)
Definition: vpRobot.cpp:207
void setMaxTranslationVelocity(double maxVt)
Definition: vpRobot.cpp:239
Implementation of a rotation matrix and operations on such kind of matrices.
vpRotationMatrix t() const
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:184
void get_eJe(vpMatrix &eJe)
static bool savePosFile(const std::string &filename, const vpColVector &q)
void setCameraParameters(const vpCameraParameters &cam)
vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
void get_cVe(vpVelocityTwistMatrix &cVe)
void initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo)
static bool readPosFile(const std::string &filename, vpColVector &q)
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height)
bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo)
void get_fMi(vpHomogeneousMatrix *fMit)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
void findHighestPositioningSpeed(vpColVector &q)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
void move(const char *filename)
static const double defaultPositioningVelocity
bool singularityTest(const vpColVector &q, vpMatrix &J)
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &displacement)
void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
void get_fJe(vpMatrix &fJe)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q)
void getExternalImage(vpImage< vpRGBa > &I)
double getPositioningVelocity(void)
void get_cMe(vpHomogeneousMatrix &cMe)
Implementation of a rotation vector as axis-angle minimal representation.
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
Class that consider the case of a translation vector.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpHomogeneousMatrix camMf2
vpHomogeneousMatrix f2Mf
void display_scene(Matrix mat, Bound_scene &sc, const vpImage< vpRGBa > &I, const vpColor &color)
vpHomogeneousMatrix fMo
vpHomogeneousMatrix camMf
void setExternalCameraParameters(const vpCameraParameters &cam)
vpHomogeneousMatrix navigation(const vpImage< vpRGBa > &I, bool &changed)
#define vpTRACE
Definition: vpDebug.h:416
#define vpERROR_TRACE
Definition: vpDebug.h:393
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double getMinTimeForUsleepCall()
VISP_EXPORT double measureTimeSecond()
VISP_EXPORT double measureTimeMs()