39 #include <visp3/core/vpConfig.h>
40 #if defined(VISP_HAVE_MODULE_GUI) && ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD))
44 #include <visp3/core/vpImagePoint.h>
45 #include <visp3/core/vpIoTools.h>
46 #include <visp3/core/vpMeterPixelConversion.h>
47 #include <visp3/core/vpPoint.h>
48 #include <visp3/core/vpTime.h>
49 #include <visp3/robot/vpRobotException.h>
50 #include <visp3/robot/vpSimulatorAfma6.h>
52 #include "../wireframe-simulator/vpBound.h"
53 #include "../wireframe-simulator/vpRfstack.h"
54 #include "../wireframe-simulator/vpScene.h"
55 #include "../wireframe-simulator/vpVwstack.h"
64 positioningVelocity(defaultPositioningVelocity), zeroPos(), reposPos(), toolCustom(false), arm_dir()
73 mutex_fMi = CreateMutexEx(NULL, NULL, 0, NULL);
79 mutex_fMi = CreateMutex(NULL, FALSE, NULL);
86 DWORD dwThreadIdArray;
87 hThread = CreateThread(NULL,
93 #elif defined(VISP_HAVE_PTHREAD)
100 pthread_attr_init(&
attr);
101 pthread_attr_setdetachstate(&
attr, PTHREAD_CREATE_JOINABLE);
117 positioningVelocity(defaultPositioningVelocity), zeroPos(), reposPos(), toolCustom(false), arm_dir()
126 mutex_fMi = CreateMutexEx(NULL, NULL, 0, NULL);
132 mutex_fMi = CreateMutex(NULL, FALSE, NULL);
139 DWORD dwThreadIdArray;
140 hThread = CreateThread(NULL,
146 #elif defined(VISP_HAVE_PTHREAD)
153 pthread_attr_init(&
attr);
154 pthread_attr_setdetachstate(&
attr, PTHREAD_CREATE_JOINABLE);
170 #if defined(WINRT_8_1)
171 WaitForSingleObjectEx(hThread, INFINITE, FALSE);
173 WaitForSingleObject(hThread, INFINITE);
175 CloseHandle(hThread);
181 #elif defined(VISP_HAVE_PTHREAD)
182 pthread_attr_destroy(&
attr);
183 pthread_join(
thread, NULL);
192 for (
int i = 0; i < 6; i++)
212 std::vector<std::string> arm_dirs =
vpIoTools::splitChain(std::string(VISP_ROBOT_ARMS_DIR), std::string(
";"));
213 bool armDirExists =
false;
214 for (
size_t i = 0; i < arm_dirs.size(); i++)
216 arm_dir = arm_dirs[i];
223 std::cout <<
"The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
225 std::cout <<
"Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
241 reposPos[1] = -M_PI / 2;
243 reposPos[4] = M_PI / 2;
250 first_time_getdis =
true;
311 unsigned int name_length = 30;
312 if (arm_dir.size() > FILENAME_MAX)
314 unsigned int full_length = (
unsigned int)arm_dir.size() + name_length;
315 if (full_length > FILENAME_MAX)
334 char *name_arm =
new char[full_length];
335 strcpy(name_arm, arm_dir.c_str());
336 strcat(name_arm,
"/afma6_tool_ccmop.bnd");
357 char *name_arm =
new char[full_length];
358 strcpy(name_arm, arm_dir.c_str());
359 strcat(name_arm,
"/afma6_tool_gripper.bnd");
381 char *name_arm =
new char[full_length];
383 strcpy(name_arm, arm_dir.c_str());
384 strcat(name_arm,
"/afma6_tool_vacuum.bnd");
393 std::cout <<
"The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
415 const unsigned int &image_height)
424 if (image_width == 640 && image_height == 480) {
429 vpTRACE(
"Cannot get default intrinsic camera parameters for this image "
436 if (image_width == 640 && image_height == 480) {
441 vpTRACE(
"Cannot get default intrinsic camera parameters for this image "
449 std::cout <<
"The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
503 double tcur_1 =
tcur;
516 double ellapsedTime = (
tcur -
tprev) * 1e-3;
535 articularVelocities = 0.0;
540 articularCoordinates[0] = articularCoordinates[0] + ellapsedTime * articularVelocities[0];
541 articularCoordinates[1] = articularCoordinates[1] + ellapsedTime * articularVelocities[1];
542 articularCoordinates[2] = articularCoordinates[2] + ellapsedTime * articularVelocities[2];
543 articularCoordinates[3] = articularCoordinates[3] + ellapsedTime * articularVelocities[3];
544 articularCoordinates[4] = articularCoordinates[4] + ellapsedTime * articularVelocities[4];
545 articularCoordinates[5] = articularCoordinates[5] + ellapsedTime * articularVelocities[5];
551 ellapsedTime = (
_joint_min[(
unsigned int)(-jl - 1)] - articularCoordinates[(
unsigned int)(-jl - 1)]) /
552 (articularVelocities[(
unsigned int)(-jl - 1)]);
554 ellapsedTime = (
_joint_max[(
unsigned int)(jl - 1)] - articularCoordinates[(
unsigned int)(jl - 1)]) /
555 (articularVelocities[(
unsigned int)(jl - 1)]);
557 for (
unsigned int i = 0; i < 6; i++)
558 articularCoordinates[i] = articularCoordinates[i] + ellapsedTime * articularVelocities[i];
597 for (
unsigned int k = 1; k < 7; k++) {
707 fMit[4][0][0] = s4 * s5;
708 fMit[4][1][0] = -c4 * s5;
710 fMit[4][0][1] = s4 * c5;
711 fMit[4][1][1] = -c4 * c5;
716 fMit[4][0][3] = c4 * this->
_long_56 + q1;
717 fMit[4][1][3] = s4 * this->
_long_56 + q2;
720 fMit[5][0][0] = s4 * s5 * c6 + c4 * s6;
721 fMit[5][1][0] = -c4 * s5 * c6 + s4 * s6;
722 fMit[5][2][0] = c5 * c6;
723 fMit[5][0][1] = -s4 * s5 * s6 + c4 * c6;
724 fMit[5][1][1] = c4 * s5 * s6 + s4 * c6;
725 fMit[5][2][1] = -c5 * s6;
726 fMit[5][0][2] = -s4 * c5;
727 fMit[5][1][2] = c4 * c5;
729 fMit[5][0][3] = c4 * this->
_long_56 + q1;
730 fMit[5][1][3] = s4 * this->
_long_56 + q2;
733 fMit[6][0][0] = fMit[5][0][0];
734 fMit[6][1][0] = fMit[5][1][0];
735 fMit[6][2][0] = fMit[5][2][0];
736 fMit[6][0][1] = fMit[5][0][1];
737 fMit[6][1][1] = fMit[5][1][1];
738 fMit[6][2][1] = fMit[5][2][1];
739 fMit[6][0][2] = fMit[5][0][2];
740 fMit[6][1][2] = fMit[5][1][2];
741 fMit[6][2][2] = fMit[5][2][2];
742 fMit[6][0][3] = fMit[5][0][3];
743 fMit[6][1][3] = fMit[5][1][3];
744 fMit[6][2][3] = fMit[5][2][3];
753 #if defined(WINRT_8_1)
754 WaitForSingleObjectEx(
mutex_fMi, INFINITE, FALSE);
756 WaitForSingleObject(
mutex_fMi, INFINITE);
758 for (
int i = 0; i < 8; i++)
761 #elif defined(VISP_HAVE_PTHREAD)
763 for (
int i = 0; i < 8; i++)
786 std::cout <<
"Change the control mode from velocity to position control.\n";
796 std::cout <<
"Change the control mode from stop to velocity control.\n";
885 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
887 "Cannot send a velocity to the robot "
888 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
893 double scale_sat = 1;
905 vpERROR_TRACE(
"The velocity vector must have a size of 6 !!!!");
909 for (
unsigned int i = 0; i < 3; ++i) {
910 vel_abs = fabs(vel[i]);
912 vel_trans_max = vel_abs;
918 vel_abs = fabs(vel[i + 3]);
920 vel_rot_max = vel_abs;
927 double scale_trans_sat = 1;
928 double scale_rot_sat = 1;
935 if ((scale_trans_sat < 1) || (scale_rot_sat < 1)) {
936 if (scale_trans_sat < scale_rot_sat)
937 scale_sat = scale_trans_sat;
939 scale_sat = scale_rot_sat;
947 vpERROR_TRACE(
"The velocity vector must have a size of 6 !!!!");
950 for (
unsigned int i = 0; i < 6; ++i) {
951 vel_abs = fabs(vel[i]);
953 vel_rot_max = vel_abs;
959 double scale_rot_sat = 1;
962 if (scale_rot_sat < 1)
963 scale_sat = scale_rot_sat;
968 "functionality not implemented");
972 "functionality not implemented");
1002 articularVelocity = eJe_ * eVc * velocityframe;
1012 articularVelocity = fJe_ * velocityframe;
1017 articularVelocity = velocityframe;
1030 for (
unsigned int i = 0; i < 6; ++i) {
1031 double vel_abs = fabs(articularVelocity[i]);
1033 vel_rot_max = vel_abs;
1036 articularVelocity[i], i + 1);
1039 double scale_rot_sat = 1;
1040 double scale_sat = 1;
1043 if (scale_rot_sat < 1)
1044 scale_sat = scale_rot_sat;
1114 vel = cVe * eJe_ * articularVelocity;
1118 vel = articularVelocity;
1124 vel = fJe_ * articularVelocity;
1133 "Case not taken in account.");
1234 double velmax = fabs(q[0]);
1235 for (
unsigned int i = 1; i < 6; i++) {
1236 if (velmax < fabs(q[i]))
1237 velmax = fabs(q[i]);
1322 "Modification of the robot state");
1337 for (
unsigned int i = 0; i < 3; i++) {
1352 qdes = articularCoordinates;
1356 error = qdes - articularCoordinates;
1357 errsqr = error.sumSquare();
1360 if (errsqr < 1e-4) {
1371 }
while (errsqr > 1e-8 && nbSol > 0);
1379 error = q - articularCoordinates;
1380 errsqr = error.sumSquare();
1384 if (errsqr < 1e-4) {
1391 }
while (errsqr > 1e-8);
1401 for (
unsigned int i = 0; i < 3; i++) {
1411 qdes = articularCoordinates;
1415 error = qdes - articularCoordinates;
1416 errsqr = error.sumSquare();
1419 if (errsqr < 1e-4) {
1428 }
while (errsqr > 1e-8 && nbSol > 0);
1432 vpERROR_TRACE(
"Positionning error. Mixt frame not implemented");
1434 "MIXT_FRAME not implemented.");
1437 vpERROR_TRACE(
"Positionning error. Mixt frame not implemented");
1439 "END_EFFECTOR_FRAME not implemented.");
1507 double pos3,
double pos4,
double pos5,
double pos6)
1659 for (
unsigned int i = 0; i < 3; i++) {
1668 "Mixt frame not implemented.");
1672 "End-effector frame not implemented.");
1730 for (
unsigned int j = 0; j < 3; j++) {
1731 position[j] = posRxyz[j];
1732 position[j + 3] = RtuVect[j];
1766 vpTRACE(
"Joint limit vector has not a size of 6 !");
1794 bool cond = fabs(q5 - M_PI / 2) < 1e-1;
1822 for (
unsigned int i = 0; i < 6; i++) {
1823 if (articularCoordinates[i] <=
_joint_min[i]) {
1824 difft =
_joint_min[i] - articularCoordinates[i];
1827 artNumb = -(int)i - 1;
1832 for (
unsigned int i = 0; i < 6; i++) {
1833 if (articularCoordinates[i] >=
_joint_max[i]) {
1834 difft = articularCoordinates[i] -
_joint_max[i];
1837 artNumb = (int)(i + 1);
1843 std::cout <<
"\nWarning: Velocity control stopped: axis " << fabs((
float)artNumb) <<
" on joint limit!"
1874 if (!first_time_getdis) {
1877 std::cout <<
"getDisplacement() CAMERA_FRAME not implemented\n";
1881 displacement = q_cur - q_prev_getdis;
1885 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
1889 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
1893 std::cout <<
"getDisplacement() END_EFFECTOR_FRAME not implemented\n";
1898 first_time_getdis =
false;
1902 q_prev_getdis = q_cur;
1954 std::ifstream fd(filename.c_str(), std::ios::in);
1956 if (!fd.is_open()) {
1961 std::string key(
"R:");
1962 std::string id(
"#AFMA6 - Position");
1963 bool pos_found =
false;
1968 while (std::getline(fd, line)) {
1971 if (!(line.compare(0,
id.size(),
id) == 0)) {
1972 std::cout <<
"Error: this position file " << filename <<
" is not for Afma6 robot" << std::endl;
1976 if ((line.compare(0, 1,
"#") == 0)) {
1979 if ((line.compare(0, key.size(), key) == 0)) {
1982 if (chain.size() <
njoint + 1)
1984 if (chain.size() <
njoint + 1)
1987 std::istringstream ss(line);
1990 for (
unsigned int i = 0; i <
njoint; i++)
2005 std::cout <<
"Error: unable to find a position for Afma6 robot in " << filename << std::endl;
2037 fd = fopen(filename.c_str(),
"w");
2042 #AFMA6 - Position - Version 2.01\n\
2045 # Joint position: X, Y, Z: translations in meters\n\
2046 # A, B, C: rotations in degrees\n\
2176 std::string scene_dir_;
2177 std::vector<std::string> scene_dirs =
vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(
";"));
2178 bool sceneDirExists =
false;
2179 for (
size_t i = 0; i < scene_dirs.size(); i++)
2181 scene_dir_ = scene_dirs[i];
2182 sceneDirExists =
true;
2185 if (!sceneDirExists) {
2188 std::cout <<
"The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
2190 std::cout <<
"Cannot get VISP_SCENES_DIR environment variable" << std::endl;
2194 unsigned int name_length = 30;
2195 if (scene_dir_.size() > FILENAME_MAX)
2197 unsigned int full_length = (
unsigned int)scene_dir_.size() + name_length;
2198 if (full_length > FILENAME_MAX)
2201 char *name_cam =
new char[full_length];
2203 strcpy(name_cam, scene_dir_.c_str());
2204 strcat(name_cam,
"/camera.bnd");
2207 if (arm_dir.size() > FILENAME_MAX)
2209 full_length = (
unsigned int)arm_dir.size() + name_length;
2210 if (full_length > FILENAME_MAX)
2213 char *name_arm =
new char[full_length];
2214 strcpy(name_arm, arm_dir.c_str());
2215 strcat(name_arm,
"/afma6_gate.bnd");
2216 std::cout <<
"name arm: " << name_arm << std::endl;
2218 strcpy(name_arm, arm_dir.c_str());
2219 strcat(name_arm,
"/afma6_arm1.bnd");
2220 set_scene(name_arm,
robotArms + 1, 1.0);
2221 strcpy(name_arm, arm_dir.c_str());
2222 strcat(name_arm,
"/afma6_arm2.bnd");
2223 set_scene(name_arm,
robotArms + 2, 1.0);
2224 strcpy(name_arm, arm_dir.c_str());
2225 strcat(name_arm,
"/afma6_arm3.bnd");
2226 set_scene(name_arm,
robotArms + 3, 1.0);
2227 strcpy(name_arm, arm_dir.c_str());
2228 strcat(name_arm,
"/afma6_arm4.bnd");
2229 set_scene(name_arm,
robotArms + 4, 1.0);
2233 strcpy(name_arm, arm_dir.c_str());
2236 strcat(name_arm,
"/afma6_tool_ccmop.bnd");
2240 strcat(name_arm,
"/afma6_tool_gripper.bnd");
2244 strcat(name_arm,
"/afma6_tool_vacuum.bnd");
2248 std::cout <<
"The custom tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
2252 std::cout <<
"The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
2256 set_scene(name_arm,
robotArms + 5, 1.0);
2258 add_rfstack(IS_BACK);
2260 add_vwstack(
"start",
"depth", 0.0, 100.0);
2261 add_vwstack(
"start",
"window", -0.1, 0.1, -0.1, 0.1);
2262 add_vwstack(
"start",
"type", PERSPECTIVE);
2274 bool changed =
false;
2278 if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
2298 float w44o[4][4], w44cext[4][4], x, y, z;
2302 add_vwstack(
"start",
"cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
2303 x = w44cext[2][0] + w44cext[3][0];
2304 y = w44cext[2][1] + w44cext[3][1];
2305 z = w44cext[2][2] + w44cext[3][2];
2306 add_vwstack(
"start",
"vrp", x, y, z);
2307 add_vwstack(
"start",
"vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
2308 add_vwstack(
"start",
"vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
2309 add_vwstack(
"start",
"window", -u, u, -v, v);
2317 vp2jlc_matrix(fMit[0], w44o);
2320 vp2jlc_matrix(fMit[2], w44o);
2323 vp2jlc_matrix(fMit[3], w44o);
2326 vp2jlc_matrix(fMit[4], w44o);
2329 vp2jlc_matrix(fMit[5], w44o);
2336 cMe = fMit[6] * cMe;
2337 vp2jlc_matrix(cMe, w44o);
2342 vp2jlc_matrix(
fMo, w44o);
2383 std::cout <<
"Used joint coordinates (rad): " << articularCoordinates.
t() << std::endl;
2413 fMo = fMit[7] * cMo_;
2442 const double lambda = 5.;
2446 unsigned int i, iter = 0;
2464 v = -lambda * cdRc.
t() * cdTc;
2465 w = -lambda * cdTUc;
2466 for (i = 0; i < 3; ++i) {
2470 err[i + 3] = cdTUc[i];
2489 #elif !defined(VISP_BUILD_SHARED_LIBS)
2492 void dummy_vpSimulatorAfma6(){};