Visual Servoing Platform version 3.5.0
vpSimulatorViper850.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 Viper850.
33 *
34 * Authors:
35 * Nicolas Melchior
36 *
37 *****************************************************************************/
38
39#include <visp3/robot/vpSimulatorViper850.h>
40
41#if defined(VISP_HAVE_MODULE_GUI) && ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD))
42
43#include <cmath> // std::fabs
44#include <limits> // numeric_limits
45#include <string>
46#include <visp3/core/vpImagePoint.h>
47#include <visp3/core/vpIoTools.h>
48#include <visp3/core/vpMeterPixelConversion.h>
49#include <visp3/core/vpPoint.h>
50#include <visp3/core/vpTime.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(), vpViper850(), 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 // free_Bound_scene (&(camera));
193 for (int i = 0; i < 6; i++)
194 free_Bound_scene(&(robotArms[i]));
195 }
196
197 delete[] robotArms;
198 delete[] fMi;
199}
200
210{
211 // set arm_dir from #define VISP_ROBOT_ARMS_DIR if it exists
212 // VISP_ROBOT_ARMS_DIR may contain multiple locations separated by ";"
213 std::vector<std::string> arm_dirs = vpIoTools::splitChain(std::string(VISP_ROBOT_ARMS_DIR), std::string(";"));
214 bool armDirExists = false;
215 for (size_t i = 0; i < arm_dirs.size(); i++)
216 if (vpIoTools::checkDirectory(arm_dirs[i]) == true) { // directory exists
217 arm_dir = arm_dirs[i];
218 armDirExists = true;
219 break;
220 }
221 if (!armDirExists) {
222 try {
223 arm_dir = vpIoTools::getenv("VISP_ROBOT_ARMS_DIR");
224 std::cout << "The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
225 } catch (...) {
226 std::cout << "Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
227 }
228 }
229
231 toolCustom = false;
232
233 size_fMi = 8;
234 fMi = new vpHomogeneousMatrix[8];
237
238 zeroPos.resize(njoint);
239 zeroPos = 0;
240 zeroPos[1] = -M_PI / 2;
241 zeroPos[2] = M_PI;
242 reposPos.resize(njoint);
243 reposPos = 0;
244 reposPos[1] = -M_PI / 2;
245 reposPos[2] = M_PI;
246 reposPos[4] = M_PI / 2;
247
248 artCoord = reposPos;
249 artVel = 0;
250
251 q_prev_getdis.resize(njoint);
252 q_prev_getdis = 0;
253 first_time_getdis = true;
254
255 positioningVelocity = defaultPositioningVelocity;
256
259
260 // Software joint limits in radians
261 // joint_min.resize(njoint);
262 joint_min[0] = vpMath::rad(-50);
263 joint_min[1] = vpMath::rad(-135);
264 joint_min[2] = vpMath::rad(-40);
265 joint_min[3] = vpMath::rad(-190);
266 joint_min[4] = vpMath::rad(-110);
267 joint_min[5] = vpMath::rad(-184);
268 // joint_max.resize(njoint);
269 joint_max[0] = vpMath::rad(50);
270 joint_max[1] = vpMath::rad(-40);
271 joint_max[2] = vpMath::rad(215);
272 joint_max[3] = vpMath::rad(190);
273 joint_max[4] = vpMath::rad(110);
274 joint_max[5] = vpMath::rad(184);
275}
276
281{
282 robotArms = NULL;
283 robotArms = new Bound_scene[6];
284 initArms();
286 cameraParam.initPersProjWithoutDistortion(558.5309599, 556.055053, 320, 240);
289 getCameraParameters(tmp, 640, 480);
290 px_int = tmp.get_px();
291 py_int = tmp.get_py();
292 sceneInitialized = true;
293}
294
311{
312 this->projModel = proj_model;
313
314 // Use here default values of the robot constant parameters.
315 switch (tool) {
317 erc[0] = vpMath::rad(0.07); // rx
318 erc[1] = vpMath::rad(2.76); // ry
319 erc[2] = vpMath::rad(-91.50); // rz
320 etc[0] = -0.0453; // tx
321 etc[1] = 0.0005; // ty
322 etc[2] = 0.0728; // tz
323
324 setCameraParameters(vpCameraParameters(1232.0, 1233.0, 320, 240));
325 break;
326 }
328 erc[0] = vpMath::rad(0.1527764261); // rx
329 erc[1] = vpMath::rad(1.285092455); // ry
330 erc[2] = vpMath::rad(-90.75777848); // rz
331 etc[0] = -0.04558630174; // tx
332 etc[1] = -0.00134326752; // ty
333 etc[2] = 0.001000828017; // tz
334
335 setCameraParameters(vpCameraParameters(868.0, 869.0, 320, 240));
336 break;
337 }
341 std::cout << "This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
342 break;
343 }
344 }
345
347 this->eMc.buildFrom(etc, eRc);
348
349 setToolType(tool);
350 return;
351}
352
363void vpSimulatorViper850::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
364 const unsigned int &image_height)
365{
366 if (toolCustom) {
367 cam.initPersProjWithoutDistortion(px_int, py_int, image_width / 2, image_height / 2);
368 }
369 // Set default parameters
370 switch (getToolType()) {
372 // Set default intrinsic camera parameters for 640x480 images
373 if (image_width == 640 && image_height == 480) {
374 std::cout << "Get default camera parameters for camera \"" << vpViper850::CONST_MARLIN_F033C_CAMERA_NAME << "\""
375 << std::endl;
376 cam.initPersProjWithoutDistortion(1232.0, 1233.0, 320, 240);
377 } else {
378 vpTRACE("Cannot get default intrinsic camera parameters for this image "
379 "resolution");
380 }
381 break;
382 }
384 // Set default intrinsic camera parameters for 640x480 images
385 if (image_width == 640 && image_height == 480) {
386 std::cout << "Get default camera parameters for camera \"" << vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME << "\""
387 << std::endl;
388 cam.initPersProjWithoutDistortion(868.0, 869.0, 320, 240);
389 } else {
390 vpTRACE("Cannot get default intrinsic camera parameters for this image "
391 "resolution");
392 }
393 break;
394 }
398 std::cout << "This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
399 break;
400 }
401 }
402 return;
403}
404
414{
415 getCameraParameters(cam, I_.getWidth(), I_.getHeight());
416}
417
427{
428 getCameraParameters(cam, I_.getWidth(), I_.getHeight());
429}
430
437{
438 px_int = cam.get_px();
439 py_int = cam.get_py();
440 toolCustom = true;
441}
442
448{
449 double tcur_1 = tcur; // temporary variable used to store the last time
450 // since the last command
451
452 while (!robotStop) {
453 // Get current time
454 tprev = tcur_1;
456
458 setVelocityCalled = false;
460
461 double ellapsedTime = (tcur - tprev) * 1e-3;
462 if (constantSamplingTimeMode) { // if we want a constant velocity, we
463 // force the ellapsed time to the given
464 // samplingTime
465 ellapsedTime = getSamplingTime(); // in second
466 }
467
468 vpColVector articularCoordinates = get_artCoord();
469 vpColVector articularVelocities = get_artVel();
470
471 if (jointLimit) {
472 double art = articularCoordinates[jointLimitArt - 1] + ellapsedTime * articularVelocities[jointLimitArt - 1];
473 if (art <= joint_min[jointLimitArt - 1] || art >= joint_max[jointLimitArt - 1]) {
474 if (verbose_) {
475 std::cout << "Joint " << jointLimitArt - 1
476 << " reaches a limit: " << vpMath::deg(joint_min[jointLimitArt - 1]) << " < " << vpMath::deg(art)
477 << " < " << vpMath::deg(joint_max[jointLimitArt - 1]) << std::endl;
478 }
479 articularVelocities = 0.0;
480 } else
481 jointLimit = false;
482 }
483
484 articularCoordinates[0] = articularCoordinates[0] + ellapsedTime * articularVelocities[0];
485 articularCoordinates[1] = articularCoordinates[1] + ellapsedTime * articularVelocities[1];
486 articularCoordinates[2] = articularCoordinates[2] + ellapsedTime * articularVelocities[2];
487 articularCoordinates[3] = articularCoordinates[3] + ellapsedTime * articularVelocities[3];
488 articularCoordinates[4] = articularCoordinates[4] + ellapsedTime * articularVelocities[4];
489 articularCoordinates[5] = articularCoordinates[5] + ellapsedTime * articularVelocities[5];
490
491 int jl = isInJointLimit();
492
493 if (jl != 0 && jointLimit == false) {
494 if (jl < 0)
495 ellapsedTime = (joint_min[(unsigned int)(-jl - 1)] - articularCoordinates[(unsigned int)(-jl - 1)]) /
496 (articularVelocities[(unsigned int)(-jl - 1)]);
497 else
498 ellapsedTime = (joint_max[(unsigned int)(jl - 1)] - articularCoordinates[(unsigned int)(jl - 1)]) /
499 (articularVelocities[(unsigned int)(jl - 1)]);
500
501 for (unsigned int i = 0; i < 6; i++)
502 articularCoordinates[i] = articularCoordinates[i] + ellapsedTime * articularVelocities[i];
503
504 jointLimit = true;
505 jointLimitArt = (unsigned int)fabs((double)jl);
506 }
507
508 set_artCoord(articularCoordinates);
509 set_artVel(articularVelocities);
510
511 compute_fMi();
512
513 if (displayAllowed) {
517 }
518
520 while (get_displayBusy())
521 vpTime::wait(2);
523 set_displayBusy(false);
524 }
525
527 vpHomogeneousMatrix fMit[8];
528 get_fMi(fMit);
529
530 // vpDisplay::displayFrame(I,getExternalCameraPosition
531 // ()*fMi[6],cameraParam,0.2,vpColor::none);
532
533 vpImagePoint iP, iP_1;
534 vpPoint pt(0, 0, 0);
535
538 pt.track(getExternalCameraPosition() * fMit[0]);
541 for (int k = 1; k < 7; k++) {
542 pt.track(getExternalCameraPosition() * fMit[k - 1]);
544
545 pt.track(getExternalCameraPosition() * fMit[k]);
547
549 }
551 thickness_);
552 }
553
555
557 tcur_1 = tcur;
558 } else {
560 }
561 }
562}
563
577{
578 // vpColVector q = get_artCoord();
579 vpColVector q(6); //; = get_artCoord();
580 q = get_artCoord();
581
582 vpHomogeneousMatrix fMit[8];
583
584 double q1 = q[0];
585 double q2 = q[1];
586 double q3 = q[2];
587 double q4 = q[3];
588 double q5 = q[4];
589 double q6 = q[5];
590
591 double c1 = cos(q1);
592 double s1 = sin(q1);
593 double c2 = cos(q2);
594 double s2 = sin(q2);
595 double c23 = cos(q2 + q3);
596 double s23 = sin(q2 + q3);
597 double c4 = cos(q4);
598 double s4 = sin(q4);
599 double c5 = cos(q5);
600 double s5 = sin(q5);
601 double c6 = cos(q6);
602 double s6 = sin(q6);
603
604 fMit[0][0][0] = c1;
605 fMit[0][1][0] = s1;
606 fMit[0][2][0] = 0;
607 fMit[0][0][1] = 0;
608 fMit[0][1][1] = 0;
609 fMit[0][2][1] = -1;
610 fMit[0][0][2] = -s1;
611 fMit[0][1][2] = c1;
612 fMit[0][2][2] = 0;
613 fMit[0][0][3] = a1 * c1;
614 fMit[0][1][3] = a1 * s1;
615 fMit[0][2][3] = d1;
616
617 fMit[1][0][0] = c1 * c2;
618 fMit[1][1][0] = s1 * c2;
619 fMit[1][2][0] = -s2;
620 fMit[1][0][1] = -c1 * s2;
621 fMit[1][1][1] = -s1 * s2;
622 fMit[1][2][1] = -c2;
623 fMit[1][0][2] = -s1;
624 fMit[1][1][2] = c1;
625 fMit[1][2][2] = 0;
626 fMit[1][0][3] = c1 * (a2 * c2 + a1);
627 fMit[1][1][3] = s1 * (a2 * c2 + a1);
628 fMit[1][2][3] = d1 - a2 * s2;
629
630 double quickcomp1 = a3 * c23 - a2 * c2 - a1;
631
632 fMit[2][0][0] = -c1 * c23;
633 fMit[2][1][0] = -s1 * c23;
634 fMit[2][2][0] = s23;
635 fMit[2][0][1] = s1;
636 fMit[2][1][1] = -c1;
637 fMit[2][2][1] = 0;
638 fMit[2][0][2] = c1 * s23;
639 fMit[2][1][2] = s1 * s23;
640 fMit[2][2][2] = c23;
641 fMit[2][0][3] = -c1 * quickcomp1;
642 fMit[2][1][3] = -s1 * quickcomp1;
643 fMit[2][2][3] = a3 * s23 - a2 * s2 + d1;
644
645 double quickcomp2 = c1 * (s23 * d4 - quickcomp1);
646 double quickcomp3 = s1 * (s23 * d4 - quickcomp1);
647
648 fMit[3][0][0] = -c1 * c23 * c4 + s1 * s4;
649 fMit[3][1][0] = -s1 * c23 * c4 - c1 * s4;
650 fMit[3][2][0] = s23 * c4;
651 fMit[3][0][1] = c1 * s23;
652 fMit[3][1][1] = s1 * s23;
653 fMit[3][2][1] = c23;
654 fMit[3][0][2] = -c1 * c23 * s4 - s1 * c4;
655 fMit[3][1][2] = -s1 * c23 * s4 + c1 * c4;
656 fMit[3][2][2] = s23 * s4;
657 fMit[3][0][3] = quickcomp2;
658 fMit[3][1][3] = quickcomp3;
659 fMit[3][2][3] = c23 * d4 + a3 * s23 - a2 * s2 + d1;
660
661 fMit[4][0][0] = c1 * (s23 * s5 - c5 * c23 * c4) + s1 * c5 * s4;
662 fMit[4][1][0] = s1 * (s23 * s5 - c5 * c23 * c4) - c1 * c5 * s4;
663 fMit[4][2][0] = s23 * c4 * c5 + c23 * s5;
664 fMit[4][0][1] = c1 * c23 * s4 + s1 * c4;
665 fMit[4][1][1] = s1 * c23 * s4 - c1 * c4;
666 fMit[4][2][1] = -s23 * s4;
667 fMit[4][0][2] = c1 * (s23 * c5 + s5 * c23 * c4) - s1 * s5 * s4;
668 fMit[4][1][2] = s1 * (s23 * c5 + s5 * c23 * c4) + c1 * s5 * s4;
669 fMit[4][2][2] = -s23 * c4 * s5 + c23 * c5;
670 fMit[4][0][3] = quickcomp2;
671 fMit[4][1][3] = quickcomp3;
672 fMit[4][2][3] = c23 * d4 + a3 * s23 - a2 * s2 + d1;
673
674 fMit[5][0][0] = c1 * (c23 * (c4 * c5 * c6 - s4 * s6) - s23 * s5 * c6) - s1 * (s4 * c5 * c6 + c4 * s6);
675 fMit[5][1][0] = -s1 * (c23 * (-c4 * c5 * c6 + s4 * s6) + s23 * s5 * c6) + c1 * (s4 * c5 * c6 + c4 * s6);
676 fMit[5][2][0] = s23 * (s4 * s6 - c4 * c5 * c6) - c23 * s5 * c6;
677 fMit[5][0][1] = -c1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) + s1 * (s4 * c5 * s6 - c4 * c6);
678 fMit[5][1][1] = -s1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) - c1 * (s4 * c5 * s6 - c4 * c6);
679 fMit[5][2][1] = s23 * (c4 * c5 * s6 + s4 * c6) + c23 * s5 * s6;
680 fMit[5][0][2] = c1 * (c23 * c4 * s5 + s23 * c5) - s1 * s4 * s5;
681 fMit[5][1][2] = s1 * (c23 * c4 * s5 + s23 * c5) + c1 * s4 * s5;
682 fMit[5][2][2] = -s23 * c4 * s5 + c23 * c5;
683 fMit[5][0][3] = quickcomp2;
684 fMit[5][1][3] = quickcomp3;
685 fMit[5][2][3] = s23 * a3 + c23 * d4 - a2 * s2 + d1;
686
687 fMit[6][0][0] = c1 * (c23 * (c4 * c5 * c6 - s4 * s6) - s23 * s5 * c6) - s1 * (s4 * c5 * c6 + c4 * s6);
688 fMit[6][1][0] = -s1 * (c23 * (-c4 * c5 * c6 + s4 * s6) + s23 * s5 * c6) + c1 * (s4 * c5 * c6 + c4 * s6);
689 fMit[6][2][0] = s23 * (s4 * s6 - c4 * c5 * c6) - c23 * s5 * c6;
690 fMit[6][0][1] = -c1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) + s1 * (s4 * c5 * s6 - c4 * c6);
691 fMit[6][1][1] = -s1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) - c1 * (s4 * c5 * s6 - c4 * c6);
692 fMit[6][2][1] = s23 * (c4 * c5 * s6 + s4 * c6) + c23 * s5 * s6;
693 fMit[6][0][2] = c1 * (c23 * c4 * s5 + s23 * c5) - s1 * s4 * s5;
694 fMit[6][1][2] = s1 * (c23 * c4 * s5 + s23 * c5) + c1 * s4 * s5;
695 fMit[6][2][2] = -s23 * c4 * s5 + c23 * c5;
696 fMit[6][0][3] = c1 * (c23 * (c4 * s5 * d6 - a3) + s23 * (c5 * d6 + d4) + a1 + a2 * c2) - s1 * s4 * s5 * d6;
697 fMit[6][1][3] = s1 * (c23 * (c4 * s5 * d6 - a3) + s23 * (c5 * d6 + d4) + a1 + a2 * c2) + c1 * s4 * s5 * d6;
698 fMit[6][2][3] = s23 * (a3 - c4 * s5 * d6) + c23 * (c5 * d6 + d4) - a2 * s2 + d1;
699
701 get_cMe(cMe);
702 cMe = cMe.inverse();
703 // fMit[7] = fMit[6] * cMe;
704 vpViper::get_fMc(q, fMit[7]);
705
706#if defined(_WIN32)
707#if defined(WINRT_8_1)
708 WaitForSingleObjectEx(mutex_fMi, INFINITE, FALSE);
709#else // pure win32
710 WaitForSingleObject(mutex_fMi, INFINITE);
711#endif
712 for (int i = 0; i < 8; i++)
713 fMi[i] = fMit[i];
714 ReleaseMutex(mutex_fMi);
715#elif defined(VISP_HAVE_PTHREAD)
716 pthread_mutex_lock(&mutex_fMi);
717 for (int i = 0; i < 8; i++)
718 fMi[i] = fMit[i];
719 pthread_mutex_unlock(&mutex_fMi);
720#endif
721}
722
729{
730 switch (newState) {
731 case vpRobot::STATE_STOP: {
732 // Start primitive STOP only if the current state is Velocity
734 stopMotion();
735 }
736 break;
737 }
740 std::cout << "Change the control mode from velocity to position control.\n";
741 stopMotion();
742 } else {
743 // std::cout << "Change the control mode from stop to position
744 // control.\n";
745 }
746 break;
747 }
750 std::cout << "Change the control mode from stop to velocity control.\n";
751 }
752 break;
753 }
755 default:
756 break;
757 }
758
759 return vpRobot::setRobotState(newState);
760}
761
836{
838 vpERROR_TRACE("Cannot send a velocity to the robot "
839 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
841 "Cannot send a velocity to the robot "
842 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
843 }
844
845 vpColVector vel_sat(6);
846
847 double scale_sat = 1;
848 double vel_trans_max = getMaxTranslationVelocity();
849 double vel_rot_max = getMaxRotationVelocity();
850
851 double vel_abs; // Absolute value
852
853 // Velocity saturation
854 switch (frame) {
855 // saturation in cartesian space
858 if (vel.getRows() != 6) {
859 vpERROR_TRACE("The velocity vector must have a size of 6 !!!!");
860 throw;
861 }
862
863 for (unsigned int i = 0; i < 3; ++i) {
864 vel_abs = fabs(vel[i]);
865 if (vel_abs > vel_trans_max && !jointLimit) {
866 vel_trans_max = vel_abs;
867 vpERROR_TRACE("Excess velocity %g m/s in TRANSLATION "
868 "(axis nr. %d).",
869 vel[i], i + 1);
870 }
871
872 vel_abs = fabs(vel[i + 3]);
873 if (vel_abs > vel_rot_max && !jointLimit) {
874 vel_rot_max = vel_abs;
875 vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
876 "(axis nr. %d).",
877 vel[i + 3], i + 4);
878 }
879 }
880
881 double scale_trans_sat = 1;
882 double scale_rot_sat = 1;
883 if (vel_trans_max > getMaxTranslationVelocity())
884 scale_trans_sat = getMaxTranslationVelocity() / vel_trans_max;
885
886 if (vel_rot_max > getMaxRotationVelocity())
887 scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
888
889 if ((scale_trans_sat < 1) || (scale_rot_sat < 1)) {
890 if (scale_trans_sat < scale_rot_sat)
891 scale_sat = scale_trans_sat;
892 else
893 scale_sat = scale_rot_sat;
894 }
895 break;
896 }
897
898 // saturation in joint space
900 if (vel.getRows() != 6) {
901 vpERROR_TRACE("The velocity vector must have a size of 6 !!!!");
902 throw;
903 }
904 for (unsigned int i = 0; i < 6; ++i) {
905 vel_abs = fabs(vel[i]);
906 if (vel_abs > vel_rot_max && !jointLimit) {
907 vel_rot_max = vel_abs;
908 vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
909 "(axis nr. %d).",
910 vel[i], i + 1);
911 }
912 }
913 double scale_rot_sat = 1;
914 if (vel_rot_max > getMaxRotationVelocity())
915 scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
916 if (scale_rot_sat < 1)
917 scale_sat = scale_rot_sat;
918 break;
919 }
921 case vpRobot::MIXT_FRAME: {
922 break;
923 }
924 }
925
926 set_velocity(vel * scale_sat);
927 setRobotFrame(frame);
928 setVelocityCalled = true;
929}
930
935{
937
938 double vel_rot_max = getMaxRotationVelocity();
939
940 vpColVector articularCoordinates = get_artCoord();
941 vpColVector velocityframe = get_velocity();
942 vpColVector articularVelocity;
943
944 switch (frame) {
946 vpMatrix eJe_;
948 vpViper850::get_eJe(articularCoordinates, eJe_);
949 eJe_ = eJe_.pseudoInverse();
951 singularityTest(articularCoordinates, eJe_);
952 articularVelocity = eJe_ * eVc * velocityframe;
953 set_artVel(articularVelocity);
954 break;
955 }
957 vpMatrix fJe_;
958 vpViper850::get_fJe(articularCoordinates, fJe_);
959 fJe_ = fJe_.pseudoInverse();
961 singularityTest(articularCoordinates, fJe_);
962 articularVelocity = fJe_ * velocityframe;
963 set_artVel(articularVelocity);
964 break;
965 }
967 articularVelocity = velocityframe;
968 set_artVel(articularVelocity);
969 break;
970 }
972 case vpRobot::MIXT_FRAME: {
973 break;
974 }
975 }
976
977 switch (frame) {
980 for (unsigned int i = 0; i < 6; ++i) {
981 double vel_abs = fabs(articularVelocity[i]);
982 if (vel_abs > vel_rot_max && !jointLimit) {
983 vel_rot_max = vel_abs;
984 vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
985 "(axis nr. %d).",
986 articularVelocity[i], i + 1);
987 }
988 }
989 double scale_rot_sat = 1;
990 double scale_sat = 1;
991
992 if (vel_rot_max > getMaxRotationVelocity())
993 scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
994 if (scale_rot_sat < 1)
995 scale_sat = scale_rot_sat;
996
997 set_artVel(articularVelocity * scale_sat);
998 break;
999 }
1002 case vpRobot::MIXT_FRAME: {
1003 break;
1004 }
1005 }
1006}
1007
1054{
1055 vel.resize(6);
1056
1057 vpColVector articularCoordinates = get_artCoord();
1058 vpColVector articularVelocity = get_artVel();
1059
1060 switch (frame) {
1061 case vpRobot::CAMERA_FRAME: {
1062 vpMatrix eJe_;
1064 vpViper850::get_eJe(articularCoordinates, eJe_);
1065 vel = cVe * eJe_ * articularVelocity;
1066 break;
1067 }
1069 vel = articularVelocity;
1070 break;
1071 }
1073 vpMatrix fJe_;
1074 vpViper850::get_fJe(articularCoordinates, fJe_);
1075 vel = fJe_ * articularVelocity;
1076 break;
1077 }
1079 case vpRobot::MIXT_FRAME: {
1080 break;
1081 }
1082 default: {
1083 vpERROR_TRACE("Error in spec of vpRobot. "
1084 "Case not taken in account.");
1085 return;
1086 }
1087 }
1088}
1089
1107{
1108 timestamp = vpTime::measureTimeSecond();
1109 getVelocity(frame, vel);
1110}
1111
1154{
1155 vpColVector vel(6);
1156 getVelocity(frame, vel);
1157
1158 return vel;
1159}
1160
1174{
1175 timestamp = vpTime::measureTimeSecond();
1176 vpColVector vel(6);
1177 getVelocity(frame, vel);
1178
1179 return vel;
1180}
1181
1183{
1184 double vel_rot_max = getMaxRotationVelocity();
1185 double velmax = fabs(q[0]);
1186 for (unsigned int i = 1; i < 6; i++) {
1187 if (velmax < fabs(q[i]))
1188 velmax = fabs(q[i]);
1189 }
1190
1191 double alpha = (getPositioningVelocity() * vel_rot_max) / (velmax * 100);
1192 q = q * alpha;
1193}
1194
1270{
1272 vpERROR_TRACE("Robot was not in position-based control\n"
1273 "Modification of the robot state");
1274 // setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
1275 }
1276
1277 vpColVector articularCoordinates = get_artCoord();
1278
1279 vpColVector error(6);
1280 double errsqr = 0;
1281 switch (frame) {
1282 case vpRobot::CAMERA_FRAME: {
1283 unsigned int nbSol;
1284 vpColVector qdes(6);
1285
1287 vpRxyzVector rxyz;
1288 for (unsigned int i = 0; i < 3; i++) {
1289 txyz[i] = q[i];
1290 rxyz[i] = q[i + 3];
1291 }
1292
1293 vpRotationMatrix cRc2(rxyz);
1294 vpHomogeneousMatrix cMc2(txyz, cRc2);
1295
1297 vpViper850::get_fMc(articularCoordinates, fMc_);
1298
1299 vpHomogeneousMatrix fMc2 = fMc_ * cMc2;
1300
1301 do {
1302 articularCoordinates = get_artCoord();
1303 qdes = articularCoordinates;
1304 nbSol = getInverseKinematics(fMc2, qdes, verbose_);
1305 setVelocityCalled = true;
1306 if (nbSol > 0) {
1307 error = qdes - articularCoordinates;
1308 errsqr = error.sumSquare();
1309 // findHighestPositioningSpeed(error);
1310 set_artVel(error);
1311 if (errsqr < 1e-4) {
1312 set_artCoord(qdes);
1313 error = 0;
1314 set_artVel(error);
1315 set_velocity(error);
1316 break;
1317 }
1318 } else {
1319 vpERROR_TRACE("Positionning error.");
1320 throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
1321 }
1322 } while (errsqr > 1e-8 && nbSol > 0);
1323
1324 break;
1325 }
1326
1328 do {
1329 articularCoordinates = get_artCoord();
1330 error = q - articularCoordinates;
1331 errsqr = error.sumSquare();
1332 // findHighestPositioningSpeed(error);
1333 set_artVel(error);
1334 setVelocityCalled = true;
1335 if (errsqr < 1e-4) {
1336 set_artCoord(q);
1337 error = 0;
1338 set_artVel(error);
1339 set_velocity(error);
1340 break;
1341 }
1342 } while (errsqr > 1e-8);
1343 break;
1344 }
1345
1347 unsigned int nbSol;
1348 vpColVector qdes(6);
1349
1351 vpRxyzVector rxyz;
1352 for (unsigned int i = 0; i < 3; i++) {
1353 txyz[i] = q[i];
1354 rxyz[i] = q[i + 3];
1355 }
1356
1357 vpRotationMatrix fRc(rxyz);
1358 vpHomogeneousMatrix fMc_(txyz, fRc);
1359
1360 do {
1361 articularCoordinates = get_artCoord();
1362 qdes = articularCoordinates;
1363 nbSol = getInverseKinematics(fMc_, qdes, verbose_);
1364 if (nbSol > 0) {
1365 error = qdes - articularCoordinates;
1366 errsqr = error.sumSquare();
1367 // findHighestPositioningSpeed(error);
1368 set_artVel(error);
1369 setVelocityCalled = true;
1370 if (errsqr < 1e-4) {
1371 set_artCoord(qdes);
1372 error = 0;
1373 set_artVel(error);
1374 set_velocity(error);
1375 break;
1376 }
1377 } else
1378 vpERROR_TRACE("Positionning error. Position unreachable");
1379 } while (errsqr > 1e-8 && nbSol > 0);
1380 break;
1381 }
1382 case vpRobot::MIXT_FRAME: {
1383 throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1384 "Mixt frame not implemented.");
1385 }
1387 throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1388 "End-effector frame not implemented.");
1389 }
1390 }
1391}
1392
1455void vpSimulatorViper850::setPosition(const vpRobot::vpControlFrameType frame, double pos1, double pos2,
1456 double pos3, double pos4, double pos5, double pos6)
1457{
1458 try {
1459 vpColVector position(6);
1460 position[0] = pos1;
1461 position[1] = pos2;
1462 position[2] = pos3;
1463 position[3] = pos4;
1464 position[4] = pos5;
1465 position[5] = pos6;
1466
1467 setPosition(frame, position);
1468 } catch (...) {
1469 vpERROR_TRACE("Error caught");
1470 throw;
1471 }
1472}
1473
1508void vpSimulatorViper850::setPosition(const char *filename)
1509{
1510 vpColVector q;
1511 bool ret;
1512
1513 ret = this->readPosFile(filename, q);
1514
1515 if (ret == false) {
1516 vpERROR_TRACE("Bad position in \"%s\"", filename);
1517 throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
1518 }
1521}
1522
1583{
1584 q.resize(6);
1585
1586 switch (frame) {
1587 case vpRobot::CAMERA_FRAME: {
1588 q = 0;
1589 break;
1590 }
1591
1593 q = get_artCoord();
1594 break;
1595 }
1596
1600
1601 vpRotationMatrix fRc;
1602 fMc_.extract(fRc);
1603 vpRxyzVector rxyz(fRc);
1604
1606 fMc_.extract(txyz);
1607
1608 for (unsigned int i = 0; i < 3; i++) {
1609 q[i] = txyz[i];
1610 q[i + 3] = rxyz[i];
1611 }
1612 break;
1613 }
1614
1615 case vpRobot::MIXT_FRAME: {
1616 vpERROR_TRACE("Positionning error. Mixt frame not implemented");
1617 throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1618 "Mixt frame not implemented.");
1619 }
1621 vpERROR_TRACE("Positionning error. Mixt frame not implemented");
1622 throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1623 "End-effector frame not implemented.");
1624 }
1625 }
1626}
1627
1655{
1656 timestamp = vpTime::measureTimeSecond();
1657 getPosition(frame, q);
1658}
1659
1672{
1673 vpColVector posRxyz;
1674 // recupere position en Rxyz
1675 this->getPosition(frame, posRxyz);
1676
1677 // recupere le vecteur thetaU correspondant
1678 vpThetaUVector RtuVect(vpRxyzVector(posRxyz[3], posRxyz[4], posRxyz[5]));
1679
1680 // remplit le vpPoseVector avec translation et rotation ThetaU
1681 for (unsigned int j = 0; j < 3; j++) {
1682 position[j] = posRxyz[j];
1683 position[j + 3] = RtuVect[j];
1684 }
1685}
1686
1699 double &timestamp)
1700{
1701 timestamp = vpTime::measureTimeSecond();
1702 getPosition(frame, position);
1703}
1704
1713void vpSimulatorViper850::setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
1714{
1715 if (limitMin.getRows() != 6 || limitMax.getRows() != 6) {
1716 vpTRACE("Joint limit vector has not a size of 6 !");
1717 return;
1718 }
1719
1720 joint_min[0] = limitMin[0];
1721 joint_min[1] = limitMin[1];
1722 joint_min[2] = limitMin[2];
1723 joint_min[3] = limitMin[3];
1724 joint_min[4] = limitMin[4];
1725 joint_min[5] = limitMin[5];
1726
1727 joint_max[0] = limitMax[0];
1728 joint_max[1] = limitMax[1];
1729 joint_max[2] = limitMax[2];
1730 joint_max[3] = limitMax[3];
1731 joint_max[4] = limitMax[4];
1732 joint_max[5] = limitMax[5];
1733}
1734
1741{
1742 double q2 = q[1];
1743 double q3 = q[2];
1744 double q5 = q[4];
1745
1746 double c2 = cos(q2);
1747 double c3 = cos(q3);
1748 double s3 = sin(q3);
1749 double c23 = cos(q2 + q3);
1750 double s23 = sin(q2 + q3);
1751 double s5 = sin(q5);
1752
1753 bool cond1 = fabs(s5) < 1e-1;
1754 bool cond2 = fabs(a3 * s3 + c3 * d4) < 1e-1;
1755 bool cond3 = fabs(a2 * c2 - a3 * c23 + s23 * d4 + a1) < 1e-1;
1756
1757 if (cond1) {
1758 J[3][0] = 0;
1759 J[5][0] = 0;
1760 J[3][1] = 0;
1761 J[5][1] = 0;
1762 J[3][2] = 0;
1763 J[5][2] = 0;
1764 J[3][3] = 0;
1765 J[5][3] = 0;
1766 J[3][4] = 0;
1767 J[5][4] = 0;
1768 J[3][5] = 0;
1769 J[5][5] = 0;
1770 return true;
1771 }
1772
1773 if (cond2) {
1774 J[1][0] = 0;
1775 J[2][0] = 0;
1776 J[3][0] = 0;
1777 J[4][0] = 0;
1778 J[5][0] = 0;
1779 J[1][1] = 0;
1780 J[2][1] = 0;
1781 J[3][1] = 0;
1782 J[4][1] = 0;
1783 J[5][1] = 0;
1784 J[1][2] = 0;
1785 J[2][2] = 0;
1786 J[3][2] = 0;
1787 J[4][2] = 0;
1788 J[5][2] = 0;
1789 return true;
1790 }
1791
1792 if (cond3) {
1793 J[0][0] = 0;
1794 J[3][0] = 0;
1795 J[4][0] = 0;
1796 J[5][0] = 0;
1797 J[0][1] = 0;
1798 J[3][1] = 0;
1799 J[4][1] = 0;
1800 J[5][1] = 0;
1801 }
1802
1803 return false;
1804}
1805
1810{
1811 int artNumb = 0;
1812 double diff = 0;
1813 double difft = 0;
1814
1815 vpColVector articularCoordinates = get_artCoord();
1816
1817 for (unsigned int i = 0; i < 6; i++) {
1818 if (articularCoordinates[i] <= joint_min[i]) {
1819 difft = joint_min[i] - articularCoordinates[i];
1820 if (difft > diff) {
1821 diff = difft;
1822 artNumb = -(int)i - 1;
1823 }
1824 }
1825 }
1826
1827 for (unsigned int i = 0; i < 6; i++) {
1828 if (articularCoordinates[i] >= joint_max[i]) {
1829 difft = articularCoordinates[i] - joint_max[i];
1830 if (difft > diff) {
1831 diff = difft;
1832 artNumb = (int)(i + 1);
1833 }
1834 }
1835 }
1836
1837 if (artNumb != 0)
1838 std::cout << "\nWarning: Velocity control stopped: axis " << fabs((float)artNumb) << " on joint limit!"
1839 << std::endl;
1840
1841 return artNumb;
1842}
1843
1862{
1863 displacement.resize(6);
1864 displacement = 0;
1865 vpColVector q_cur(6);
1866
1867 q_cur = get_artCoord();
1868
1869 if (!first_time_getdis) {
1870 switch (frame) {
1871 case vpRobot::CAMERA_FRAME: {
1872 std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
1873 return;
1874 }
1875
1877 displacement = q_cur - q_prev_getdis;
1878 break;
1879 }
1880
1882 std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
1883 return;
1884 }
1885
1886 case vpRobot::MIXT_FRAME: {
1887 std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
1888 return;
1889 }
1891 std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
1892 return;
1893 }
1894 }
1895 } else {
1896 first_time_getdis = false;
1897 }
1898
1899 // Memorize the joint position for the next call
1900 q_prev_getdis = q_cur;
1901}
1902
1964bool vpSimulatorViper850::readPosFile(const std::string &filename, vpColVector &q)
1965{
1966 std::ifstream fd(filename.c_str(), std::ios::in);
1967
1968 if (!fd.is_open()) {
1969 return false;
1970 }
1971
1972 std::string line;
1973 std::string key("R:");
1974 std::string id("#Viper850 - Position");
1975 bool pos_found = false;
1976 int lineNum = 0;
1977
1978 q.resize(njoint);
1979
1980 while (std::getline(fd, line)) {
1981 lineNum++;
1982 if (lineNum == 1) {
1983 if (!(line.compare(0, id.size(), id) == 0)) { // check if Viper850 position file
1984 std::cout << "Error: this position file " << filename << " is not for Viper850 robot" << std::endl;
1985 return false;
1986 }
1987 }
1988 if ((line.compare(0, 1, "#") == 0)) { // skip comment
1989 continue;
1990 }
1991 if ((line.compare(0, key.size(), key) == 0)) { // decode position
1992 // check if there are at least njoint values in the line
1993 std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
1994 if (chain.size() < njoint + 1) // try to split with tab separator
1995 chain = vpIoTools::splitChain(line, std::string("\t"));
1996 if (chain.size() < njoint + 1)
1997 continue;
1998
1999 std::istringstream ss(line);
2000 std::string key_;
2001 ss >> key_;
2002 for (unsigned int i = 0; i < njoint; i++)
2003 ss >> q[i];
2004 pos_found = true;
2005 break;
2006 }
2007 }
2008
2009 // converts rotations from degrees into radians
2010 q.deg2rad();
2011
2012 fd.close();
2013
2014 if (!pos_found) {
2015 std::cout << "Error: unable to find a position for Viper850 robot in " << filename << std::endl;
2016 return false;
2017 }
2018
2019 return true;
2020}
2021
2043bool vpSimulatorViper850::savePosFile(const std::string &filename, const vpColVector &q)
2044{
2045
2046 FILE *fd;
2047 fd = fopen(filename.c_str(), "w");
2048 if (fd == NULL)
2049 return false;
2050
2051 fprintf(fd, "\
2052#Viper - Position - Version 1.0\n\
2053#\n\
2054# R: A B C D E F\n\
2055# Joint position in degrees\n\
2056#\n\
2057#\n\n");
2058
2059 // Save positions in mm and deg
2060 fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n", vpMath::deg(q[0]), vpMath::deg(q[1]), vpMath::deg(q[2]),
2061 vpMath::deg(q[3]), vpMath::deg(q[4]), vpMath::deg(q[5]));
2062
2063 fclose(fd);
2064 return (true);
2065}
2066
2074void vpSimulatorViper850::move(const char *filename)
2075{
2076 vpColVector q;
2077
2078 try {
2079 this->readPosFile(filename, q);
2082 } catch (...) {
2083 throw;
2084 }
2085}
2086
2097
2106{
2109
2110 cVe.buildFrom(cMe);
2111}
2112
2123{
2124 try {
2126 } catch (...) {
2127 vpERROR_TRACE("catch exception ");
2128 throw;
2129 }
2130}
2131
2143{
2144 try {
2145 vpColVector articularCoordinates = get_artCoord();
2146 vpViper850::get_fJe(articularCoordinates, fJe_);
2147 } catch (...) {
2148 vpERROR_TRACE("Error caught");
2149 throw;
2150 }
2151}
2152
2157{
2159 return;
2160
2161 vpColVector stop(6);
2162 stop = 0;
2163 set_artVel(stop);
2164 set_velocity(stop);
2166}
2167
2168/**********************************************************************************/
2169/**********************************************************************************/
2170/**********************************************************************************/
2171/**********************************************************************************/
2172
2182{
2183 // set scene_dir from #define VISP_SCENE_DIR if it exists
2184 // VISP_SCENES_DIR may contain multiple locations separated by ";"
2185 std::string scene_dir_;
2186 std::vector<std::string> scene_dirs = vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(";"));
2187 bool sceneDirExists = false;
2188 for (size_t i = 0; i < scene_dirs.size(); i++)
2189 if (vpIoTools::checkDirectory(scene_dirs[i]) == true) { // directory exists
2190 scene_dir_ = scene_dirs[i];
2191 sceneDirExists = true;
2192 break;
2193 }
2194 if (!sceneDirExists) {
2195 try {
2196 scene_dir_ = vpIoTools::getenv("VISP_SCENES_DIR");
2197 std::cout << "The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
2198 } catch (...) {
2199 std::cout << "Cannot get VISP_SCENES_DIR environment variable" << std::endl;
2200 }
2201 }
2202
2203 unsigned int name_length = 30; // the size of this kind of string "/viper850_arm2.bnd"
2204 if (scene_dir_.size() > FILENAME_MAX)
2205 throw vpException(vpException::dimensionError, "Cannot initialize Viper850 simulator");
2206
2207 unsigned int full_length = (unsigned int)scene_dir_.size() + name_length;
2208 if (full_length > FILENAME_MAX)
2209 throw vpException(vpException::dimensionError, "Cannot initialize Viper850 simulator");
2210
2211 char *name_cam = new char[full_length];
2212
2213 strcpy(name_cam, scene_dir_.c_str());
2214 strcat(name_cam, "/camera.bnd");
2215 set_scene(name_cam, &camera, cameraFactor);
2216
2217 if (arm_dir.size() > FILENAME_MAX)
2218 throw vpException(vpException::dimensionError, "Cannot initialize Viper850 simulator");
2219 full_length = (unsigned int)arm_dir.size() + name_length;
2220 if (full_length > FILENAME_MAX)
2221 throw vpException(vpException::dimensionError, "Cannot initialize Viper850 simulator");
2222
2223 char *name_arm = new char[full_length];
2224 strcpy(name_arm, arm_dir.c_str());
2225 strcat(name_arm, "/viper850_arm1.bnd");
2226 set_scene(name_arm, robotArms, 1.0);
2227 strcpy(name_arm, arm_dir.c_str());
2228 strcat(name_arm, "/viper850_arm2.bnd");
2229 set_scene(name_arm, robotArms + 1, 1.0);
2230 strcpy(name_arm, arm_dir.c_str());
2231 strcat(name_arm, "/viper850_arm3.bnd");
2232 set_scene(name_arm, robotArms + 2, 1.0);
2233 strcpy(name_arm, arm_dir.c_str());
2234 strcat(name_arm, "/viper850_arm4.bnd");
2235 set_scene(name_arm, robotArms + 3, 1.0);
2236 strcpy(name_arm, arm_dir.c_str());
2237 strcat(name_arm, "/viper850_arm5.bnd");
2238 set_scene(name_arm, robotArms + 4, 1.0);
2239 strcpy(name_arm, arm_dir.c_str());
2240 strcat(name_arm, "/viper850_arm6.bnd");
2241 set_scene(name_arm, robotArms + 5, 1.0);
2242
2243 // set_scene("./arm2.bnd", robotArms+1, 1.0);
2244 // set_scene("./arm3.bnd", robotArms+2, 1.0);
2245 // set_scene("./arm4.bnd", robotArms+3, 1.0);
2246 // set_scene("./arm5.bnd", robotArms+4, 1.0);
2247 // set_scene("./arm6.bnd", robotArms+5, 1.0);
2248
2249 add_rfstack(IS_BACK);
2250
2251 add_vwstack("start", "depth", 0.0, 100.0);
2252 add_vwstack("start", "window", -0.1, 0.1, -0.1, 0.1);
2253 add_vwstack("start", "type", PERSPECTIVE);
2254 //
2255 // sceneInitialized = true;
2256 // displayObject = true;
2257 displayCamera = true;
2258
2259 delete[] name_cam;
2260 delete[] name_arm;
2261}
2262
2264{
2265 bool changed = false;
2266 vpHomogeneousMatrix displacement = navigation(I_, changed);
2267
2268 // if (displacement[2][3] != 0)
2269 if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
2270 camMf2 = camMf2 * displacement;
2271
2272 f2Mf = camMf2.inverse() * camMf;
2273
2274 camMf = camMf2 * displacement * f2Mf;
2275
2276 double u;
2277 double v;
2278 // if(px_ext != 1 && py_ext != 1)
2279 // we assume px_ext and py_ext > 0
2280 if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
2281 (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon())) {
2282 u = (double)I_.getWidth() / (2 * px_ext);
2283 v = (double)I_.getHeight() / (2 * py_ext);
2284 } else {
2285 u = (double)I_.getWidth() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
2286 v = (double)I_.getHeight() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
2287 }
2288
2289 float w44o[4][4], w44cext[4][4], x, y, z;
2290
2291 vp2jlc_matrix(camMf.inverse(), w44cext);
2292
2293 add_vwstack("start", "cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
2294 x = w44cext[2][0] + w44cext[3][0];
2295 y = w44cext[2][1] + w44cext[3][1];
2296 z = w44cext[2][2] + w44cext[3][2];
2297 add_vwstack("start", "vrp", x, y, z);
2298 add_vwstack("start", "vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
2299 add_vwstack("start", "vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
2300 add_vwstack("start", "window", -u, u, -v, v);
2301
2302 vpHomogeneousMatrix fMit[8];
2303 get_fMi(fMit);
2304
2305 vp2jlc_matrix(vpHomogeneousMatrix(0, 0, 0, 0, 0, 0), w44o);
2306 display_scene(w44o, robotArms[0], I_, curColor);
2307
2308 vp2jlc_matrix(fMit[0], w44o);
2309 display_scene(w44o, robotArms[1], I_, curColor);
2310
2311 vp2jlc_matrix(fMit[1], w44o);
2312 display_scene(w44o, robotArms[2], I_, curColor);
2313
2314 vp2jlc_matrix(fMit[2], w44o);
2315 display_scene(w44o, robotArms[3], I_, curColor);
2316
2317 vp2jlc_matrix(fMit[3], w44o);
2318 display_scene(w44o, robotArms[4], I_, curColor);
2319
2320 vp2jlc_matrix(fMit[6], w44o);
2321 display_scene(w44o, robotArms[5], I_, curColor);
2322
2323 if (displayCamera) {
2325 get_cMe(cMe);
2326 cMe = cMe.inverse();
2327 cMe = fMit[6] * cMe;
2328 vp2jlc_matrix(cMe, w44o);
2329 display_scene(w44o, camera, I_, camColor);
2330 }
2331
2332 if (displayObject) {
2333 vp2jlc_matrix(fMo, w44o);
2334 display_scene(w44o, scene, I_, curColor);
2335 }
2336}
2337
2355{
2356 vpColVector stop(6);
2357 bool status = true;
2358 stop = 0;
2359 set_artVel(stop);
2360 set_velocity(stop);
2362 fMc_ = fMo * cMo_.inverse();
2363
2364 vpColVector articularCoordinates = get_artCoord();
2365 unsigned int nbSol = getInverseKinematics(fMc_, articularCoordinates, verbose_);
2366
2367 if (nbSol == 0) {
2368 status = false;
2369 vpERROR_TRACE("Positionning error. Position unreachable");
2370 }
2371
2372 if (verbose_)
2373 std::cout << "Used joint coordinates (rad): " << articularCoordinates.t() << std::endl;
2374
2375 set_artCoord(articularCoordinates);
2376
2377 compute_fMi();
2378
2379 return status;
2380}
2381
2396{
2397 vpColVector stop(6);
2398 stop = 0;
2399 set_artVel(stop);
2400 set_velocity(stop);
2401 vpHomogeneousMatrix fMit[8];
2402 get_fMi(fMit);
2403 fMo = fMit[7] * cMo_;
2404}
2405
2406#elif !defined(VISP_BUILD_SHARED_LIBS)
2407// Work arround to avoid warning: libvisp_robot.a(vpSimulatorViper850.cpp.o)
2408// has no symbols
2409void dummy_vpSimulatorViper850(){};
2410#endif
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
void deg2rad()
Definition: vpColVector.h:196
double sumSquare() const
vpRowVector t() 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)
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
vpControlFrameType setRobotFrame(vpRobot::vpControlFrameType newFrame)
Definition: vpRobot.cpp:207
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:184
static bool savePosFile(const std::string &filename, const vpColVector &q)
void get_fMi(vpHomogeneousMatrix *fMit)
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &displacement)
void get_fJe(vpMatrix &fJe)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
void setCameraParameters(const vpCameraParameters &cam)
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height)
void getExternalImage(vpImage< vpRGBa > &I)
void get_cMe(vpHomogeneousMatrix &cMe)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
void get_cVe(vpVelocityTwistMatrix &cVe)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q)
void findHighestPositioningSpeed(vpColVector &q)
bool singularityTest(const vpColVector &q, vpMatrix &J)
void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
static const double defaultPositioningVelocity
vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
void initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo)
static bool readPosFile(const std::string &filename, vpColVector &q)
void move(const char *filename)
void get_eJe(vpMatrix &eJe)
double getPositioningVelocity(void)
bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo)
Implementation of a rotation vector as axis-angle minimal representation.
Class that consider the case of a translation vector.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Modelisation of the ADEPT Viper 850 robot.
Definition: vpViper850.h:104
static const char *const CONST_PTGREY_FLEA2_CAMERA_NAME
Definition: vpViper850.h:123
void setToolType(vpViper850::vpToolType tool)
Set the current tool type.
Definition: vpViper850.h:170
static const vpToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpViper850.h:137
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpViper850.h:177
vpToolType getToolType() const
Get the current tool type.
Definition: vpViper850.h:161
vpToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpViper850.h:128
@ TOOL_SCHUNK_GRIPPER_CAMERA
Definition: vpViper850.h:131
@ TOOL_PTGREY_FLEA2_CAMERA
Definition: vpViper850.h:130
@ TOOL_MARLIN_F033C_CAMERA
Definition: vpViper850.h:129
@ TOOL_GENERIC_CAMERA
Definition: vpViper850.h:132
static const char *const CONST_MARLIN_F033C_CAMERA_NAME
Definition: vpViper850.h:122
vpTranslationVector etc
Definition: vpViper.h:159
double d6
for joint 6
Definition: vpViper.h:167
double a3
for joint 3
Definition: vpViper.h:165
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpViper.cpp:922
double d4
for joint 4
Definition: vpViper.h:166
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpViper.cpp:1159
vpColVector joint_max
Definition: vpViper.h:172
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false) const
Definition: vpViper.cpp:563
vpHomogeneousMatrix eMc
End effector to camera transformation.
Definition: vpViper.h:157
double a1
Definition: vpViper.h:163
vpRxyzVector erc
Definition: vpViper.h:160
vpColVector joint_min
Definition: vpViper.h:173
double a2
for joint 2
Definition: vpViper.h:164
static const unsigned int njoint
Number of joint.
Definition: vpViper.h:154
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpViper.cpp:970
double d1
for joint 1
Definition: vpViper.h:163
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpViper.cpp:600
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()