Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpSimulatorViper850.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See https://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Class which provides a simulator for the robot Viper850.
33 *
34*****************************************************************************/
35
36#include <visp3/robot/vpSimulatorViper850.h>
37
38#if defined(VISP_HAVE_MODULE_GUI) && ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD))
39
40#include <cmath> // std::fabs
41#include <limits> // numeric_limits
42#include <string>
43#include <visp3/core/vpImagePoint.h>
44#include <visp3/core/vpIoTools.h>
45#include <visp3/core/vpMeterPixelConversion.h>
46#include <visp3/core/vpPoint.h>
47#include <visp3/core/vpTime.h>
48
49#include "../wireframe-simulator/vpBound.h"
50#include "../wireframe-simulator/vpRfstack.h"
51#include "../wireframe-simulator/vpScene.h"
52#include "../wireframe-simulator/vpVwstack.h"
53
55
60 : vpRobotWireFrameSimulator(), vpViper850(), q_prev_getdis(), first_time_getdis(true),
61 positioningVelocity(defaultPositioningVelocity), zeroPos(), reposPos(), toolCustom(false), arm_dir()
62{
63 init();
65
67
68#if defined(_WIN32)
69
70 DWORD dwThreadIdArray;
71 hThread = CreateThread(NULL, // default security attributes
72 0, // use default stack size
73 launcher, // thread function name
74 this, // argument to thread function
75 0, // use default creation flags
76 &dwThreadIdArray); // returns the thread identifier
77#elif defined(VISP_HAVE_PTHREAD)
78 pthread_attr_init(&attr);
79 pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
80
81 pthread_create(&thread, NULL, launcher, (void *)this);
82#endif
83
85}
86
94 : vpRobotWireFrameSimulator(do_display), q_prev_getdis(), first_time_getdis(true),
95 positioningVelocity(defaultPositioningVelocity), zeroPos(), reposPos(), toolCustom(false), arm_dir()
96{
97 init();
99
101
102#if defined(_WIN32)
103 DWORD dwThreadIdArray;
104 hThread = CreateThread(NULL, // default security attributes
105 0, // use default stack size
106 launcher, // thread function name
107 this, // argument to thread function
108 0, // use default creation flags
109 &dwThreadIdArray); // returns the thread identifier
110#elif defined(VISP_HAVE_PTHREAD)
111 pthread_attr_init(&attr);
112 pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
113
114 pthread_create(&thread, NULL, launcher, (void *)this);
115#endif
116
117 compute_fMi();
118}
119
124{
126 robotStop = true;
128
129#if defined(_WIN32)
130#if defined(WINRT_8_1)
131 WaitForSingleObjectEx(hThread, INFINITE, FALSE);
132#else // pure win32
133 WaitForSingleObject(hThread, INFINITE);
134#endif
135 CloseHandle(hThread);
136#elif defined(VISP_HAVE_PTHREAD)
137 pthread_attr_destroy(&attr);
138 pthread_join(thread, NULL);
139#endif
140
141 if (robotArms != NULL) {
142 // free_Bound_scene (&(camera));
143 for (int i = 0; i < 6; i++)
144 free_Bound_scene(&(robotArms[i]));
145 }
146
147 delete[] robotArms;
148 delete[] fMi;
149}
150
160{
161 // set arm_dir from #define VISP_ROBOT_ARMS_DIR if it exists
162 // VISP_ROBOT_ARMS_DIR may contain multiple locations separated by ";"
163 std::vector<std::string> arm_dirs = vpIoTools::splitChain(std::string(VISP_ROBOT_ARMS_DIR), std::string(";"));
164 bool armDirExists = false;
165 for (size_t i = 0; i < arm_dirs.size(); i++)
166 if (vpIoTools::checkDirectory(arm_dirs[i]) == true) { // directory exists
167 arm_dir = arm_dirs[i];
168 armDirExists = true;
169 break;
170 }
171 if (!armDirExists) {
172 try {
173 arm_dir = vpIoTools::getenv("VISP_ROBOT_ARMS_DIR");
174 std::cout << "The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
175 } catch (...) {
176 std::cout << "Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
177 }
178 }
179
181 toolCustom = false;
182
183 size_fMi = 8;
184 fMi = new vpHomogeneousMatrix[8];
187
188 zeroPos.resize(njoint);
189 zeroPos = 0;
190 zeroPos[1] = -M_PI / 2;
191 zeroPos[2] = M_PI;
192 reposPos.resize(njoint);
193 reposPos = 0;
194 reposPos[1] = -M_PI / 2;
195 reposPos[2] = M_PI;
196 reposPos[4] = M_PI / 2;
197
198 artCoord = reposPos;
199 artVel = 0;
200
201 q_prev_getdis.resize(njoint);
202 q_prev_getdis = 0;
203 first_time_getdis = true;
204
205 positioningVelocity = defaultPositioningVelocity;
206
209
210 // Software joint limits in radians
211 // joint_min.resize(njoint);
212 joint_min[0] = vpMath::rad(-50);
213 joint_min[1] = vpMath::rad(-135);
214 joint_min[2] = vpMath::rad(-40);
215 joint_min[3] = vpMath::rad(-190);
216 joint_min[4] = vpMath::rad(-110);
217 joint_min[5] = vpMath::rad(-184);
218 // joint_max.resize(njoint);
219 joint_max[0] = vpMath::rad(50);
220 joint_max[1] = vpMath::rad(-40);
221 joint_max[2] = vpMath::rad(215);
222 joint_max[3] = vpMath::rad(190);
223 joint_max[4] = vpMath::rad(110);
224 joint_max[5] = vpMath::rad(184);
225}
226
231{
232 robotArms = NULL;
233 robotArms = new Bound_scene[6];
234 initArms();
236 cameraParam.initPersProjWithoutDistortion(558.5309599, 556.055053, 320, 240);
239 getCameraParameters(tmp, 640, 480);
240 px_int = tmp.get_px();
241 py_int = tmp.get_py();
242 sceneInitialized = true;
243}
244
261{
262 this->projModel = proj_model;
263
264 // Use here default values of the robot constant parameters.
265 switch (tool) {
267 erc[0] = vpMath::rad(0.07); // rx
268 erc[1] = vpMath::rad(2.76); // ry
269 erc[2] = vpMath::rad(-91.50); // rz
270 etc[0] = -0.0453; // tx
271 etc[1] = 0.0005; // ty
272 etc[2] = 0.0728; // tz
273
274 setCameraParameters(vpCameraParameters(1232.0, 1233.0, 320, 240));
275 break;
276 }
278 erc[0] = vpMath::rad(0.1527764261); // rx
279 erc[1] = vpMath::rad(1.285092455); // ry
280 erc[2] = vpMath::rad(-90.75777848); // rz
281 etc[0] = -0.04558630174; // tx
282 etc[1] = -0.00134326752; // ty
283 etc[2] = 0.001000828017; // tz
284
285 setCameraParameters(vpCameraParameters(868.0, 869.0, 320, 240));
286 break;
287 }
291 std::cout << "This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
292 break;
293 }
294 }
295
297
299 this->eMc.buildFrom(etc, eRc);
301
302 setToolType(tool);
303 return;
304}
305
316void vpSimulatorViper850::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
317 const unsigned int &image_height)
318{
319 if (toolCustom) {
320 cam.initPersProjWithoutDistortion(px_int, py_int, image_width / 2, image_height / 2);
321 }
322 // Set default parameters
323 switch (getToolType()) {
325 // Set default intrinsic camera parameters for 640x480 images
326 if (image_width == 640 && image_height == 480) {
327 std::cout << "Get default camera parameters for camera \"" << vpViper850::CONST_MARLIN_F033C_CAMERA_NAME << "\""
328 << std::endl;
329 cam.initPersProjWithoutDistortion(1232.0, 1233.0, 320, 240);
330 } else {
331 vpTRACE("Cannot get default intrinsic camera parameters for this image "
332 "resolution");
333 }
334 break;
335 }
337 // Set default intrinsic camera parameters for 640x480 images
338 if (image_width == 640 && image_height == 480) {
339 std::cout << "Get default camera parameters for camera \"" << vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME << "\""
340 << std::endl;
341 cam.initPersProjWithoutDistortion(868.0, 869.0, 320, 240);
342 } else {
343 vpTRACE("Cannot get default intrinsic camera parameters for this image "
344 "resolution");
345 }
346 break;
347 }
351 std::cout << "This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
352 break;
353 }
354 }
355 return;
356}
357
370
383
390{
391 px_int = cam.get_px();
392 py_int = cam.get_py();
393 toolCustom = true;
394}
395
401{
402 double tcur_1 = tcur; // temporary variable used to store the last time
403 // since the last command
404
405 bool stop = false;
406 bool setVelocityCalled_ = false;
407 while (!stop) {
408 // Get current time
409 tprev = tcur_1;
411
413 setVelocityCalled_ = setVelocityCalled;
415
416 if (setVelocityCalled_ || !constantSamplingTimeMode) {
418 setVelocityCalled = false;
421
422 double ellapsedTime = (tcur - tprev) * 1e-3;
423 if (constantSamplingTimeMode) { // if we want a constant velocity, we
424 // force the ellapsed time to the given
425 // samplingTime
426 ellapsedTime = getSamplingTime(); // in second
427 }
428
429 vpColVector articularCoordinates = get_artCoord();
430 vpColVector articularVelocities = get_artVel();
431
432 if (jointLimit) {
433 double art = articularCoordinates[jointLimitArt - 1] + ellapsedTime * articularVelocities[jointLimitArt - 1];
434 if (art <= joint_min[jointLimitArt - 1] || art >= joint_max[jointLimitArt - 1]) {
435 if (verbose_) {
436 std::cout << "Joint " << jointLimitArt - 1
437 << " reaches a limit: " << vpMath::deg(joint_min[jointLimitArt - 1]) << " < " << vpMath::deg(art)
438 << " < " << vpMath::deg(joint_max[jointLimitArt - 1]) << std::endl;
439 }
440 articularVelocities = 0.0;
441 } else
442 jointLimit = false;
443 }
444
445 articularCoordinates[0] = articularCoordinates[0] + ellapsedTime * articularVelocities[0];
446 articularCoordinates[1] = articularCoordinates[1] + ellapsedTime * articularVelocities[1];
447 articularCoordinates[2] = articularCoordinates[2] + ellapsedTime * articularVelocities[2];
448 articularCoordinates[3] = articularCoordinates[3] + ellapsedTime * articularVelocities[3];
449 articularCoordinates[4] = articularCoordinates[4] + ellapsedTime * articularVelocities[4];
450 articularCoordinates[5] = articularCoordinates[5] + ellapsedTime * articularVelocities[5];
451
452 int jl = isInJointLimit();
453
454 if (jl != 0 && jointLimit == false) {
455 if (jl < 0)
456 ellapsedTime = (joint_min[(unsigned int)(-jl - 1)] - articularCoordinates[(unsigned int)(-jl - 1)]) /
457 (articularVelocities[(unsigned int)(-jl - 1)]);
458 else
459 ellapsedTime = (joint_max[(unsigned int)(jl - 1)] - articularCoordinates[(unsigned int)(jl - 1)]) /
460 (articularVelocities[(unsigned int)(jl - 1)]);
461
462 for (unsigned int i = 0; i < 6; i++)
463 articularCoordinates[i] = articularCoordinates[i] + ellapsedTime * articularVelocities[i];
464
465 jointLimit = true;
466 jointLimitArt = (unsigned int)fabs((double)jl);
467 }
468
469 set_artCoord(articularCoordinates);
470 set_artVel(articularVelocities);
471
472 compute_fMi();
473
474 if (displayAllowed) {
478 }
479
481 while (get_displayBusy()) {
482 vpTime::wait(2);
483 }
485 set_displayBusy(false);
486 }
487
489 vpHomogeneousMatrix fMit[8];
490 get_fMi(fMit);
491
492 // vpDisplay::displayFrame(I,getExternalCameraPosition
493 // ()*fMi[6],cameraParam,0.2,vpColor::none);
494
495 vpImagePoint iP, iP_1;
496 vpPoint pt(0, 0, 0);
497
500 pt.track(getExternalCameraPosition() * fMit[0]);
503 for (int k = 1; k < 7; k++) {
504 pt.track(getExternalCameraPosition() * fMit[k - 1]);
506
507 pt.track(getExternalCameraPosition() * fMit[k]);
509
511 }
513 thickness_);
514 }
515
517
519 tcur_1 = tcur;
520 } else {
522 }
524 stop = robotStop;
526 }
527}
528
542{
543 // vpColVector q = get_artCoord();
544 vpColVector q(6); //; = get_artCoord();
545 q = get_artCoord();
546
547 vpHomogeneousMatrix fMit[8];
548
549 double q1 = q[0];
550 double q2 = q[1];
551 double q3 = q[2];
552 double q4 = q[3];
553 double q5 = q[4];
554 double q6 = q[5];
555
556 double c1 = cos(q1);
557 double s1 = sin(q1);
558 double c2 = cos(q2);
559 double s2 = sin(q2);
560 double c23 = cos(q2 + q3);
561 double s23 = sin(q2 + q3);
562 double c4 = cos(q4);
563 double s4 = sin(q4);
564 double c5 = cos(q5);
565 double s5 = sin(q5);
566 double c6 = cos(q6);
567 double s6 = sin(q6);
568
569 fMit[0][0][0] = c1;
570 fMit[0][1][0] = s1;
571 fMit[0][2][0] = 0;
572 fMit[0][0][1] = 0;
573 fMit[0][1][1] = 0;
574 fMit[0][2][1] = -1;
575 fMit[0][0][2] = -s1;
576 fMit[0][1][2] = c1;
577 fMit[0][2][2] = 0;
578 fMit[0][0][3] = a1 * c1;
579 fMit[0][1][3] = a1 * s1;
580 fMit[0][2][3] = d1;
581
582 fMit[1][0][0] = c1 * c2;
583 fMit[1][1][0] = s1 * c2;
584 fMit[1][2][0] = -s2;
585 fMit[1][0][1] = -c1 * s2;
586 fMit[1][1][1] = -s1 * s2;
587 fMit[1][2][1] = -c2;
588 fMit[1][0][2] = -s1;
589 fMit[1][1][2] = c1;
590 fMit[1][2][2] = 0;
591 fMit[1][0][3] = c1 * (a2 * c2 + a1);
592 fMit[1][1][3] = s1 * (a2 * c2 + a1);
593 fMit[1][2][3] = d1 - a2 * s2;
594
595 double quickcomp1 = a3 * c23 - a2 * c2 - a1;
596
597 fMit[2][0][0] = -c1 * c23;
598 fMit[2][1][0] = -s1 * c23;
599 fMit[2][2][0] = s23;
600 fMit[2][0][1] = s1;
601 fMit[2][1][1] = -c1;
602 fMit[2][2][1] = 0;
603 fMit[2][0][2] = c1 * s23;
604 fMit[2][1][2] = s1 * s23;
605 fMit[2][2][2] = c23;
606 fMit[2][0][3] = -c1 * quickcomp1;
607 fMit[2][1][3] = -s1 * quickcomp1;
608 fMit[2][2][3] = a3 * s23 - a2 * s2 + d1;
609
610 double quickcomp2 = c1 * (s23 * d4 - quickcomp1);
611 double quickcomp3 = s1 * (s23 * d4 - quickcomp1);
612
613 fMit[3][0][0] = -c1 * c23 * c4 + s1 * s4;
614 fMit[3][1][0] = -s1 * c23 * c4 - c1 * s4;
615 fMit[3][2][0] = s23 * c4;
616 fMit[3][0][1] = c1 * s23;
617 fMit[3][1][1] = s1 * s23;
618 fMit[3][2][1] = c23;
619 fMit[3][0][2] = -c1 * c23 * s4 - s1 * c4;
620 fMit[3][1][2] = -s1 * c23 * s4 + c1 * c4;
621 fMit[3][2][2] = s23 * s4;
622 fMit[3][0][3] = quickcomp2;
623 fMit[3][1][3] = quickcomp3;
624 fMit[3][2][3] = c23 * d4 + a3 * s23 - a2 * s2 + d1;
625
626 fMit[4][0][0] = c1 * (s23 * s5 - c5 * c23 * c4) + s1 * c5 * s4;
627 fMit[4][1][0] = s1 * (s23 * s5 - c5 * c23 * c4) - c1 * c5 * s4;
628 fMit[4][2][0] = s23 * c4 * c5 + c23 * s5;
629 fMit[4][0][1] = c1 * c23 * s4 + s1 * c4;
630 fMit[4][1][1] = s1 * c23 * s4 - c1 * c4;
631 fMit[4][2][1] = -s23 * s4;
632 fMit[4][0][2] = c1 * (s23 * c5 + s5 * c23 * c4) - s1 * s5 * s4;
633 fMit[4][1][2] = s1 * (s23 * c5 + s5 * c23 * c4) + c1 * s5 * s4;
634 fMit[4][2][2] = -s23 * c4 * s5 + c23 * c5;
635 fMit[4][0][3] = quickcomp2;
636 fMit[4][1][3] = quickcomp3;
637 fMit[4][2][3] = c23 * d4 + a3 * s23 - a2 * s2 + d1;
638
639 fMit[5][0][0] = c1 * (c23 * (c4 * c5 * c6 - s4 * s6) - s23 * s5 * c6) - s1 * (s4 * c5 * c6 + c4 * s6);
640 fMit[5][1][0] = -s1 * (c23 * (-c4 * c5 * c6 + s4 * s6) + s23 * s5 * c6) + c1 * (s4 * c5 * c6 + c4 * s6);
641 fMit[5][2][0] = s23 * (s4 * s6 - c4 * c5 * c6) - c23 * s5 * c6;
642 fMit[5][0][1] = -c1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) + s1 * (s4 * c5 * s6 - c4 * c6);
643 fMit[5][1][1] = -s1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) - c1 * (s4 * c5 * s6 - c4 * c6);
644 fMit[5][2][1] = s23 * (c4 * c5 * s6 + s4 * c6) + c23 * s5 * s6;
645 fMit[5][0][2] = c1 * (c23 * c4 * s5 + s23 * c5) - s1 * s4 * s5;
646 fMit[5][1][2] = s1 * (c23 * c4 * s5 + s23 * c5) + c1 * s4 * s5;
647 fMit[5][2][2] = -s23 * c4 * s5 + c23 * c5;
648 fMit[5][0][3] = quickcomp2;
649 fMit[5][1][3] = quickcomp3;
650 fMit[5][2][3] = s23 * a3 + c23 * d4 - a2 * s2 + d1;
651
652 fMit[6][0][0] = c1 * (c23 * (c4 * c5 * c6 - s4 * s6) - s23 * s5 * c6) - s1 * (s4 * c5 * c6 + c4 * s6);
653 fMit[6][1][0] = -s1 * (c23 * (-c4 * c5 * c6 + s4 * s6) + s23 * s5 * c6) + c1 * (s4 * c5 * c6 + c4 * s6);
654 fMit[6][2][0] = s23 * (s4 * s6 - c4 * c5 * c6) - c23 * s5 * c6;
655 fMit[6][0][1] = -c1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) + s1 * (s4 * c5 * s6 - c4 * c6);
656 fMit[6][1][1] = -s1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) - c1 * (s4 * c5 * s6 - c4 * c6);
657 fMit[6][2][1] = s23 * (c4 * c5 * s6 + s4 * c6) + c23 * s5 * s6;
658 fMit[6][0][2] = c1 * (c23 * c4 * s5 + s23 * c5) - s1 * s4 * s5;
659 fMit[6][1][2] = s1 * (c23 * c4 * s5 + s23 * c5) + c1 * s4 * s5;
660 fMit[6][2][2] = -s23 * c4 * s5 + c23 * c5;
661 fMit[6][0][3] = c1 * (c23 * (c4 * s5 * d6 - a3) + s23 * (c5 * d6 + d4) + a1 + a2 * c2) - s1 * s4 * s5 * d6;
662 fMit[6][1][3] = s1 * (c23 * (c4 * s5 * d6 - a3) + s23 * (c5 * d6 + d4) + a1 + a2 * c2) + c1 * s4 * s5 * d6;
663 fMit[6][2][3] = s23 * (a3 - c4 * s5 * d6) + c23 * (c5 * d6 + d4) - a2 * s2 + d1;
664
665 // vpHomogeneousMatrix cMe;
666 // get_cMe(cMe);
667 // cMe = cMe.inverse();
668 // fMit[7] = fMit[6] * cMe;
670 vpViper::get_fMc(q, fMit[7]);
672
674 for (int i = 0; i < 8; i++) {
675 fMi[i] = fMit[i];
676 }
678}
679
686{
687 switch (newState) {
688 case vpRobot::STATE_STOP: {
689 // Start primitive STOP only if the current state is Velocity
691 stopMotion();
692 }
693 break;
694 }
697 std::cout << "Change the control mode from velocity to position control.\n";
698 stopMotion();
699 } else {
700 // std::cout << "Change the control mode from stop to position
701 // control.\n";
702 }
703 break;
704 }
707 std::cout << "Change the control mode from stop to velocity control.\n";
708 }
709 break;
710 }
712 default:
713 break;
714 }
715
716 return vpRobot::setRobotState(newState);
717}
718
793{
795 vpERROR_TRACE("Cannot send a velocity to the robot "
796 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
798 "Cannot send a velocity to the robot "
799 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
800 }
801
802 vpColVector vel_sat(6);
803
804 double scale_sat = 1;
805 double vel_trans_max = getMaxTranslationVelocity();
806 double vel_rot_max = getMaxRotationVelocity();
807
808 double vel_abs; // Absolute value
809
810 // Velocity saturation
811 switch (frame) {
812 // saturation in cartesian space
815 if (vel.getRows() != 6) {
816 vpERROR_TRACE("The velocity vector must have a size of 6 !!!!");
817 throw;
818 }
819
820 for (unsigned int i = 0; i < 3; ++i) {
821 vel_abs = fabs(vel[i]);
822 if (vel_abs > vel_trans_max && !jointLimit) {
823 vel_trans_max = vel_abs;
824 vpERROR_TRACE("Excess velocity %g m/s in TRANSLATION "
825 "(axis nr. %d).",
826 vel[i], i + 1);
827 }
828
829 vel_abs = fabs(vel[i + 3]);
830 if (vel_abs > vel_rot_max && !jointLimit) {
831 vel_rot_max = vel_abs;
832 vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
833 "(axis nr. %d).",
834 vel[i + 3], i + 4);
835 }
836 }
837
838 double scale_trans_sat = 1;
839 double scale_rot_sat = 1;
840 if (vel_trans_max > getMaxTranslationVelocity())
841 scale_trans_sat = getMaxTranslationVelocity() / vel_trans_max;
842
843 if (vel_rot_max > getMaxRotationVelocity())
844 scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
845
846 if ((scale_trans_sat < 1) || (scale_rot_sat < 1)) {
847 if (scale_trans_sat < scale_rot_sat)
848 scale_sat = scale_trans_sat;
849 else
850 scale_sat = scale_rot_sat;
851 }
852 break;
853 }
854
855 // saturation in joint space
857 if (vel.getRows() != 6) {
858 vpERROR_TRACE("The velocity vector must have a size of 6 !!!!");
859 throw;
860 }
861 for (unsigned int i = 0; i < 6; ++i) {
862 vel_abs = fabs(vel[i]);
863 if (vel_abs > vel_rot_max && !jointLimit) {
864 vel_rot_max = vel_abs;
865 vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
866 "(axis nr. %d).",
867 vel[i], i + 1);
868 }
869 }
870 double scale_rot_sat = 1;
871 if (vel_rot_max > getMaxRotationVelocity())
872 scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
873 if (scale_rot_sat < 1)
874 scale_sat = scale_rot_sat;
875 break;
876 }
878 case vpRobot::MIXT_FRAME: {
879 break;
880 }
881 }
882
883 set_velocity(vel * scale_sat);
884
886 setRobotFrame(frame);
888
890 setVelocityCalled = true;
892}
893
898{
900
902 frame = getRobotFrame();
904
905 double vel_rot_max = getMaxRotationVelocity();
906
907 vpColVector articularCoordinates = get_artCoord();
908 vpColVector velocityframe = get_velocity();
909 vpColVector articularVelocity;
910
911 switch (frame) {
913 vpMatrix eJe_;
915 vpViper850::get_eJe(articularCoordinates, eJe_);
916 eJe_ = eJe_.pseudoInverse();
918 singularityTest(articularCoordinates, eJe_);
919 articularVelocity = eJe_ * eVc * velocityframe;
920 set_artVel(articularVelocity);
921 break;
922 }
924 vpMatrix fJe_;
925 vpViper850::get_fJe(articularCoordinates, fJe_);
926 fJe_ = fJe_.pseudoInverse();
928 singularityTest(articularCoordinates, fJe_);
929 articularVelocity = fJe_ * velocityframe;
930 set_artVel(articularVelocity);
931 break;
932 }
934 articularVelocity = velocityframe;
935 set_artVel(articularVelocity);
936 break;
937 }
939 case vpRobot::MIXT_FRAME: {
940 break;
941 }
942 }
943
944 switch (frame) {
947 for (unsigned int i = 0; i < 6; ++i) {
948 double vel_abs = fabs(articularVelocity[i]);
949 if (vel_abs > vel_rot_max && !jointLimit) {
950 vel_rot_max = vel_abs;
951 vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
952 "(axis nr. %d).",
953 articularVelocity[i], i + 1);
954 }
955 }
956 double scale_rot_sat = 1;
957 double scale_sat = 1;
958
959 if (vel_rot_max > getMaxRotationVelocity())
960 scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
961 if (scale_rot_sat < 1)
962 scale_sat = scale_rot_sat;
963
964 set_artVel(articularVelocity * scale_sat);
965 break;
966 }
969 case vpRobot::MIXT_FRAME: {
970 break;
971 }
972 }
973}
974
1021{
1022 vel.resize(6);
1023
1024 vpColVector articularCoordinates = get_artCoord();
1025 vpColVector articularVelocity = get_artVel();
1026
1027 switch (frame) {
1028 case vpRobot::CAMERA_FRAME: {
1029 vpMatrix eJe_;
1031 vpViper850::get_eJe(articularCoordinates, eJe_);
1032 vel = cVe * eJe_ * articularVelocity;
1033 break;
1034 }
1036 vel = articularVelocity;
1037 break;
1038 }
1040 vpMatrix fJe_;
1041 vpViper850::get_fJe(articularCoordinates, fJe_);
1042 vel = fJe_ * articularVelocity;
1043 break;
1044 }
1046 case vpRobot::MIXT_FRAME: {
1047 break;
1048 }
1049 default: {
1050 vpERROR_TRACE("Error in spec of vpRobot. "
1051 "Case not taken in account.");
1052 return;
1053 }
1054 }
1055}
1056
1074{
1075 timestamp = vpTime::measureTimeSecond();
1076 getVelocity(frame, vel);
1077}
1078
1121{
1122 vpColVector vel(6);
1123 getVelocity(frame, vel);
1124
1125 return vel;
1126}
1127
1141{
1142 timestamp = vpTime::measureTimeSecond();
1143 vpColVector vel(6);
1144 getVelocity(frame, vel);
1145
1146 return vel;
1147}
1148
1150{
1151 double vel_rot_max = getMaxRotationVelocity();
1152 double velmax = fabs(q[0]);
1153 for (unsigned int i = 1; i < 6; i++) {
1154 if (velmax < fabs(q[i]))
1155 velmax = fabs(q[i]);
1156 }
1157
1158 double alpha = (getPositioningVelocity() * vel_rot_max) / (velmax * 100);
1159 q = q * alpha;
1160}
1161
1237{
1239 vpERROR_TRACE("Robot was not in position-based control\n"
1240 "Modification of the robot state");
1241 // setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
1242 }
1243
1244 vpColVector articularCoordinates = get_artCoord();
1245
1246 vpColVector error(6);
1247 double errsqr = 0;
1248 switch (frame) {
1249 case vpRobot::CAMERA_FRAME: {
1250 unsigned int nbSol;
1251 vpColVector qdes(6);
1252
1254 vpRxyzVector rxyz;
1255 for (unsigned int i = 0; i < 3; i++) {
1256 txyz[i] = q[i];
1257 rxyz[i] = q[i + 3];
1258 }
1259
1260 vpRotationMatrix cRc2(rxyz);
1261 vpHomogeneousMatrix cMc2(txyz, cRc2);
1262
1264 vpViper850::get_fMc(articularCoordinates, fMc_);
1265
1266 vpHomogeneousMatrix fMc2 = fMc_ * cMc2;
1267
1268 do {
1269 articularCoordinates = get_artCoord();
1270 qdes = articularCoordinates;
1271 nbSol = getInverseKinematics(fMc2, qdes, verbose_);
1273 setVelocityCalled = true;
1275 if (nbSol > 0) {
1276 error = qdes - articularCoordinates;
1277 errsqr = error.sumSquare();
1278 // findHighestPositioningSpeed(error);
1279 set_artVel(error);
1280 if (errsqr < 1e-4) {
1281 set_artCoord(qdes);
1282 error = 0;
1283 set_artVel(error);
1284 set_velocity(error);
1285 break;
1286 }
1287 } else {
1288 vpERROR_TRACE("Positioning error.");
1289 throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
1290 }
1291 } while (errsqr > 1e-8 && nbSol > 0);
1292
1293 break;
1294 }
1295
1297 do {
1298 articularCoordinates = get_artCoord();
1299 error = q - articularCoordinates;
1300 errsqr = error.sumSquare();
1301 // findHighestPositioningSpeed(error);
1302 set_artVel(error);
1304 setVelocityCalled = true;
1306 if (errsqr < 1e-4) {
1307 set_artCoord(q);
1308 error = 0;
1309 set_artVel(error);
1310 set_velocity(error);
1311 break;
1312 }
1313 } while (errsqr > 1e-8);
1314 break;
1315 }
1316
1318 unsigned int nbSol;
1319 vpColVector qdes(6);
1320
1322 vpRxyzVector rxyz;
1323 for (unsigned int i = 0; i < 3; i++) {
1324 txyz[i] = q[i];
1325 rxyz[i] = q[i + 3];
1326 }
1327
1328 vpRotationMatrix fRc(rxyz);
1329 vpHomogeneousMatrix fMc_(txyz, fRc);
1330
1331 do {
1332 articularCoordinates = get_artCoord();
1333 qdes = articularCoordinates;
1334 nbSol = getInverseKinematics(fMc_, qdes, verbose_);
1335 if (nbSol > 0) {
1336 error = qdes - articularCoordinates;
1337 errsqr = error.sumSquare();
1338 // findHighestPositioningSpeed(error);
1339 set_artVel(error);
1341 setVelocityCalled = true;
1343 if (errsqr < 1e-4) {
1344 set_artCoord(qdes);
1345 error = 0;
1346 set_artVel(error);
1347 set_velocity(error);
1348 break;
1349 }
1350 } else
1351 vpERROR_TRACE("Positioning error. Position unreachable");
1352 } while (errsqr > 1e-8 && nbSol > 0);
1353 break;
1354 }
1355 case vpRobot::MIXT_FRAME: {
1356 throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1357 "Mixt frame not implemented.");
1358 }
1360 throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1361 "End-effector frame not implemented.");
1362 }
1363 }
1364}
1365
1428void vpSimulatorViper850::setPosition(const vpRobot::vpControlFrameType frame, double pos1, double pos2, double pos3,
1429 double pos4, double pos5, double pos6)
1430{
1431 try {
1432 vpColVector position(6);
1433 position[0] = pos1;
1434 position[1] = pos2;
1435 position[2] = pos3;
1436 position[3] = pos4;
1437 position[4] = pos5;
1438 position[5] = pos6;
1439
1440 setPosition(frame, position);
1441 } catch (...) {
1442 vpERROR_TRACE("Error caught");
1443 throw;
1444 }
1445}
1446
1481void vpSimulatorViper850::setPosition(const char *filename)
1482{
1483 vpColVector q;
1484 bool ret;
1485
1486 ret = this->readPosFile(filename, q);
1487
1488 if (ret == false) {
1489 vpERROR_TRACE("Bad position in \"%s\"", filename);
1490 throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
1491 }
1494}
1495
1556{
1557 q.resize(6);
1558
1559 switch (frame) {
1560 case vpRobot::CAMERA_FRAME: {
1561 q = 0;
1562 break;
1563 }
1564
1566 q = get_artCoord();
1567 break;
1568 }
1569
1573
1574 vpRotationMatrix fRc;
1575 fMc_.extract(fRc);
1576 vpRxyzVector rxyz(fRc);
1577
1579 fMc_.extract(txyz);
1580
1581 for (unsigned int i = 0; i < 3; i++) {
1582 q[i] = txyz[i];
1583 q[i + 3] = rxyz[i];
1584 }
1585 break;
1586 }
1587
1588 case vpRobot::MIXT_FRAME: {
1589 vpERROR_TRACE("Positioning error. Mixt frame not implemented");
1590 throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1591 "Mixt frame not implemented.");
1592 }
1594 vpERROR_TRACE("Positioning error. Mixt frame not implemented");
1595 throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1596 "End-effector frame not implemented.");
1597 }
1598 }
1599}
1600
1628{
1629 timestamp = vpTime::measureTimeSecond();
1630 getPosition(frame, q);
1631}
1632
1645{
1646 vpColVector posRxyz;
1647 // recupere position en Rxyz
1648 this->getPosition(frame, posRxyz);
1649
1650 // recupere le vecteur thetaU correspondant
1651 vpThetaUVector RtuVect(vpRxyzVector(posRxyz[3], posRxyz[4], posRxyz[5]));
1652
1653 // remplit le vpPoseVector avec translation et rotation ThetaU
1654 for (unsigned int j = 0; j < 3; j++) {
1655 position[j] = posRxyz[j];
1656 position[j + 3] = RtuVect[j];
1657 }
1658}
1659
1672 double &timestamp)
1673{
1674 timestamp = vpTime::measureTimeSecond();
1675 getPosition(frame, position);
1676}
1677
1686void vpSimulatorViper850::setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
1687{
1688 if (limitMin.getRows() != 6 || limitMax.getRows() != 6) {
1689 vpTRACE("Joint limit vector has not a size of 6 !");
1690 return;
1691 }
1692
1693 joint_min[0] = limitMin[0];
1694 joint_min[1] = limitMin[1];
1695 joint_min[2] = limitMin[2];
1696 joint_min[3] = limitMin[3];
1697 joint_min[4] = limitMin[4];
1698 joint_min[5] = limitMin[5];
1699
1700 joint_max[0] = limitMax[0];
1701 joint_max[1] = limitMax[1];
1702 joint_max[2] = limitMax[2];
1703 joint_max[3] = limitMax[3];
1704 joint_max[4] = limitMax[4];
1705 joint_max[5] = limitMax[5];
1706}
1707
1714{
1715 double q2 = q[1];
1716 double q3 = q[2];
1717 double q5 = q[4];
1718
1719 double c2 = cos(q2);
1720 double c3 = cos(q3);
1721 double s3 = sin(q3);
1722 double c23 = cos(q2 + q3);
1723 double s23 = sin(q2 + q3);
1724 double s5 = sin(q5);
1725
1726 bool cond1 = fabs(s5) < 1e-1;
1727 bool cond2 = fabs(a3 * s3 + c3 * d4) < 1e-1;
1728 bool cond3 = fabs(a2 * c2 - a3 * c23 + s23 * d4 + a1) < 1e-1;
1729
1730 if (cond1) {
1731 J[3][0] = 0;
1732 J[5][0] = 0;
1733 J[3][1] = 0;
1734 J[5][1] = 0;
1735 J[3][2] = 0;
1736 J[5][2] = 0;
1737 J[3][3] = 0;
1738 J[5][3] = 0;
1739 J[3][4] = 0;
1740 J[5][4] = 0;
1741 J[3][5] = 0;
1742 J[5][5] = 0;
1743 return true;
1744 }
1745
1746 if (cond2) {
1747 J[1][0] = 0;
1748 J[2][0] = 0;
1749 J[3][0] = 0;
1750 J[4][0] = 0;
1751 J[5][0] = 0;
1752 J[1][1] = 0;
1753 J[2][1] = 0;
1754 J[3][1] = 0;
1755 J[4][1] = 0;
1756 J[5][1] = 0;
1757 J[1][2] = 0;
1758 J[2][2] = 0;
1759 J[3][2] = 0;
1760 J[4][2] = 0;
1761 J[5][2] = 0;
1762 return true;
1763 }
1764
1765 if (cond3) {
1766 J[0][0] = 0;
1767 J[3][0] = 0;
1768 J[4][0] = 0;
1769 J[5][0] = 0;
1770 J[0][1] = 0;
1771 J[3][1] = 0;
1772 J[4][1] = 0;
1773 J[5][1] = 0;
1774 }
1775
1776 return false;
1777}
1778
1783{
1784 int artNumb = 0;
1785 double diff = 0;
1786 double difft = 0;
1787
1788 vpColVector articularCoordinates = get_artCoord();
1789
1790 for (unsigned int i = 0; i < 6; i++) {
1791 if (articularCoordinates[i] <= joint_min[i]) {
1792 difft = joint_min[i] - articularCoordinates[i];
1793 if (difft > diff) {
1794 diff = difft;
1795 artNumb = -(int)i - 1;
1796 }
1797 }
1798 }
1799
1800 for (unsigned int i = 0; i < 6; i++) {
1801 if (articularCoordinates[i] >= joint_max[i]) {
1802 difft = articularCoordinates[i] - joint_max[i];
1803 if (difft > diff) {
1804 diff = difft;
1805 artNumb = (int)(i + 1);
1806 }
1807 }
1808 }
1809
1810 if (artNumb != 0)
1811 std::cout << "\nWarning: Velocity control stopped: axis " << fabs((float)artNumb) << " on joint limit!"
1812 << std::endl;
1813
1814 return artNumb;
1815}
1816
1835{
1836 displacement.resize(6);
1837 displacement = 0;
1838 vpColVector q_cur(6);
1839
1840 q_cur = get_artCoord();
1841
1842 if (!first_time_getdis) {
1843 switch (frame) {
1844 case vpRobot::CAMERA_FRAME: {
1845 std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
1846 return;
1847 }
1848
1850 displacement = q_cur - q_prev_getdis;
1851 break;
1852 }
1853
1855 std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
1856 return;
1857 }
1858
1859 case vpRobot::MIXT_FRAME: {
1860 std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
1861 return;
1862 }
1864 std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
1865 return;
1866 }
1867 }
1868 } else {
1869 first_time_getdis = false;
1870 }
1871
1872 // Memorize the joint position for the next call
1873 q_prev_getdis = q_cur;
1874}
1875
1937bool vpSimulatorViper850::readPosFile(const std::string &filename, vpColVector &q)
1938{
1939 std::ifstream fd(filename.c_str(), std::ios::in);
1940
1941 if (!fd.is_open()) {
1942 return false;
1943 }
1944
1945 std::string line;
1946 std::string key("R:");
1947 std::string id("#Viper850 - Position");
1948 bool pos_found = false;
1949 int lineNum = 0;
1950
1951 q.resize(njoint);
1952
1953 while (std::getline(fd, line)) {
1954 lineNum++;
1955 if (lineNum == 1) {
1956 if (!(line.compare(0, id.size(), id) == 0)) { // check if Viper850 position file
1957 std::cout << "Error: this position file " << filename << " is not for Viper850 robot" << std::endl;
1958 return false;
1959 }
1960 }
1961 if ((line.compare(0, 1, "#") == 0)) { // skip comment
1962 continue;
1963 }
1964 if ((line.compare(0, key.size(), key) == 0)) { // decode position
1965 // check if there are at least njoint values in the line
1966 std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
1967 if (chain.size() < njoint + 1) // try to split with tab separator
1968 chain = vpIoTools::splitChain(line, std::string("\t"));
1969 if (chain.size() < njoint + 1)
1970 continue;
1971
1972 std::istringstream ss(line);
1973 std::string key_;
1974 ss >> key_;
1975 for (unsigned int i = 0; i < njoint; i++)
1976 ss >> q[i];
1977 pos_found = true;
1978 break;
1979 }
1980 }
1981
1982 // converts rotations from degrees into radians
1983 q.deg2rad();
1984
1985 fd.close();
1986
1987 if (!pos_found) {
1988 std::cout << "Error: unable to find a position for Viper850 robot in " << filename << std::endl;
1989 return false;
1990 }
1991
1992 return true;
1993}
1994
2016bool vpSimulatorViper850::savePosFile(const std::string &filename, const vpColVector &q)
2017{
2018
2019 FILE *fd;
2020 fd = fopen(filename.c_str(), "w");
2021 if (fd == NULL)
2022 return false;
2023
2024 fprintf(fd, "\
2025#Viper - Position - Version 1.0\n\
2026#\n\
2027# R: A B C D E F\n\
2028# Joint position in degrees\n\
2029#\n\
2030#\n\n");
2031
2032 // Save positions in mm and deg
2033 fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n", vpMath::deg(q[0]), vpMath::deg(q[1]), vpMath::deg(q[2]),
2034 vpMath::deg(q[3]), vpMath::deg(q[4]), vpMath::deg(q[5]));
2035
2036 fclose(fd);
2037 return (true);
2038}
2039
2047void vpSimulatorViper850::move(const char *filename)
2048{
2049 vpColVector q;
2050
2051 try {
2052 this->readPosFile(filename, q);
2055 } catch (...) {
2056 throw;
2057 }
2058}
2059
2070
2079{
2082
2083 cVe.buildFrom(cMe);
2084}
2085
2096{
2097 try {
2099 } catch (...) {
2100 vpERROR_TRACE("catch exception ");
2101 throw;
2102 }
2103}
2104
2116{
2117 try {
2118 vpColVector articularCoordinates = get_artCoord();
2119 vpViper850::get_fJe(articularCoordinates, fJe_);
2120 } catch (...) {
2121 vpERROR_TRACE("Error caught");
2122 throw;
2123 }
2124}
2125
2130{
2132 return;
2133
2134 vpColVector stop(6);
2135 stop = 0;
2136 set_artVel(stop);
2137 set_velocity(stop);
2139}
2140
2141/**********************************************************************************/
2142/**********************************************************************************/
2143/**********************************************************************************/
2144/**********************************************************************************/
2145
2155{
2156 // set scene_dir from #define VISP_SCENE_DIR if it exists
2157 // VISP_SCENES_DIR may contain multiple locations separated by ";"
2158 std::string scene_dir_;
2159 std::vector<std::string> scene_dirs = vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(";"));
2160 bool sceneDirExists = false;
2161 for (size_t i = 0; i < scene_dirs.size(); i++)
2162 if (vpIoTools::checkDirectory(scene_dirs[i]) == true) { // directory exists
2163 scene_dir_ = scene_dirs[i];
2164 sceneDirExists = true;
2165 break;
2166 }
2167 if (!sceneDirExists) {
2168 try {
2169 scene_dir_ = vpIoTools::getenv("VISP_SCENES_DIR");
2170 std::cout << "The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
2171 } catch (...) {
2172 std::cout << "Cannot get VISP_SCENES_DIR environment variable" << std::endl;
2173 }
2174 }
2175
2176 unsigned int name_length = 30; // the size of this kind of string "/viper850_arm2.bnd"
2177 if (scene_dir_.size() > FILENAME_MAX)
2178 throw vpException(vpException::dimensionError, "Cannot initialize Viper850 simulator");
2179
2180 unsigned int full_length = (unsigned int)scene_dir_.size() + name_length;
2181 if (full_length > FILENAME_MAX)
2182 throw vpException(vpException::dimensionError, "Cannot initialize Viper850 simulator");
2183
2184 char *name_cam = new char[full_length];
2185
2186 strcpy(name_cam, scene_dir_.c_str());
2187 strcat(name_cam, "/camera.bnd");
2188 set_scene(name_cam, &camera, cameraFactor);
2189
2190 if (arm_dir.size() > FILENAME_MAX)
2191 throw vpException(vpException::dimensionError, "Cannot initialize Viper850 simulator");
2192 full_length = (unsigned int)arm_dir.size() + name_length;
2193 if (full_length > FILENAME_MAX)
2194 throw vpException(vpException::dimensionError, "Cannot initialize Viper850 simulator");
2195
2196 char *name_arm = new char[full_length];
2197 strcpy(name_arm, arm_dir.c_str());
2198 strcat(name_arm, "/viper850_arm1.bnd");
2199 set_scene(name_arm, robotArms, 1.0);
2200 strcpy(name_arm, arm_dir.c_str());
2201 strcat(name_arm, "/viper850_arm2.bnd");
2202 set_scene(name_arm, robotArms + 1, 1.0);
2203 strcpy(name_arm, arm_dir.c_str());
2204 strcat(name_arm, "/viper850_arm3.bnd");
2205 set_scene(name_arm, robotArms + 2, 1.0);
2206 strcpy(name_arm, arm_dir.c_str());
2207 strcat(name_arm, "/viper850_arm4.bnd");
2208 set_scene(name_arm, robotArms + 3, 1.0);
2209 strcpy(name_arm, arm_dir.c_str());
2210 strcat(name_arm, "/viper850_arm5.bnd");
2211 set_scene(name_arm, robotArms + 4, 1.0);
2212 strcpy(name_arm, arm_dir.c_str());
2213 strcat(name_arm, "/viper850_arm6.bnd");
2214 set_scene(name_arm, robotArms + 5, 1.0);
2215
2216 // set_scene("./arm2.bnd", robotArms+1, 1.0);
2217 // set_scene("./arm3.bnd", robotArms+2, 1.0);
2218 // set_scene("./arm4.bnd", robotArms+3, 1.0);
2219 // set_scene("./arm5.bnd", robotArms+4, 1.0);
2220 // set_scene("./arm6.bnd", robotArms+5, 1.0);
2221
2222 add_rfstack(IS_BACK);
2223
2224 add_vwstack("start", "depth", 0.0, 100.0);
2225 add_vwstack("start", "window", -0.1, 0.1, -0.1, 0.1);
2226 add_vwstack("start", "type", PERSPECTIVE);
2227 //
2228 // sceneInitialized = true;
2229 // displayObject = true;
2230 displayCamera = true;
2231
2232 delete[] name_cam;
2233 delete[] name_arm;
2234}
2235
2237{
2239 bool changed = false;
2240 vpHomogeneousMatrix displacement = navigation(I_, changed);
2241
2242 // if (displacement[2][3] != 0)
2243 if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
2244 camMf2 = camMf2 * displacement;
2245
2246 f2Mf = camMf2.inverse() * camMf;
2247
2248 camMf = camMf2 * displacement * f2Mf;
2249
2250 double u;
2251 double v;
2252 // if(px_ext != 1 && py_ext != 1)
2253 // we assume px_ext and py_ext > 0
2254 if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
2255 (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon())) {
2256 u = (double)I_.getWidth() / (2 * px_ext);
2257 v = (double)I_.getHeight() / (2 * py_ext);
2258 } else {
2259 u = (double)I_.getWidth() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
2260 v = (double)I_.getHeight() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
2261 }
2262
2263 float w44o[4][4], w44cext[4][4], x, y, z;
2264
2265 vp2jlc_matrix(camMf.inverse(), w44cext);
2266
2267 add_vwstack("start", "cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
2268 x = w44cext[2][0] + w44cext[3][0];
2269 y = w44cext[2][1] + w44cext[3][1];
2270 z = w44cext[2][2] + w44cext[3][2];
2271 add_vwstack("start", "vrp", x, y, z);
2272 add_vwstack("start", "vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
2273 add_vwstack("start", "vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
2274 add_vwstack("start", "window", -u, u, -v, v);
2275
2276 vpHomogeneousMatrix fMit[8];
2277 get_fMi(fMit);
2278
2279 vp2jlc_matrix(vpHomogeneousMatrix(0, 0, 0, 0, 0, 0), w44o);
2280 display_scene(w44o, robotArms[0], I_, curColor);
2281
2282 vp2jlc_matrix(fMit[0], w44o);
2283 display_scene(w44o, robotArms[1], I_, curColor);
2284
2285 vp2jlc_matrix(fMit[1], w44o);
2286 display_scene(w44o, robotArms[2], I_, curColor);
2287
2288 vp2jlc_matrix(fMit[2], w44o);
2289 display_scene(w44o, robotArms[3], I_, curColor);
2290
2291 vp2jlc_matrix(fMit[3], w44o);
2292 display_scene(w44o, robotArms[4], I_, curColor);
2293
2294 vp2jlc_matrix(fMit[6], w44o);
2295 display_scene(w44o, robotArms[5], I_, curColor);
2296
2297 if (displayCamera) {
2299 get_cMe(cMe);
2300 cMe = cMe.inverse();
2301 cMe = fMit[6] * cMe;
2302 vp2jlc_matrix(cMe, w44o);
2303 display_scene(w44o, camera, I_, camColor);
2304 }
2305
2306 if (displayObject) {
2307 vp2jlc_matrix(fMo, w44o);
2308 display_scene(w44o, scene, I_, curColor);
2309 }
2311}
2312
2330{
2331 vpColVector stop(6);
2332 bool status = true;
2333 stop = 0;
2334 set_artVel(stop);
2335 set_velocity(stop);
2337 fMc_ = fMo * cMo_.inverse();
2338
2339 vpColVector articularCoordinates = get_artCoord();
2340 unsigned int nbSol = getInverseKinematics(fMc_, articularCoordinates, verbose_);
2341
2342 if (nbSol == 0) {
2343 status = false;
2344 vpERROR_TRACE("Positioning error. Position unreachable");
2345 }
2346
2347 if (verbose_)
2348 std::cout << "Used joint coordinates (rad): " << articularCoordinates.t() << std::endl;
2349
2350 set_artCoord(articularCoordinates);
2351
2352 compute_fMi();
2353
2354 return status;
2355}
2356
2371{
2372 vpColVector stop(6);
2373 stop = 0;
2375 set_artVel(stop);
2376 set_velocity(stop);
2377 vpHomogeneousMatrix fMit[8];
2378 get_fMi(fMit);
2379 fMo = fMit[7] * cMo_;
2381}
2382
2383#elif !defined(VISP_BUILD_SHARED_LIBS)
2384// Work around to avoid warning: libvisp_robot.a(vpSimulatorViper850.cpp.o)
2385// has no symbols
2386void dummy_vpSimulatorViper850(){};
2387#endif
unsigned int getRows() const
Definition vpArray2D.h:290
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
Implementation of column vector and the associated operations.
double sumSquare() const
vpColVector & deg2rad()
vpRowVector t() const
void resize(unsigned int i, bool flagNullify=true)
static const vpColor none
Definition vpColor.h:223
static const vpColor green
Definition vpColor.h:214
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 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), const std::string &frameName="", const vpColor &textColor=vpColor::black, const vpImagePoint &textOffset=vpImagePoint(15, 15))
static void flush(const vpImage< unsigned char > &I)
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 emitted by ViSP classes.
Definition vpException.h:59
@ dimensionError
Bad dimension.
Definition vpException.h:83
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 of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
unsigned int getHeight() const
Definition vpImage.h:184
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
static bool checkDirectory(const std::string &dirname)
static std::string getenv(const std::string &env)
static double rad(double deg)
Definition vpMath.h:116
static Type maximum(const Type &a, const Type &b)
Definition vpMath.h:172
static Type minimum(const Type &a, const Type &b)
Definition vpMath.h:180
static double deg(double rad)
Definition vpMath.h:106
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
vpMatrix pseudoInverse(double svThreshold=1e-6) const
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
void unlock()
Definition vpMutex.h:106
void lock()
Definition vpMutex.h:90
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition vpPoint.h:77
double get_y() const
Get the point y coordinate in the image plane.
Definition vpPoint.cpp:469
double get_x() const
Get the point x coordinate in the image plane.
Definition vpPoint.cpp:467
Implementation of a pose vector and operations on poses.
Error that can be emitted by the vpRobot class and its derivatives.
@ wrongStateError
Wrong robot state.
@ positionOutOfRangeError
Position is out of range.
@ lowLevelError
Error thrown by the low level sdk.
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:142
vpControlFrameType
Definition vpRobot.h:73
@ REFERENCE_FRAME
Definition vpRobot.h:74
@ ARTICULAR_FRAME
Definition vpRobot.h:76
@ MIXT_FRAME
Definition vpRobot.h:84
@ CAMERA_FRAME
Definition vpRobot.h:80
@ END_EFFECTOR_FRAME
Definition vpRobot.h:79
vpControlFrameType getRobotFrame(void) const
Definition vpRobot.h:170
vpRobotStateType
Definition vpRobot.h:62
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition vpRobot.h:65
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition vpRobot.h:64
@ STATE_ACCELERATION_CONTROL
Initialize the acceleration controller.
Definition vpRobot.h:66
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition vpRobot.h:63
double getMaxRotationVelocity(void) const
Definition vpRobot.cpp:270
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition vpRobot.cpp:198
double getMaxTranslationVelocity(void) const
Definition vpRobot.cpp:248
vpControlFrameType setRobotFrame(vpRobot::vpControlFrameType newFrame)
Definition vpRobot.cpp:204
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as Euler angle minimal representation.
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:101
static const char *const CONST_PTGREY_FLEA2_CAMERA_NAME
Definition vpViper850.h:120
void setToolType(vpViper850::vpToolType tool)
Set the current tool type.
Definition vpViper850.h:167
static const vpToolType defaultTool
Default tool attached to the robot end effector.
Definition vpViper850.h:134
vpCameraParameters::vpCameraParametersProjType projModel
Definition vpViper850.h:174
vpToolType getToolType() const
Get the current tool type.
Definition vpViper850.h:158
vpToolType
List of possible tools that can be attached to the robot end-effector.
Definition vpViper850.h:125
@ TOOL_SCHUNK_GRIPPER_CAMERA
Definition vpViper850.h:128
@ TOOL_PTGREY_FLEA2_CAMERA
Definition vpViper850.h:127
@ TOOL_MARLIN_F033C_CAMERA
Definition vpViper850.h:126
@ TOOL_GENERIC_CAMERA
Definition vpViper850.h:129
static const char *const CONST_MARLIN_F033C_CAMERA_NAME
Definition vpViper850.h:119
vpTranslationVector etc
Definition vpViper.h:156
double d6
for joint 6
Definition vpViper.h:164
double a3
for joint 3
Definition vpViper.h:162
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition vpViper.cpp:904
double d4
for joint 4
Definition vpViper.h:163
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition vpViper.cpp:1141
vpColVector joint_max
Definition vpViper.h:169
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false) const
Definition vpViper.cpp:555
vpHomogeneousMatrix eMc
End effector to camera transformation.
Definition vpViper.h:154
double a1
Definition vpViper.h:160
vpRxyzVector erc
Definition vpViper.h:157
vpColVector joint_min
Definition vpViper.h:170
double a2
for joint 2
Definition vpViper.h:161
static const unsigned int njoint
Number of joint.
Definition vpViper.h:151
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition vpViper.cpp:952
double d1
for joint 1
Definition vpViper.h:160
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition vpViper.cpp:592
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:411
#define vpERROR_TRACE
Definition vpDebug.h:388
VISP_EXPORT double measureTimeMs()
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double getMinTimeForUsleepCall()
VISP_EXPORT double measureTimeSecond()