Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpSimulatorAfma6.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 Afma6.
33 *
34*****************************************************************************/
35
36#include <visp3/core/vpConfig.h>
37#if defined(VISP_HAVE_MODULE_GUI) && ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD))
38#include <cmath> // std::fabs
39#include <limits> // numeric_limits
40#include <string>
41#include <visp3/core/vpImagePoint.h>
42#include <visp3/core/vpIoTools.h>
43#include <visp3/core/vpMeterPixelConversion.h>
44#include <visp3/core/vpPoint.h>
45#include <visp3/core/vpTime.h>
46#include <visp3/robot/vpRobotException.h>
47#include <visp3/robot/vpSimulatorAfma6.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(), vpAfma6(), 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 DWORD dwThreadIdArray;
70 hThread = CreateThread(NULL, // default security attributes
71 0, // use default stack size
72 launcher, // thread function name
73 this, // argument to thread function
74 0, // use default creation flags
75 &dwThreadIdArray); // returns the thread identifier
76#elif defined(VISP_HAVE_PTHREAD)
77 pthread_attr_init(&attr);
78 pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
79
80 pthread_create(&thread, NULL, launcher, (void *)this);
81#endif
82
84}
85
93 : vpRobotWireFrameSimulator(do_display), q_prev_getdis(), first_time_getdis(true),
94 positioningVelocity(defaultPositioningVelocity), zeroPos(), reposPos(), toolCustom(false), arm_dir()
95{
96 init();
98
100
101#if defined(_WIN32)
102 DWORD dwThreadIdArray;
103 hThread = CreateThread(NULL, // default security attributes
104 0, // use default stack size
105 launcher, // thread function name
106 this, // argument to thread function
107 0, // use default creation flags
108 &dwThreadIdArray); // returns the thread identifier
109#elif defined(VISP_HAVE_PTHREAD)
110 pthread_attr_init(&attr);
111 pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
112
113 pthread_create(&thread, NULL, launcher, (void *)this);
114#endif
115
116 compute_fMi();
117}
118
123{
125 robotStop = true;
127
128#if defined(_WIN32)
129#if defined(WINRT_8_1)
130 WaitForSingleObjectEx(hThread, INFINITE, FALSE);
131#else // pure win32
132 WaitForSingleObject(hThread, INFINITE);
133#endif
134 CloseHandle(hThread);
135#elif defined(VISP_HAVE_PTHREAD)
136 pthread_attr_destroy(&attr);
137 pthread_join(thread, NULL);
138#endif
139
140 if (robotArms != NULL) {
141 for (int i = 0; i < 6; i++)
142 free_Bound_scene(&(robotArms[i]));
143 }
144
145 delete[] robotArms;
146 delete[] fMi;
147}
148
158{
159 // set arm_dir from #define VISP_ROBOT_ARMS_DIR if it exists
160 // VISP_ROBOT_ARMS_DIR may contain multiple locations separated by ";"
161 std::vector<std::string> arm_dirs = vpIoTools::splitChain(std::string(VISP_ROBOT_ARMS_DIR), std::string(";"));
162 bool armDirExists = false;
163 for (size_t i = 0; i < arm_dirs.size(); i++)
164 if (vpIoTools::checkDirectory(arm_dirs[i]) == true) { // directory exists
165 arm_dir = arm_dirs[i];
166 armDirExists = true;
167 break;
168 }
169 if (!armDirExists) {
170 try {
171 arm_dir = vpIoTools::getenv("VISP_ROBOT_ARMS_DIR");
172 std::cout << "The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
173 } catch (...) {
174 std::cout << "Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
175 }
176 }
177
179 toolCustom = false;
180
181 size_fMi = 8;
182 fMi = new vpHomogeneousMatrix[8];
185
186 zeroPos.resize(njoint);
187 zeroPos = 0;
188 reposPos.resize(njoint);
189 reposPos = 0;
190 reposPos[1] = -M_PI / 2;
191 reposPos[2] = M_PI;
192 reposPos[4] = M_PI / 2;
193
194 artCoord = zeroPos;
195 artVel = 0;
196
197 q_prev_getdis.resize(njoint);
198 q_prev_getdis = 0;
199 first_time_getdis = true;
200
201 positioningVelocity = defaultPositioningVelocity;
202
205
206 // Software joint limits in radians
207 //_joint_min.resize(njoint);
208 _joint_min[0] = -0.6501;
209 _joint_min[1] = -0.6001;
210 _joint_min[2] = -0.5001;
211 _joint_min[3] = -2.7301;
212 _joint_min[4] = -0.1001;
213 _joint_min[5] = -1.5901;
214 //_joint_max.resize(njoint);
215 _joint_max[0] = 0.7001;
216 _joint_max[1] = 0.5201;
217 _joint_max[2] = 0.4601;
218 _joint_max[3] = 2.7301;
219 _joint_max[4] = 2.4801;
220 _joint_max[5] = 1.5901;
221}
222
227{
228 robotArms = NULL;
229 robotArms = new Bound_scene[6];
230 initArms();
232 vpHomogeneousMatrix(-0.1, 0, 4, vpMath::rad(90), 0, 0));
233 cameraParam.initPersProjWithoutDistortion(558.5309599, 556.055053, 320, 240);
236 getCameraParameters(tmp, 640, 480);
237 px_int = tmp.get_px();
238 py_int = tmp.get_py();
239 sceneInitialized = true;
240}
241
258{
259 this->projModel = proj_model;
260 unsigned int name_length = 30; // the size of this kind of string "/afma6_tool_vacuum.bnd"
261 if (arm_dir.size() > FILENAME_MAX)
262 throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
263 unsigned int full_length = (unsigned int)arm_dir.size() + name_length;
264 if (full_length > FILENAME_MAX)
265 throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
266
267 // Use here default values of the robot constant parameters.
268 switch (tool) {
269 case vpAfma6::TOOL_CCMOP: {
270 _erc[0] = vpMath::rad(164.35); // rx
271 _erc[1] = vpMath::rad(89.64); // ry
272 _erc[2] = vpMath::rad(-73.05); // rz
273 _etc[0] = 0.0117; // tx
274 _etc[1] = 0.0033; // ty
275 _etc[2] = 0.2272; // tz
276
277 setCameraParameters(vpCameraParameters(1109.5735473989, 1112.1520168160, 320, 240));
278
279 if (robotArms != NULL) {
280 while (get_displayBusy())
281 vpTime::wait(2);
282 free_Bound_scene(&(robotArms[5]));
283 char *name_arm = new char[full_length];
284 strcpy(name_arm, arm_dir.c_str());
285 strcat(name_arm, "/afma6_tool_ccmop.bnd");
286 set_scene(name_arm, robotArms + 5, 1.0);
287 set_displayBusy(false);
288 delete[] name_arm;
289 }
290 break;
291 }
293 _erc[0] = vpMath::rad(88.33); // rx
294 _erc[1] = vpMath::rad(72.07); // ry
295 _erc[2] = vpMath::rad(2.53); // rz
296 _etc[0] = 0.0783; // tx
297 _etc[1] = 0.1234; // ty
298 _etc[2] = 0.1638; // tz
299
300 setCameraParameters(vpCameraParameters(852.6583228197, 854.8084224761, 320, 240));
301
302 if (robotArms != NULL) {
303 while (get_displayBusy())
304 vpTime::wait(2);
305 free_Bound_scene(&(robotArms[5]));
306 char *name_arm = new char[full_length];
307 strcpy(name_arm, arm_dir.c_str());
308 strcat(name_arm, "/afma6_tool_gripper.bnd");
309 set_scene(name_arm, robotArms + 5, 1.0);
310 set_displayBusy(false);
311 delete[] name_arm;
312 }
313 break;
314 }
316 _erc[0] = vpMath::rad(90.40); // rx
317 _erc[1] = vpMath::rad(75.11); // ry
318 _erc[2] = vpMath::rad(0.18); // rz
319 _etc[0] = 0.0038; // tx
320 _etc[1] = 0.1281; // ty
321 _etc[2] = 0.1658; // tz
322
323 setCameraParameters(vpCameraParameters(853.4876600807, 856.0339170706, 320, 240));
324
325 if (robotArms != NULL) {
326 while (get_displayBusy())
327 vpTime::wait(2);
328 free_Bound_scene(&(robotArms[5]));
329
330 char *name_arm = new char[full_length];
331
332 strcpy(name_arm, arm_dir.c_str());
333 strcat(name_arm, "/afma6_tool_vacuum.bnd");
334 set_scene(name_arm, robotArms + 5, 1.0);
335 set_displayBusy(false);
336 delete[] name_arm;
337 }
338 break;
339 }
341 std::cout << "The custom tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
342 break;
343 }
345 std::cout << "The Intel D435 camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
346 break;
347 }
349 std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
350 break;
351 }
352 }
353
355
357 this->_eMc.buildFrom(_etc, eRc);
359
360 setToolType(tool);
361 return;
362}
363
374void vpSimulatorAfma6::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
375 const unsigned int &image_height)
376{
377 if (toolCustom) {
378 cam.initPersProjWithoutDistortion(px_int, py_int, image_width / 2, image_height / 2);
379 }
380 // Set default parameters
381 switch (getToolType()) {
382 case vpAfma6::TOOL_CCMOP: {
383 // Set default intrinsic camera parameters for 640x480 images
384 if (image_width == 640 && image_height == 480) {
385 std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\""
386 << std::endl;
387 cam.initPersProjWithoutDistortion(1109.5735473989, 1112.1520168160, 320, 240);
388 } else {
389 vpTRACE("Cannot get default intrinsic camera parameters for this image "
390 "resolution");
391 }
392 break;
393 }
395 // Set default intrinsic camera parameters for 640x480 images
396 if (image_width == 640 && image_height == 480) {
397 std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\""
398 << std::endl;
399 cam.initPersProjWithoutDistortion(852.6583228197, 854.8084224761, 320, 240);
400 } else {
401 vpTRACE("Cannot get default intrinsic camera parameters for this image "
402 "resolution");
403 }
404 break;
405 }
407 std::cout << "The generic tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
408 break;
409 }
411 std::cout << "The Intel D435 camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
412 break;
413 }
415 std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
416 break;
417 }
419 std::cout << "The vacuum tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
420 break;
421 }
422 default:
423 vpERROR_TRACE("This error should not occur!");
424 break;
425 }
426 return;
427}
428
441
454
461{
462 px_int = cam.get_px();
463 py_int = cam.get_py();
464 toolCustom = true;
465}
466
472{
473 double tcur_1 = tcur; // temporary variable used to store the last time
474 // since the last command
475 bool stop = false;
476 bool setVelocityCalled_ = false;
477 while (!stop) {
478 // Get current time
479 tprev = tcur_1;
481
483 setVelocityCalled_ = setVelocityCalled;
485
486 if (setVelocityCalled_ || !constantSamplingTimeMode) {
488 setVelocityCalled = false;
490
492
493 double ellapsedTime = (tcur - tprev) * 1e-3;
494 if (constantSamplingTimeMode) { // if we want a constant velocity, we
495 // force the ellapsed time to the given
496 // samplingTime
497 ellapsedTime = getSamplingTime(); // in second
498 }
499
500 vpColVector articularCoordinates = get_artCoord();
501 vpColVector articularVelocities = get_artVel();
502
503 if (jointLimit) {
504 double art = articularCoordinates[jointLimitArt - 1] + ellapsedTime * articularVelocities[jointLimitArt - 1];
505 if (art <= _joint_min[jointLimitArt - 1] || art >= _joint_max[jointLimitArt - 1]) {
506 if (verbose_) {
507 std::cout << "Joint " << jointLimitArt - 1
508 << " reaches a limit: " << vpMath::deg(_joint_min[jointLimitArt - 1]) << " < " << vpMath::deg(art)
509 << " < " << vpMath::deg(_joint_max[jointLimitArt - 1]) << std::endl;
510 }
511
512 articularVelocities = 0.0;
513 } else
514 jointLimit = false;
515 }
516
517 articularCoordinates[0] = articularCoordinates[0] + ellapsedTime * articularVelocities[0];
518 articularCoordinates[1] = articularCoordinates[1] + ellapsedTime * articularVelocities[1];
519 articularCoordinates[2] = articularCoordinates[2] + ellapsedTime * articularVelocities[2];
520 articularCoordinates[3] = articularCoordinates[3] + ellapsedTime * articularVelocities[3];
521 articularCoordinates[4] = articularCoordinates[4] + ellapsedTime * articularVelocities[4];
522 articularCoordinates[5] = articularCoordinates[5] + ellapsedTime * articularVelocities[5];
523
524 int jl = isInJointLimit();
525
526 if (jl != 0 && jointLimit == false) {
527 if (jl < 0)
528 ellapsedTime = (_joint_min[(unsigned int)(-jl - 1)] - articularCoordinates[(unsigned int)(-jl - 1)]) /
529 (articularVelocities[(unsigned int)(-jl - 1)]);
530 else
531 ellapsedTime = (_joint_max[(unsigned int)(jl - 1)] - articularCoordinates[(unsigned int)(jl - 1)]) /
532 (articularVelocities[(unsigned int)(jl - 1)]);
533
534 for (unsigned int i = 0; i < 6; i++)
535 articularCoordinates[i] = articularCoordinates[i] + ellapsedTime * articularVelocities[i];
536
537 jointLimit = true;
538 jointLimitArt = (unsigned int)fabs((double)jl);
539 }
540
541 set_artCoord(articularCoordinates);
542 set_artVel(articularVelocities);
543
544 compute_fMi();
545
546 if (displayAllowed) {
550 }
551
553 while (get_displayBusy()) {
554 vpTime::wait(2);
555 }
557 set_displayBusy(false);
558 }
559
561 vpHomogeneousMatrix fMit[8];
562 get_fMi(fMit);
563
564 // vpDisplay::displayFrame(I,getExternalCameraPosition
565 // ()*fMi[6],cameraParam,0.2,vpColor::none);
566
567 vpImagePoint iP, iP_1;
568 vpPoint pt(0, 0, 0);
569
572 pt.track(getExternalCameraPosition() * fMit[0]);
575 for (unsigned int k = 1; k < 7; k++) {
576 pt.track(getExternalCameraPosition() * fMit[k - 1]);
578
579 pt.track(getExternalCameraPosition() * fMit[k]);
581
583 }
585 thickness_);
586 }
587
589
591 tcur_1 = tcur;
592 } else {
594 }
595
597 stop = robotStop;
599 }
600}
601
616{
617 // vpColVector q = get_artCoord();
618 vpColVector q(6); //; = get_artCoord();
619 q = get_artCoord();
620
621 vpHomogeneousMatrix fMit[8];
622
623 double q1 = q[0];
624 double q2 = q[1];
625 double q3 = q[2];
626 double q4 = q[3];
627 double q5 = q[4];
628 double q6 = q[5];
629
630 double c4 = cos(q4);
631 double s4 = sin(q4);
632 double c5 = cos(q5);
633 double s5 = sin(q5);
634 double c6 = cos(q6);
635 double s6 = sin(q6);
636
637 fMit[0][0][0] = 1;
638 fMit[0][1][0] = 0;
639 fMit[0][2][0] = 0;
640 fMit[0][0][1] = 0;
641 fMit[0][1][1] = 1;
642 fMit[0][2][1] = 0;
643 fMit[0][0][2] = 0;
644 fMit[0][1][2] = 0;
645 fMit[0][2][2] = 1;
646 fMit[0][0][3] = q1;
647 fMit[0][1][3] = 0;
648 fMit[0][2][3] = 0;
649
650 fMit[1][0][0] = 1;
651 fMit[1][1][0] = 0;
652 fMit[1][2][0] = 0;
653 fMit[1][0][1] = 0;
654 fMit[1][1][1] = 1;
655 fMit[1][2][1] = 0;
656 fMit[1][0][2] = 0;
657 fMit[1][1][2] = 0;
658 fMit[1][2][2] = 1;
659 fMit[1][0][3] = q1;
660 fMit[1][1][3] = q2;
661 fMit[1][2][3] = 0;
662
663 fMit[2][0][0] = 1;
664 fMit[2][1][0] = 0;
665 fMit[2][2][0] = 0;
666 fMit[2][0][1] = 0;
667 fMit[2][1][1] = 1;
668 fMit[2][2][1] = 0;
669 fMit[2][0][2] = 0;
670 fMit[2][1][2] = 0;
671 fMit[2][2][2] = 1;
672 fMit[2][0][3] = q1;
673 fMit[2][1][3] = q2;
674 fMit[2][2][3] = q3;
675
676 fMit[3][0][0] = s4;
677 fMit[3][1][0] = -c4;
678 fMit[3][2][0] = 0;
679 fMit[3][0][1] = c4;
680 fMit[3][1][1] = s4;
681 fMit[3][2][1] = 0;
682 fMit[3][0][2] = 0;
683 fMit[3][1][2] = 0;
684 fMit[3][2][2] = 1;
685 fMit[3][0][3] = q1;
686 fMit[3][1][3] = q2;
687 fMit[3][2][3] = q3;
688
689 fMit[4][0][0] = s4 * s5;
690 fMit[4][1][0] = -c4 * s5;
691 fMit[4][2][0] = c5;
692 fMit[4][0][1] = s4 * c5;
693 fMit[4][1][1] = -c4 * c5;
694 fMit[4][2][1] = -s5;
695 fMit[4][0][2] = c4;
696 fMit[4][1][2] = s4;
697 fMit[4][2][2] = 0;
698 fMit[4][0][3] = c4 * this->_long_56 + q1;
699 fMit[4][1][3] = s4 * this->_long_56 + q2;
700 fMit[4][2][3] = q3;
701
702 fMit[5][0][0] = s4 * s5 * c6 + c4 * s6;
703 fMit[5][1][0] = -c4 * s5 * c6 + s4 * s6;
704 fMit[5][2][0] = c5 * c6;
705 fMit[5][0][1] = -s4 * s5 * s6 + c4 * c6;
706 fMit[5][1][1] = c4 * s5 * s6 + s4 * c6;
707 fMit[5][2][1] = -c5 * s6;
708 fMit[5][0][2] = -s4 * c5;
709 fMit[5][1][2] = c4 * c5;
710 fMit[5][2][2] = s5;
711 fMit[5][0][3] = c4 * this->_long_56 + q1;
712 fMit[5][1][3] = s4 * this->_long_56 + q2;
713 fMit[5][2][3] = q3;
714
715 fMit[6][0][0] = fMit[5][0][0];
716 fMit[6][1][0] = fMit[5][1][0];
717 fMit[6][2][0] = fMit[5][2][0];
718 fMit[6][0][1] = fMit[5][0][1];
719 fMit[6][1][1] = fMit[5][1][1];
720 fMit[6][2][1] = fMit[5][2][1];
721 fMit[6][0][2] = fMit[5][0][2];
722 fMit[6][1][2] = fMit[5][1][2];
723 fMit[6][2][2] = fMit[5][2][2];
724 fMit[6][0][3] = fMit[5][0][3];
725 fMit[6][1][3] = fMit[5][1][3];
726 fMit[6][2][3] = fMit[5][2][3];
727
728 // vpHomogeneousMatrix cMe;
729 // get_cMe(cMe);
730 // cMe = cMe.inverse();
731 // fMit[7] = fMit[6] * cMe;
733 vpAfma6::get_fMc(q, fMit[7]);
735
737 for (int i = 0; i < 8; i++) {
738 fMi[i] = fMit[i];
739 }
741}
742
749{
750 switch (newState) {
751 case vpRobot::STATE_STOP: {
752 // Start primitive STOP only if the current state is Velocity
754 stopMotion();
755 }
756 break;
757 }
760 std::cout << "Change the control mode from velocity to position control.\n";
761 stopMotion();
762 } else {
763 // std::cout << "Change the control mode from stop to position
764 // control.\n";
765 }
766 break;
767 }
770 std::cout << "Change the control mode from stop to velocity control.\n";
771 }
772 break;
773 }
775 default:
776 break;
777 }
778
779 return vpRobot::setRobotState(newState);
780}
781
856{
858 vpERROR_TRACE("Cannot send a velocity to the robot "
859 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
861 "Cannot send a velocity to the robot "
862 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
863 }
864
865 vpColVector vel_sat(6);
866
867 double scale_sat = 1;
868 double vel_trans_max = getMaxTranslationVelocity();
869 double vel_rot_max = getMaxRotationVelocity();
870
871 double vel_abs; // Absolute value
872
873 // Velocity saturation
874 switch (frame) {
875 // saturation in cartesian space
878 if (vel.getRows() != 6) {
879 vpERROR_TRACE("The velocity vector must have a size of 6 !!!!");
880 throw;
881 }
882
883 for (unsigned int i = 0; i < 3; ++i) {
884 vel_abs = fabs(vel[i]);
885 if (vel_abs > vel_trans_max && !jointLimit) {
886 vel_trans_max = vel_abs;
887 vpERROR_TRACE("Excess velocity %g m/s in TRANSLATION "
888 "(axis nr. %d).",
889 vel[i], i + 1);
890 }
891
892 vel_abs = fabs(vel[i + 3]);
893 if (vel_abs > vel_rot_max && !jointLimit) {
894 vel_rot_max = vel_abs;
895 vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
896 "(axis nr. %d).",
897 vel[i + 3], i + 4);
898 }
899 }
900
901 double scale_trans_sat = 1;
902 double scale_rot_sat = 1;
903 if (vel_trans_max > getMaxTranslationVelocity())
904 scale_trans_sat = getMaxTranslationVelocity() / vel_trans_max;
905
906 if (vel_rot_max > getMaxRotationVelocity())
907 scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
908
909 if ((scale_trans_sat < 1) || (scale_rot_sat < 1)) {
910 if (scale_trans_sat < scale_rot_sat)
911 scale_sat = scale_trans_sat;
912 else
913 scale_sat = scale_rot_sat;
914 }
915 break;
916 }
917
918 // saturation in joint space
920 if (vel.getRows() != 6) {
921 vpERROR_TRACE("The velocity vector must have a size of 6 !!!!");
922 throw;
923 }
924 for (unsigned int i = 0; i < 6; ++i) {
925 vel_abs = fabs(vel[i]);
926 if (vel_abs > vel_rot_max && !jointLimit) {
927 vel_rot_max = vel_abs;
928 vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
929 "(axis nr. %d).",
930 vel[i], i + 1);
931 }
932 }
933 double scale_rot_sat = 1;
934 if (vel_rot_max > getMaxRotationVelocity())
935 scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
936 if (scale_rot_sat < 1)
937 scale_sat = scale_rot_sat;
938 break;
939 }
940 case vpRobot::MIXT_FRAME: {
941 throw vpRobotException(vpRobotException::wrongStateError, "Cannot set a velocity in MIXT_FRAME frame:"
942 "functionality not implemented");
943 }
945 throw vpRobotException(vpRobotException::wrongStateError, "Cannot set a velocity in END_EFFECTOR_FRAME frame:"
946 "functionality not implemented");
947 }
948 }
949
950 set_velocity(vel * scale_sat);
951
953 setRobotFrame(frame);
955
957 setVelocityCalled = true;
959}
960
965{
967
969 frame = getRobotFrame();
971
972 double vel_rot_max = getMaxRotationVelocity();
973
974 vpColVector articularCoordinates = get_artCoord();
975 vpColVector velocityframe = get_velocity();
976 vpColVector articularVelocity;
977
978 switch (frame) {
980 vpMatrix eJe_;
982 vpAfma6::get_eJe(articularCoordinates, eJe_);
983 eJe_ = eJe_.pseudoInverse();
985 singularityTest(articularCoordinates, eJe_);
986 articularVelocity = eJe_ * eVc * velocityframe;
987 set_artVel(articularVelocity);
988 break;
989 }
991 vpMatrix fJe_;
992 vpAfma6::get_fJe(articularCoordinates, fJe_);
993 fJe_ = fJe_.pseudoInverse();
995 singularityTest(articularCoordinates, fJe_);
996 articularVelocity = fJe_ * velocityframe;
997 set_artVel(articularVelocity);
998 break;
999 }
1001 articularVelocity = velocityframe;
1002 set_artVel(articularVelocity);
1003 break;
1004 }
1006 case vpRobot::MIXT_FRAME: {
1007 break;
1008 }
1009 }
1010
1011 switch (frame) {
1014 for (unsigned int i = 0; i < 6; ++i) {
1015 double vel_abs = fabs(articularVelocity[i]);
1016 if (vel_abs > vel_rot_max && !jointLimit) {
1017 vel_rot_max = vel_abs;
1018 vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
1019 "(axis nr. %d).",
1020 articularVelocity[i], i + 1);
1021 }
1022 }
1023 double scale_rot_sat = 1;
1024 double scale_sat = 1;
1025 if (vel_rot_max > getMaxRotationVelocity())
1026 scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
1027 if (scale_rot_sat < 1)
1028 scale_sat = scale_rot_sat;
1029
1030 set_artVel(articularVelocity * scale_sat);
1031 break;
1032 }
1035 case vpRobot::MIXT_FRAME: {
1036 break;
1037 }
1038 }
1039}
1040
1087{
1088 vel.resize(6);
1089
1090 vpColVector articularCoordinates = get_artCoord();
1091 vpColVector articularVelocity = get_artVel();
1092
1093 switch (frame) {
1094 case vpRobot::CAMERA_FRAME: {
1095 vpMatrix eJe_;
1097 vpAfma6::get_eJe(articularCoordinates, eJe_);
1098 vel = cVe * eJe_ * articularVelocity;
1099 break;
1100 }
1102 vel = articularVelocity;
1103 break;
1104 }
1106 vpMatrix fJe_;
1107 vpAfma6::get_fJe(articularCoordinates, fJe_);
1108 vel = fJe_ * articularVelocity;
1109 break;
1110 }
1112 case vpRobot::MIXT_FRAME: {
1113 break;
1114 }
1115 default: {
1116 vpERROR_TRACE("Error in spec of vpRobot. "
1117 "Case not taken in account.");
1118 return;
1119 }
1120 }
1121}
1122
1140{
1141 timestamp = vpTime::measureTimeSecond();
1142 getVelocity(frame, vel);
1143}
1144
1187{
1188 vpColVector vel(6);
1189 getVelocity(frame, vel);
1190
1191 return vel;
1192}
1193
1207{
1208 timestamp = vpTime::measureTimeSecond();
1209 vpColVector vel(6);
1210 getVelocity(frame, vel);
1211
1212 return vel;
1213}
1214
1216{
1217 double vel_rot_max = getMaxRotationVelocity();
1218 double velmax = fabs(q[0]);
1219 for (unsigned int i = 1; i < 6; i++) {
1220 if (velmax < fabs(q[i]))
1221 velmax = fabs(q[i]);
1222 }
1223
1224 double alpha = (getPositioningVelocity() * vel_rot_max) / (velmax * 100);
1225 q = q * alpha;
1226}
1227
1303{
1305 vpERROR_TRACE("Robot was not in position-based control\n"
1306 "Modification of the robot state");
1307 // setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
1308 }
1309
1310 vpColVector articularCoordinates = get_artCoord();
1311
1312 vpColVector error(6);
1313 double errsqr = 0;
1314 switch (frame) {
1315 case vpRobot::CAMERA_FRAME: {
1316 int nbSol;
1317 vpColVector qdes(6);
1318
1320 vpRxyzVector rxyz;
1321 for (unsigned int i = 0; i < 3; i++) {
1322 txyz[i] = q[i];
1323 rxyz[i] = q[i + 3];
1324 }
1325
1326 vpRotationMatrix cRc2(rxyz);
1327 vpHomogeneousMatrix cMc2(txyz, cRc2);
1328
1330 vpAfma6::get_fMc(articularCoordinates, fMc_);
1331
1332 vpHomogeneousMatrix fMc2 = fMc_ * cMc2;
1333
1334 do {
1335 articularCoordinates = get_artCoord();
1336 qdes = articularCoordinates;
1337 nbSol = getInverseKinematics(fMc2, qdes, true, verbose_);
1338
1340 setVelocityCalled = true;
1342
1343 if (nbSol > 0) {
1344 error = qdes - articularCoordinates;
1345 errsqr = error.sumSquare();
1346 // findHighestPositioningSpeed(error);
1347 set_artVel(error);
1348 if (errsqr < 1e-4) {
1349 set_artCoord(qdes);
1350 error = 0;
1351 set_artVel(error);
1352 set_velocity(error);
1353 break;
1354 }
1355 } else {
1356 vpERROR_TRACE("Positioning error.");
1357 throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
1358 }
1359 } while (errsqr > 1e-8 && nbSol > 0);
1360
1361 break;
1362 }
1363
1365 do {
1366 articularCoordinates = get_artCoord();
1367 error = q - articularCoordinates;
1368 errsqr = error.sumSquare();
1369 // findHighestPositioningSpeed(error);
1370 set_artVel(error);
1372 setVelocityCalled = true;
1374 if (errsqr < 1e-4) {
1375 set_artCoord(q);
1376 error = 0;
1377 set_artVel(error);
1378 set_velocity(error);
1379 break;
1380 }
1381 } while (errsqr > 1e-8);
1382 break;
1383 }
1384
1386 int nbSol;
1387 vpColVector qdes(6);
1388
1390 vpRxyzVector rxyz;
1391 for (unsigned int i = 0; i < 3; i++) {
1392 txyz[i] = q[i];
1393 rxyz[i] = q[i + 3];
1394 }
1395
1396 vpRotationMatrix fRc(rxyz);
1397 vpHomogeneousMatrix fMc_(txyz, fRc);
1398
1399 do {
1400 articularCoordinates = get_artCoord();
1401 qdes = articularCoordinates;
1402 nbSol = getInverseKinematics(fMc_, qdes, true, verbose_);
1404 setVelocityCalled = true;
1406 if (nbSol > 0) {
1407 error = qdes - articularCoordinates;
1408 errsqr = error.sumSquare();
1409 // findHighestPositioningSpeed(error);
1410 set_artVel(error);
1411 if (errsqr < 1e-4) {
1412 set_artCoord(qdes);
1413 error = 0;
1414 set_artVel(error);
1415 set_velocity(error);
1416 break;
1417 }
1418 } else
1419 vpERROR_TRACE("Positioning error. Position unreachable");
1420 } while (errsqr > 1e-8 && nbSol > 0);
1421 break;
1422 }
1423 case vpRobot::MIXT_FRAME: {
1424 vpERROR_TRACE("Positioning error. Mixt frame not implemented");
1425 throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1426 "MIXT_FRAME not implemented.");
1427 }
1429 vpERROR_TRACE("Positioning error. Mixt frame not implemented");
1430 throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1431 "END_EFFECTOR_FRAME not implemented.");
1432 }
1433 }
1434}
1435
1498void vpSimulatorAfma6::setPosition(const vpRobot::vpControlFrameType frame, double pos1, double pos2, double pos3,
1499 double pos4, double pos5, double pos6)
1500{
1501 try {
1502 vpColVector position(6);
1503 position[0] = pos1;
1504 position[1] = pos2;
1505 position[2] = pos3;
1506 position[3] = pos4;
1507 position[4] = pos5;
1508 position[5] = pos6;
1509
1510 setPosition(frame, position);
1511 } catch (...) {
1512 vpERROR_TRACE("Error caught");
1513 throw;
1514 }
1515}
1516
1551void vpSimulatorAfma6::setPosition(const char *filename)
1552{
1553 vpColVector q;
1554 bool ret;
1555
1556 ret = this->readPosFile(filename, q);
1557
1558 if (ret == false) {
1559 vpERROR_TRACE("Bad position in \"%s\"", filename);
1560 throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
1561 }
1564}
1565
1626{
1627 q.resize(6);
1628
1629 switch (frame) {
1630 case vpRobot::CAMERA_FRAME: {
1631 q = 0;
1632 break;
1633 }
1634
1636 q = get_artCoord();
1637 break;
1638 }
1639
1643
1644 vpRotationMatrix fRc;
1645 fMc_.extract(fRc);
1646 vpRxyzVector rxyz(fRc);
1647
1649 fMc_.extract(txyz);
1650
1651 for (unsigned int i = 0; i < 3; i++) {
1652 q[i] = txyz[i];
1653 q[i + 3] = rxyz[i];
1654 }
1655 break;
1656 }
1657
1658 case vpRobot::MIXT_FRAME: {
1659 throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1660 "Mixt frame not implemented.");
1661 }
1663 throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1664 "End-effector frame not implemented.");
1665 }
1666 }
1667}
1668
1696{
1697 timestamp = vpTime::measureTimeSecond();
1698 getPosition(frame, q);
1699}
1700
1713{
1714 vpColVector posRxyz;
1715 // recupere position en Rxyz
1716 this->getPosition(frame, posRxyz);
1717
1718 // recupere le vecteur thetaU correspondant
1719 vpThetaUVector RtuVect(vpRxyzVector(posRxyz[3], posRxyz[4], posRxyz[5]));
1720
1721 // remplit le vpPoseVector avec translation et rotation ThetaU
1722 for (unsigned int j = 0; j < 3; j++) {
1723 position[j] = posRxyz[j];
1724 position[j + 3] = RtuVect[j];
1725 }
1726}
1727
1739void vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double &timestamp)
1740{
1741 timestamp = vpTime::measureTimeSecond();
1742 getPosition(frame, position);
1743}
1744
1755void vpSimulatorAfma6::setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
1756{
1757 if (limitMin.getRows() != 6 || limitMax.getRows() != 6) {
1758 vpTRACE("Joint limit vector has not a size of 6 !");
1759 return;
1760 }
1761
1762 _joint_min[0] = limitMin[0];
1763 _joint_min[1] = limitMin[1];
1764 _joint_min[2] = limitMin[2];
1765 _joint_min[3] = limitMin[3];
1766 _joint_min[4] = limitMin[4];
1767 _joint_min[5] = limitMin[5];
1768
1769 _joint_max[0] = limitMax[0];
1770 _joint_max[1] = limitMax[1];
1771 _joint_max[2] = limitMax[2];
1772 _joint_max[3] = limitMax[3];
1773 _joint_max[4] = limitMax[4];
1774 _joint_max[5] = limitMax[5];
1775}
1776
1783{
1784 double q5 = q[4];
1785
1786 bool cond = fabs(q5 - M_PI / 2) < 1e-1;
1787
1788 if (cond) {
1789 J[0][3] = 0;
1790 J[0][4] = 0;
1791 J[1][3] = 0;
1792 J[1][4] = 0;
1793 J[3][3] = 0;
1794 J[3][4] = 0;
1795 J[5][3] = 0;
1796 J[5][4] = 0;
1797 return true;
1798 }
1799
1800 return false;
1801}
1802
1807{
1808 int artNumb = 0;
1809 double diff = 0;
1810 double difft = 0;
1811
1812 vpColVector articularCoordinates = get_artCoord();
1813
1814 for (unsigned int i = 0; i < 6; i++) {
1815 if (articularCoordinates[i] <= _joint_min[i]) {
1816 difft = _joint_min[i] - articularCoordinates[i];
1817 if (difft > diff) {
1818 diff = difft;
1819 artNumb = -(int)i - 1;
1820 }
1821 }
1822 }
1823
1824 for (unsigned int i = 0; i < 6; i++) {
1825 if (articularCoordinates[i] >= _joint_max[i]) {
1826 difft = articularCoordinates[i] - _joint_max[i];
1827 if (difft > diff) {
1828 diff = difft;
1829 artNumb = (int)(i + 1);
1830 }
1831 }
1832 }
1833
1834 if (artNumb != 0)
1835 std::cout << "\nWarning: Velocity control stopped: axis " << fabs((float)artNumb) << " on joint limit!"
1836 << std::endl;
1837
1838 return artNumb;
1839}
1840
1859{
1860 displacement.resize(6);
1861 displacement = 0;
1862 vpColVector q_cur(6);
1863
1864 q_cur = get_artCoord();
1865
1866 if (!first_time_getdis) {
1867 switch (frame) {
1868 case vpRobot::CAMERA_FRAME: {
1869 std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
1870 return;
1871 }
1873 displacement = q_cur - q_prev_getdis;
1874 break;
1875 }
1877 std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
1878 return;
1879 }
1880 case vpRobot::MIXT_FRAME: {
1881 std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
1882 return;
1883 }
1885 std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
1886 return;
1887 }
1888 }
1889 } else {
1890 first_time_getdis = false;
1891 }
1892
1893 // Memorize the joint position for the next call
1894 q_prev_getdis = q_cur;
1895}
1896
1944bool vpSimulatorAfma6::readPosFile(const std::string &filename, vpColVector &q)
1945{
1946 std::ifstream fd(filename.c_str(), std::ios::in);
1947
1948 if (!fd.is_open()) {
1949 return false;
1950 }
1951
1952 std::string line;
1953 std::string key("R:");
1954 std::string id("#AFMA6 - Position");
1955 bool pos_found = false;
1956 int lineNum = 0;
1957
1958 q.resize(njoint);
1959
1960 while (std::getline(fd, line)) {
1961 lineNum++;
1962 if (lineNum == 1) {
1963 if (!(line.compare(0, id.size(), id) == 0)) { // check if Afma6 position file
1964 std::cout << "Error: this position file " << filename << " is not for Afma6 robot" << std::endl;
1965 return false;
1966 }
1967 }
1968 if ((line.compare(0, 1, "#") == 0)) { // skip comment
1969 continue;
1970 }
1971 if ((line.compare(0, key.size(), key) == 0)) { // decode position
1972 // check if there are at least njoint values in the line
1973 std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
1974 if (chain.size() < njoint + 1) // try to split with tab separator
1975 chain = vpIoTools::splitChain(line, std::string("\t"));
1976 if (chain.size() < njoint + 1)
1977 continue;
1978
1979 std::istringstream ss(line);
1980 std::string key_;
1981 ss >> key_;
1982 for (unsigned int i = 0; i < njoint; i++)
1983 ss >> q[i];
1984 pos_found = true;
1985 break;
1986 }
1987 }
1988
1989 // converts rotations from degrees into radians
1990 q[3] = vpMath::rad(q[3]);
1991 q[4] = vpMath::rad(q[4]);
1992 q[5] = vpMath::rad(q[5]);
1993
1994 fd.close();
1995
1996 if (!pos_found) {
1997 std::cout << "Error: unable to find a position for Afma6 robot in " << filename << std::endl;
1998 return false;
1999 }
2000
2001 return true;
2002}
2003
2026bool vpSimulatorAfma6::savePosFile(const std::string &filename, const vpColVector &q)
2027{
2028 FILE *fd;
2029 fd = fopen(filename.c_str(), "w");
2030 if (fd == NULL)
2031 return false;
2032
2033 fprintf(fd, "\
2034#AFMA6 - Position - Version 2.01\n\
2035#\n\
2036# R: X Y Z A B C\n\
2037# Joint position: X, Y, Z: translations in meters\n\
2038# A, B, C: rotations in degrees\n\
2039#\n\
2040#\n\n");
2041
2042 // Save positions in mm and deg
2043 fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n", q[0], q[1], q[2], vpMath::deg(q[3]), vpMath::deg(q[4]),
2044 vpMath::deg(q[5]));
2045
2046 fclose(fd);
2047 return (true);
2048}
2049
2057void vpSimulatorAfma6::move(const char *filename)
2058{
2059 vpColVector q;
2060
2061 try {
2062 this->readPosFile(filename, q);
2065 } catch (...) {
2066 throw;
2067 }
2068}
2069
2080
2089{
2091 vpAfma6::get_cMe(cMe);
2092
2093 cVe.buildFrom(cMe);
2094}
2095
2106{
2107 try {
2109 } catch (...) {
2110 vpERROR_TRACE("catch exception ");
2111 throw;
2112 }
2113}
2114
2126{
2127 try {
2128 vpColVector articularCoordinates = get_artCoord();
2129 vpAfma6::get_fJe(articularCoordinates, fJe_);
2130 } catch (...) {
2131 vpERROR_TRACE("Error caught");
2132 throw;
2133 }
2134}
2135
2140{
2142 return;
2143
2144 vpColVector stop(6);
2145 stop = 0;
2146 set_artVel(stop);
2147 set_velocity(stop);
2149}
2150
2151/**********************************************************************************/
2152/**********************************************************************************/
2153/**********************************************************************************/
2154/**********************************************************************************/
2155
2165{
2166 // set scene_dir from #define VISP_SCENE_DIR if it exists
2167 // VISP_SCENES_DIR may contain multiple locations separated by ";"
2168 std::string scene_dir_;
2169 std::vector<std::string> scene_dirs = vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(";"));
2170 bool sceneDirExists = false;
2171 for (size_t i = 0; i < scene_dirs.size(); i++)
2172 if (vpIoTools::checkDirectory(scene_dirs[i]) == true) { // directory exists
2173 scene_dir_ = scene_dirs[i];
2174 sceneDirExists = true;
2175 break;
2176 }
2177 if (!sceneDirExists) {
2178 try {
2179 scene_dir_ = vpIoTools::getenv("VISP_SCENES_DIR");
2180 std::cout << "The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
2181 } catch (...) {
2182 std::cout << "Cannot get VISP_SCENES_DIR environment variable" << std::endl;
2183 }
2184 }
2185
2186 unsigned int name_length = 30; // the size of this kind of string "/afma6_arm2.bnd"
2187 if (scene_dir_.size() > FILENAME_MAX)
2188 throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
2189 unsigned int full_length = (unsigned int)scene_dir_.size() + name_length;
2190 if (full_length > FILENAME_MAX)
2191 throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
2192
2193 char *name_cam = new char[full_length];
2194
2195 strcpy(name_cam, scene_dir_.c_str());
2196 strcat(name_cam, "/camera.bnd");
2197 set_scene(name_cam, &camera, cameraFactor);
2198
2199 if (arm_dir.size() > FILENAME_MAX)
2200 throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
2201 full_length = (unsigned int)arm_dir.size() + name_length;
2202 if (full_length > FILENAME_MAX)
2203 throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
2204
2205 char *name_arm = new char[full_length];
2206 strcpy(name_arm, arm_dir.c_str());
2207 strcat(name_arm, "/afma6_gate.bnd");
2208 std::cout << "name arm: " << name_arm << std::endl;
2209 set_scene(name_arm, robotArms, 1.0);
2210 strcpy(name_arm, arm_dir.c_str());
2211 strcat(name_arm, "/afma6_arm1.bnd");
2212 set_scene(name_arm, robotArms + 1, 1.0);
2213 strcpy(name_arm, arm_dir.c_str());
2214 strcat(name_arm, "/afma6_arm2.bnd");
2215 set_scene(name_arm, robotArms + 2, 1.0);
2216 strcpy(name_arm, arm_dir.c_str());
2217 strcat(name_arm, "/afma6_arm3.bnd");
2218 set_scene(name_arm, robotArms + 3, 1.0);
2219 strcpy(name_arm, arm_dir.c_str());
2220 strcat(name_arm, "/afma6_arm4.bnd");
2221 set_scene(name_arm, robotArms + 4, 1.0);
2222
2224 tool = getToolType();
2225 strcpy(name_arm, arm_dir.c_str());
2226 switch (tool) {
2227 case vpAfma6::TOOL_CCMOP: {
2228 strcat(name_arm, "/afma6_tool_ccmop.bnd");
2229 break;
2230 }
2231 case vpAfma6::TOOL_GRIPPER: {
2232 strcat(name_arm, "/afma6_tool_gripper.bnd");
2233 break;
2234 }
2235 case vpAfma6::TOOL_VACUUM: {
2236 strcat(name_arm, "/afma6_tool_vacuum.bnd");
2237 break;
2238 }
2239 case vpAfma6::TOOL_CUSTOM: {
2240 std::cout << "The custom tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
2241 break;
2242 }
2244 std::cout << "The Intel D435 camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
2245 break;
2246 }
2248 std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
2249 break;
2250 }
2251 }
2252 set_scene(name_arm, robotArms + 5, 1.0);
2253
2254 add_rfstack(IS_BACK);
2255
2256 add_vwstack("start", "depth", 0.0, 100.0);
2257 add_vwstack("start", "window", -0.1, 0.1, -0.1, 0.1);
2258 add_vwstack("start", "type", PERSPECTIVE);
2259 //
2260 // sceneInitialized = true;
2261 // displayObject = true;
2262 displayCamera = true;
2263
2264 delete[] name_cam;
2265 delete[] name_arm;
2266}
2267
2269{
2271 bool changed = false;
2272 vpHomogeneousMatrix displacement = navigation(I_, changed);
2273
2274 // if (displacement[2][3] != 0)
2275 if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
2276 camMf2 = camMf2 * displacement;
2277
2278 f2Mf = camMf2.inverse() * camMf;
2279
2280 camMf = camMf2 * displacement * f2Mf;
2281
2282 double u;
2283 double v;
2284 // if(px_ext != 1 && py_ext != 1)
2285 // we assume px_ext and py_ext > 0
2286 if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
2287 (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon())) {
2288 u = (double)I_.getWidth() / (2 * px_ext);
2289 v = (double)I_.getHeight() / (2 * py_ext);
2290 } else {
2291 u = (double)I_.getWidth() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
2292 v = (double)I_.getHeight() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
2293 }
2294
2295 float w44o[4][4], w44cext[4][4], x, y, z;
2296
2297 vp2jlc_matrix(camMf.inverse(), w44cext);
2298
2299 add_vwstack("start", "cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
2300 x = w44cext[2][0] + w44cext[3][0];
2301 y = w44cext[2][1] + w44cext[3][1];
2302 z = w44cext[2][2] + w44cext[3][2];
2303 add_vwstack("start", "vrp", x, y, z);
2304 add_vwstack("start", "vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
2305 add_vwstack("start", "vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
2306 add_vwstack("start", "window", -u, u, -v, v);
2307
2308 vpHomogeneousMatrix fMit[8];
2309 get_fMi(fMit);
2310
2311 vp2jlc_matrix(vpHomogeneousMatrix(0, 0, 0, 0, 0, 0), w44o);
2312 display_scene(w44o, robotArms[0], I_, curColor);
2313
2314 vp2jlc_matrix(fMit[0], w44o);
2315 display_scene(w44o, robotArms[1], I_, curColor);
2316
2317 vp2jlc_matrix(fMit[2], w44o);
2318 display_scene(w44o, robotArms[2], I_, curColor);
2319
2320 vp2jlc_matrix(fMit[3], w44o);
2321 display_scene(w44o, robotArms[3], I_, curColor);
2322
2323 vp2jlc_matrix(fMit[4], w44o);
2324 display_scene(w44o, robotArms[4], I_, curColor);
2325
2326 vp2jlc_matrix(fMit[5], w44o);
2327 display_scene(w44o, robotArms[5], I_, curColor);
2328
2329 if (displayCamera) {
2331 get_cMe(cMe);
2332 cMe = cMe.inverse();
2333 cMe = fMit[6] * cMe;
2334 vp2jlc_matrix(cMe, w44o);
2335 display_scene(w44o, camera, I_, camColor);
2336 }
2337
2338 if (displayObject) {
2339 vp2jlc_matrix(fMo, w44o);
2340 display_scene(w44o, scene, I_, curColor);
2341 }
2343}
2344
2363{
2364 vpColVector stop(6);
2365 bool status = true;
2366 stop = 0;
2368 set_artVel(stop);
2369 set_velocity(stop);
2371 fMc_ = fMo * cMo_.inverse();
2372
2373 vpColVector articularCoordinates = get_artCoord();
2374 int nbSol = getInverseKinematics(fMc_, articularCoordinates, true, verbose_);
2375
2376 if (nbSol == 0) {
2377 status = false;
2378 vpERROR_TRACE("Positioning error. Position unreachable");
2379 }
2380
2381 if (verbose_)
2382 std::cout << "Used joint coordinates (rad): " << articularCoordinates.t() << std::endl;
2383
2384 set_artCoord(articularCoordinates);
2385
2386 compute_fMi();
2388
2389 return status;
2390}
2391
2406{
2407 vpColVector stop(6);
2408 stop = 0;
2410 set_artVel(stop);
2411 set_velocity(stop);
2412 vpHomogeneousMatrix fMit[8];
2413 get_fMi(fMit);
2414 fMo = fMit[7] * cMo_;
2416}
2417
2429bool vpSimulatorAfma6::setPosition(const vpHomogeneousMatrix &cdMo_, vpImage<unsigned char> *Iint, const double &errMax)
2430{
2431 // get rid of max velocity
2432 double vMax = getMaxTranslationVelocity();
2433 double wMax = getMaxRotationVelocity();
2434 setMaxTranslationVelocity(10. * vMax);
2435 setMaxRotationVelocity(10. * wMax);
2436
2437 vpColVector v(3), w(3), vel(6);
2440 vpRotationMatrix cdRc;
2441 vpThetaUVector cdTUc;
2442 vpColVector err(6);
2443 err = 1.;
2444 const double lambda = 5.;
2445
2447
2448 unsigned int i, iter = 0;
2449 while ((iter++ < 300) && (err.frobeniusNorm() > errMax)) {
2450 double t = vpTime::measureTimeMs();
2451
2452 // update image
2453 if (Iint != NULL) {
2454 vpDisplay::display(*Iint);
2455 getInternalView(*Iint);
2456 vpDisplay::flush(*Iint);
2457 }
2458
2459 // update pose error
2460 cdMc = cdMo_ * get_cMo().inverse();
2461 cdMc.extract(cdRc);
2462 cdMc.extract(cdTc);
2463 cdTUc.buildFrom(cdRc);
2464
2465 // compute v,w and velocity
2466 v = -lambda * cdRc.t() * cdTc;
2467 w = -lambda * cdTUc;
2468 for (i = 0; i < 3; ++i) {
2469 vel[i] = v[i];
2470 vel[i + 3] = w[i];
2471 err[i] = cdTc[i];
2472 err[i + 3] = cdTUc[i];
2473 }
2474
2475 // update feat
2477
2478 // wait for it
2479 vpTime::wait(t, 10);
2480 }
2481 vel = 0.;
2482 set_velocity(vel);
2483 set_artVel(vel);
2486
2487 // std::cout << "setPosition: final error " << err.t() << std::endl;
2488 return (err.frobeniusNorm() <= errMax);
2489}
2490
2491#elif !defined(VISP_BUILD_SHARED_LIBS)
2492// Work around to avoid warning: libvisp_robot.a(vpSimulatorAfma6.cpp.o) has
2493// no symbols
2494void dummy_vpSimulatorAfma6(){};
2495#endif
Modelisation of Irisa's gantry robot named Afma6.
Definition vpAfma6.h:76
static const char *const CONST_CCMOP_CAMERA_NAME
Definition vpAfma6.h:99
double _joint_min[6]
Definition vpAfma6.h:201
static const unsigned int njoint
Number of joint.
Definition vpAfma6.h:195
vpRxyzVector _erc
Definition vpAfma6.h:204
int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest=true, const bool &verbose=false) const
Definition vpAfma6.cpp:599
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition vpAfma6.cpp:887
void setToolType(vpAfma6::vpAfma6ToolType tool)
Set the current tool type.
Definition vpAfma6.h:191
vpAfma6ToolType getToolType() const
Get the current tool type.
Definition vpAfma6.h:166
static const char *const CONST_GRIPPER_CAMERA_NAME
Definition vpAfma6.h:104
vpHomogeneousMatrix _eMc
Definition vpAfma6.h:206
double _long_56
Definition vpAfma6.h:199
vpTranslationVector _etc
Definition vpAfma6.h:203
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition vpAfma6.cpp:774
vpCameraParameters::vpCameraParametersProjType projModel
Definition vpAfma6.h:212
double _joint_max[6]
Definition vpAfma6.h:200
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition vpAfma6.cpp:931
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition vpAfma6.cpp:1001
vpAfma6ToolType
List of possible tools that can be attached to the robot end-effector.
Definition vpAfma6.h:123
@ TOOL_CCMOP
Definition vpAfma6.h:124
@ TOOL_GENERIC_CAMERA
Definition vpAfma6.h:127
@ TOOL_CUSTOM
Definition vpAfma6.h:129
@ TOOL_VACUUM
Definition vpAfma6.h:126
@ TOOL_INTEL_D435_CAMERA
Definition vpAfma6.h:128
@ TOOL_GRIPPER
Definition vpAfma6.h:125
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
vpRowVector t() const
double frobeniusNorm() 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)
void getInternalView(vpImage< vpRGBa > &I)
vpHomogeneousMatrix getExternalCameraPosition() const
void set_artCoord(const vpColVector &coord)
static void * launcher(void *arg)
void setExternalCameraPosition(const vpHomogeneousMatrix &camMf_)
void set_artVel(const vpColVector &vel)
virtual vpRobotStateType getRobotState(void) const
Definition vpRobot.h: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
void setMaxRotationVelocity(double maxVr)
Definition vpRobot.cpp:257
vpControlFrameType setRobotFrame(vpRobot::vpControlFrameType newFrame)
Definition vpRobot.cpp:204
void setMaxTranslationVelocity(double maxVt)
Definition vpRobot.cpp:236
Implementation of a rotation matrix and operations on such kind of matrices.
vpRotationMatrix t() const
Implementation of a rotation vector as Euler angle minimal representation.
void get_eJe(vpMatrix &eJe)
static bool savePosFile(const std::string &filename, const vpColVector &q)
void setCameraParameters(const vpCameraParameters &cam)
vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
void get_cVe(vpVelocityTwistMatrix &cVe)
void initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo)
static bool readPosFile(const std::string &filename, vpColVector &q)
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height)
bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo)
void get_fMi(vpHomogeneousMatrix *fMit)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
void findHighestPositioningSpeed(vpColVector &q)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
void move(const char *filename)
static const double defaultPositioningVelocity
bool singularityTest(const vpColVector &q, vpMatrix &J)
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &displacement)
void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
void get_fJe(vpMatrix &fJe)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q)
void getExternalImage(vpImage< vpRGBa > &I)
double getPositioningVelocity(void)
void get_cMe(vpHomogeneousMatrix &cMe)
Implementation of a rotation vector as axis-angle minimal representation.
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
Class that consider the case of a translation vector.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpHomogeneousMatrix camMf2
vpHomogeneousMatrix f2Mf
void display_scene(Matrix mat, Bound_scene &sc, const vpImage< vpRGBa > &I, const vpColor &color)
vpHomogeneousMatrix fMo
vpHomogeneousMatrix camMf
void setExternalCameraParameters(const vpCameraParameters &cam)
vpHomogeneousMatrix navigation(const vpImage< vpRGBa > &I, bool &changed)
#define vpTRACE
Definition vpDebug.h: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()