44 #include <visp/vpConfig.h>
45 #if defined(WIN32) || defined(VISP_HAVE_PTHREAD)
46 #include <visp/vpSimulatorAfma6.h>
47 #include <visp/vpTime.h>
48 #include <visp/vpImagePoint.h>
49 #include <visp/vpPoint.h>
50 #include <visp/vpMeterPixelConversion.h>
51 #include <visp/vpRobotException.h>
52 #include <visp/vpIoTools.h>
77 DWORD dwThreadIdArray;
85 #elif defined (VISP_HAVE_PTHREAD)
92 pthread_attr_init(&attr);
93 pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
95 pthread_create(&thread, NULL,
launcher, (
void *)
this);
115 mutex_fMi = CreateMutex(NULL,FALSE,NULL);
122 DWORD dwThreadIdArray;
130 #elif defined(VISP_HAVE_PTHREAD)
137 pthread_attr_init(&attr);
138 pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
140 pthread_create(&thread, NULL,
launcher, (
void *)
this);
154 WaitForSingleObject(
hThread,INFINITE);
161 #elif defined(VISP_HAVE_PTHREAD)
162 pthread_attr_destroy(&attr);
163 pthread_join(thread, NULL);
173 for(
int i = 0; i < 6; i++)
194 arm_dir = VISP_ROBOT_ARMS_DIR;
198 std::cout <<
"The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
201 std::cout <<
"Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
217 reposPos[1] = -M_PI/2; reposPos[2] = M_PI; reposPos[4] = M_PI/2;
224 first_time_getdis =
true;
257 setExternalCameraPosition(
vpHomogeneousMatrix(0,0,0,0,0,
vpMath::rad(180))*
vpHomogeneousMatrix(-0.1,0,4,
vpMath::rad(90),0,0));
305 char name_arm[FILENAME_MAX];
306 strcpy(name_arm, arm_dir.c_str());
307 strcat(name_arm,
"/afma6_tool_ccmop.bnd");
327 char name_arm[FILENAME_MAX];
328 strcpy(name_arm, arm_dir.c_str());
329 strcat(name_arm,
"/afma6_tool_gripper.bnd");
349 char name_arm[FILENAME_MAX];
350 strcpy(name_arm, arm_dir.c_str());
351 strcat(name_arm,
"/afma6_tool_vacuum.bnd");
358 std::cout <<
"The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
381 const unsigned int &image_width,
382 const unsigned int &image_height)
392 if (image_width == 640 && image_height == 480)
394 std::cout <<
"Get default camera parameters for camera \""
399 vpTRACE(
"Cannot get default intrinsic camera parameters for this image resolution");
405 if (image_width == 640 && image_height == 480) {
406 std::cout <<
"Get default camera parameters for camera \""
411 vpTRACE(
"Cannot get default intrinsic camera parameters for this image resolution");
416 std::cout <<
"The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
477 double tcur_1 =
tcur;
490 double ellapsedTime = (
tcur -
tprev) * 1e-3;
509 articularVelocities = 0.0;
515 articularCoordinates[0] = articularCoordinates[0] + ellapsedTime*articularVelocities[0];
516 articularCoordinates[1] = articularCoordinates[1] + ellapsedTime*articularVelocities[1];
517 articularCoordinates[2] = articularCoordinates[2] + ellapsedTime*articularVelocities[2];
518 articularCoordinates[3] = articularCoordinates[3] + ellapsedTime*articularVelocities[3];
519 articularCoordinates[4] = articularCoordinates[4] + ellapsedTime*articularVelocities[4];
520 articularCoordinates[5] = articularCoordinates[5] + ellapsedTime*articularVelocities[5];
527 ellapsedTime = (
_joint_min[(
unsigned int)(-jl-1)] - articularCoordinates[(
unsigned int)(-jl-1)])/(articularVelocities[(
unsigned int)(-jl-1)]);
529 ellapsedTime = (
_joint_max[(
unsigned int)(jl-1)] - articularCoordinates[(
unsigned int)(jl-1)])/(articularVelocities[(
unsigned int)(jl-1)]);
531 for (
unsigned int i = 0; i < 6; i++)
532 articularCoordinates[i] = articularCoordinates[i] + ellapsedTime*articularVelocities[i];
574 for (
unsigned int k = 1; k < 7; k++)
680 fMit[4][0][0] = s4*s5;
681 fMit[4][1][0] = -c4*s5;
683 fMit[4][0][1] = s4*c5;
684 fMit[4][1][1] = -c4*c5;
689 fMit[4][0][3] = c4*this->
_long_56+q1;
690 fMit[4][1][3] = s4*this->
_long_56+q2;
693 fMit[5][0][0] = s4*s5*c6+c4*s6;
694 fMit[5][1][0] = -c4*s5*c6+s4*s6;
695 fMit[5][2][0] = c5*c6;
696 fMit[5][0][1] = -s4*s5*s6+c4*c6;
697 fMit[5][1][1] = c4*s5*s6+s4*c6;
698 fMit[5][2][1] = -c5*s6;
699 fMit[5][0][2] = -s4*c5;
700 fMit[5][1][2] = c4*c5;
702 fMit[5][0][3] = c4*this->
_long_56+q1;
703 fMit[5][1][3] = s4*this->
_long_56+q2;
706 fMit[6][0][0] = fMit[5][0][0];
707 fMit[6][1][0] = fMit[5][1][0];
708 fMit[6][2][0] = fMit[5][2][0];
709 fMit[6][0][1] = fMit[5][0][1];
710 fMit[6][1][1] = fMit[5][1][1];
711 fMit[6][2][1] = fMit[5][2][1];
712 fMit[6][0][2] = fMit[5][0][2];
713 fMit[6][1][2] = fMit[5][1][2];
714 fMit[6][2][2] = fMit[5][2][2];
715 fMit[6][0][3] = fMit[5][0][3];
716 fMit[6][1][3] = fMit[5][1][3];
717 fMit[6][2][3] = fMit[5][2][3];
727 for (
int i = 0; i < 8; i++)
730 #elif defined(VISP_HAVE_PTHREAD)
732 for (
int i = 0; i < 8; i++)
757 std::cout <<
"Change the control mode from velocity to position control.\n";
767 std::cout <<
"Change the control mode from stop to velocity control.\n";
853 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
855 "Cannot send a velocity to the robot "
856 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
861 double scale_trans_sat = 1;
862 double scale_rot_sat = 1;
863 double scale_sat = 1;
878 vpERROR_TRACE (
"The velocity vector must have a size of 6 !!!!");
882 for (
unsigned int i = 0 ; i < 3; ++ i)
884 vel_abs = fabs (vel[i]);
887 vel_trans_max = vel_abs;
889 "(axis nr. %d).", vel[i], i+1);
892 vel_abs = fabs (vel[i+3]);
894 vel_rot_max = vel_abs;
896 "(axis nr. %d).", vel[i+3], i+4);
906 if ( (scale_trans_sat < 1) || (scale_rot_sat < 1) )
908 if (scale_trans_sat < scale_rot_sat)
909 scale_sat = scale_trans_sat;
911 scale_sat = scale_rot_sat;
921 vpERROR_TRACE (
"The velocity vector must have a size of 6 !!!!");
924 for (
unsigned int i = 0 ; i < 6; ++ i)
926 vel_abs = fabs (vel[i]);
929 vel_rot_max = vel_abs;
931 "(axis nr. %d).", vel[i], i+1);
936 if ( scale_rot_sat < 1 )
937 scale_sat = scale_rot_sat;
960 double scale_rot_sat = 1;
961 double scale_sat = 1;
979 articularVelocity = eJe*eVc*velocityframe;
990 articularVelocity = fJe*velocityframe;
996 articularVelocity = velocityframe;
1013 for (
unsigned int i = 0 ; i < 6; ++ i)
1015 vel_abs = fabs (articularVelocity[i]);
1018 vel_rot_max = vel_abs;
1020 "(axis nr. %d).", articularVelocity[i], i+1);
1025 if ( scale_rot_sat < 1 )
1026 scale_sat = scale_rot_sat;
1101 vel = cVe*eJe*articularVelocity;
1106 vel = articularVelocity;
1113 vel = fJe*articularVelocity;
1123 "Case not taken in account.");
1185 double velmax = fabs(q[0]);
1186 for (
unsigned int i = 1; i < 6; i++)
1188 if (velmax < fabs(q[i]))
1189 velmax = fabs(q[i]);
1276 "Modification of the robot state");
1293 for (
unsigned int i=0; i < 3; i++)
1310 qdes = articularCoordinates;
1315 error = qdes - articularCoordinates;
1332 "Position out of range.");
1334 }
while (errsqr > 1e-8 && nbSol > 0);
1344 error = q - articularCoordinates;
1357 }
while (errsqr > 1e-8);
1368 for (
unsigned int i=0; i < 3; i++)
1380 qdes = articularCoordinates;
1385 error = qdes - articularCoordinates;
1400 }
while (errsqr > 1e-8 && nbSol > 0);
1405 vpERROR_TRACE (
"Positionning error. Mixt frame not implemented");
1407 "Positionning error: "
1408 "Mixt frame not implemented.");
1487 position[0] = pos1 ;
1488 position[1] = pos2 ;
1489 position[2] = pos3 ;
1490 position[3] = pos4 ;
1491 position[4] = pos5 ;
1492 position[5] = pos6 ;
1548 "Bad position in filename.");
1644 for (
unsigned int i=0; i <3; i++)
1654 vpERROR_TRACE (
"Positionning error. Mixt frame not implemented");
1656 "Positionning error: "
1657 "Mixt frame not implemented.");
1686 for(
unsigned int j=0;j<3;j++)
1688 position[j]=posRxyz[j];
1689 position[j+3]=RtuVect[j];
1704 vpTRACE(
"Joint limit vector has not a size of 6 !");
1734 bool cond = fabs(q5-M_PI/2) < 1e-1;
1764 for (
unsigned int i = 0; i < 6; i++)
1766 if (articularCoordinates[i] <=
_joint_min[i])
1768 difft =
_joint_min[i] - articularCoordinates[i];
1772 artNumb = -(int)i-1;
1777 for (
unsigned int i = 0; i < 6; i++)
1779 if (articularCoordinates[i] >=
_joint_max[i])
1781 difft = articularCoordinates[i] -
_joint_max[i];
1785 artNumb = (int)(i+1);
1791 std::cout <<
"\nWarning: Velocity control stopped: axis " << fabs((
float)artNumb) <<
" on joint limit!" <<std::endl;
1808 vpSimulatorAfma6::getCameraDisplacement(
vpColVector &displacement)
1823 vpSimulatorAfma6::getArticularDisplacement(
vpColVector &displacement)
1856 if ( ! first_time_getdis )
1862 std::cout <<
"getDisplacement() CAMERA_FRAME not implemented\n";
1869 displacement = q_cur - q_prev_getdis;
1875 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
1882 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
1890 first_time_getdis =
false;
1894 q_prev_getdis = q_cur;
1948 fd = fopen(filename,
"r") ;
1952 char line[FILENAME_MAX];
1953 char dummy[FILENAME_MAX];
1955 bool sortie =
false;
1959 if (fgets (line, FILENAME_MAX, fd) != NULL) {
1960 if ( strncmp (line,
"#", 1) != 0) {
1962 if ( strncmp (line, head,
sizeof(head)-1) == 0) {
1974 while ( sortie !=
true );
1978 sscanf(line,
"%s %lf %lf %lf %lf %lf %lf",
1980 &q[0], &q[1], &q[2],
1981 &q[3], &q[4], &q[5]);
2021 fd = fopen(filename,
"w") ;
2026 #AFMA6 - Position - Version 2.01\n\
2029 # Joint position: X, Y, Z: translations in meters\n\
2030 # A, B, C: rotations in degrees\n\
2035 fprintf(fd,
"R: %lf %lf %lf %lf %lf %lf\n",
2185 std::string scene_dir;
2187 scene_dir = VISP_SCENES_DIR;
2191 std::cout <<
"The simulator uses data from VISP_SCENES_DIR=" << scene_dir << std::endl;
2194 std::cout <<
"Cannot get VISP_SCENES_DIR environment variable" << std::endl;
2198 char name_cam[FILENAME_MAX];
2200 strcpy(name_cam, scene_dir.c_str());
2201 strcat(name_cam,
"/camera.bnd");
2204 char name_arm[FILENAME_MAX];
2205 strcpy(name_arm, arm_dir.c_str());
2206 strcat(name_arm,
"/afma6_gate.bnd");
2208 strcpy(name_arm, arm_dir.c_str());
2209 strcat(name_arm,
"/afma6_arm1.bnd");
2211 strcpy(name_arm, arm_dir.c_str());
2212 strcat(name_arm,
"/afma6_arm2.bnd");
2214 strcpy(name_arm, arm_dir.c_str());
2215 strcat(name_arm,
"/afma6_arm3.bnd");
2217 strcpy(name_arm, arm_dir.c_str());
2218 strcat(name_arm,
"/afma6_arm4.bnd");
2223 strcpy(name_arm, arm_dir.c_str());
2226 strcat(name_arm,
"/afma6_tool_ccmop.bnd");
2230 strcat(name_arm,
"/afma6_tool_gripper.bnd");
2234 strcat(name_arm,
"/afma6_tool_vacuum.bnd");
2238 std::cout <<
"The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
2244 add_rfstack(IS_BACK);
2246 add_vwstack (
"start",
"depth", 0.0, 100.0);
2247 add_vwstack (
"start",
"window", -0.1,0.1,-0.1,0.1);
2248 add_vwstack (
"start",
"type", PERSPECTIVE);
2259 bool changed =
false;
2263 if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
2286 float w44o[4][4],w44cext[4][4],x,y,z;
2290 add_vwstack (
"start",
"cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
2291 x = w44cext[2][0] + w44cext[3][0];
2292 y = w44cext[2][1] + w44cext[3][1];
2293 z = w44cext[2][2] + w44cext[3][2];
2294 add_vwstack (
"start",
"vrp", x,y,z);
2295 add_vwstack (
"start",
"vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
2296 add_vwstack (
"start",
"vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
2297 add_vwstack (
"start",
"window", -u, u, -v, v);
2305 vp2jlc_matrix(fMit[0],w44o);
2308 vp2jlc_matrix(fMit[2],w44o);
2311 vp2jlc_matrix(fMit[3],w44o);
2314 vp2jlc_matrix(fMit[4],w44o);
2317 vp2jlc_matrix(fMit[5],w44o);
2325 cMe = fMit[6] * cMe;
2326 vp2jlc_matrix(cMe,w44o);
2332 vp2jlc_matrix(
fMo,w44o);
2373 std::cout <<
"Used joint coordinates (rad): " << articularCoordinates.
t() << std::endl;
2429 const double lambda = 5.;
2435 unsigned int i,iter=0;
2455 v = -lambda*cdRc.
t()*cdTc;
2462 err[i+3] = cdTUc[i];