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);
112 mutex_fMi = CreateMutex(NULL,FALSE,NULL);
119 DWORD dwThreadIdArray;
127 #elif defined(VISP_HAVE_PTHREAD)
134 pthread_attr_init(&attr);
135 pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
137 pthread_create(&thread, NULL,
launcher, (
void *)
this);
151 WaitForSingleObject(
hThread,INFINITE);
158 #elif defined(VISP_HAVE_PTHREAD)
159 pthread_attr_destroy(&attr);
160 pthread_join(thread, NULL);
170 for(
int i = 0; i < 6; i++)
191 arm_dir = VISP_ROBOT_ARMS_DIR;
195 std::cout <<
"The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
198 std::cout <<
"Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
214 reposPos[1] = -M_PI/2; reposPos[2] = M_PI; reposPos[4] = M_PI/2;
221 first_time_getdis =
true;
254 setExternalCameraPosition(
vpHomogeneousMatrix(0,0,0,0,0,
vpMath::rad(180))*
vpHomogeneousMatrix(-0.1,0,4,
vpMath::rad(90),0,0));
302 char name_arm[FILENAME_MAX];
303 strcpy(name_arm, arm_dir.c_str());
304 strcat(name_arm,
"/afma6_tool_ccmop.bnd");
324 char name_arm[FILENAME_MAX];
325 strcpy(name_arm, arm_dir.c_str());
326 strcat(name_arm,
"/afma6_tool_gripper.bnd");
346 char name_arm[FILENAME_MAX];
347 strcpy(name_arm, arm_dir.c_str());
348 strcat(name_arm,
"/afma6_tool_vacuum.bnd");
355 std::cout <<
"The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
378 const unsigned int &image_width,
379 const unsigned int &image_height)
389 if (image_width == 640 && image_height == 480)
391 std::cout <<
"Get default camera parameters for camera \""
396 vpTRACE(
"Cannot get default intrinsic camera parameters for this image resolution");
402 if (image_width == 640 && image_height == 480) {
403 std::cout <<
"Get default camera parameters for camera \""
408 vpTRACE(
"Cannot get default intrinsic camera parameters for this image resolution");
413 std::cout <<
"The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
474 double tcur_1 =
tcur;
487 double ellapsedTime = (
tcur -
tprev) * 1e-3;
500 articularVelocities = 0.0;
505 articularCoordinates[0] = articularCoordinates[0] + ellapsedTime*articularVelocities[0];
506 articularCoordinates[1] = articularCoordinates[1] + ellapsedTime*articularVelocities[1];
507 articularCoordinates[2] = articularCoordinates[2] + ellapsedTime*articularVelocities[2];
508 articularCoordinates[3] = articularCoordinates[3] + ellapsedTime*articularVelocities[3];
509 articularCoordinates[4] = articularCoordinates[4] + ellapsedTime*articularVelocities[4];
510 articularCoordinates[5] = articularCoordinates[5] + ellapsedTime*articularVelocities[5];
517 ellapsedTime = (
_joint_min[(
unsigned int)(-jl-1)] - articularCoordinates[(
unsigned int)(-jl-1)])/(articularVelocities[(
unsigned int)(-jl-1)]);
519 ellapsedTime = (
_joint_max[(
unsigned int)(jl-1)] - articularCoordinates[(
unsigned int)(jl-1)])/(articularVelocities[(
unsigned int)(jl-1)]);
521 for (
unsigned int i = 0; i < 6; i++)
522 articularCoordinates[i] = articularCoordinates[i] + ellapsedTime*articularVelocities[i];
564 for (
unsigned int k = 1; k < 7; k++)
670 fMit[4][0][0] = s4*s5;
671 fMit[4][1][0] = -c4*s5;
673 fMit[4][0][1] = s4*c5;
674 fMit[4][1][1] = -c4*c5;
679 fMit[4][0][3] = c4*this->
_long_56+q1;
680 fMit[4][1][3] = s4*this->
_long_56+q2;
683 fMit[5][0][0] = s4*s5*c6+c4*s6;
684 fMit[5][1][0] = -c4*s5*c6+s4*s6;
685 fMit[5][2][0] = c5*c6;
686 fMit[5][0][1] = -s4*s5*s6+c4*c6;
687 fMit[5][1][1] = c4*s5*s6+s4*c6;
688 fMit[5][2][1] = -c5*s6;
689 fMit[5][0][2] = -s4*c5;
690 fMit[5][1][2] = c4*c5;
692 fMit[5][0][3] = c4*this->
_long_56+q1;
693 fMit[5][1][3] = s4*this->
_long_56+q2;
696 fMit[6][0][0] = fMit[5][0][0];
697 fMit[6][1][0] = fMit[5][1][0];
698 fMit[6][2][0] = fMit[5][2][0];
699 fMit[6][0][1] = fMit[5][0][1];
700 fMit[6][1][1] = fMit[5][1][1];
701 fMit[6][2][1] = fMit[5][2][1];
702 fMit[6][0][2] = fMit[5][0][2];
703 fMit[6][1][2] = fMit[5][1][2];
704 fMit[6][2][2] = fMit[5][2][2];
705 fMit[6][0][3] = fMit[5][0][3];
706 fMit[6][1][3] = fMit[5][1][3];
707 fMit[6][2][3] = fMit[5][2][3];
717 for (
int i = 0; i < 8; i++)
720 #elif defined(VISP_HAVE_PTHREAD)
722 for (
int i = 0; i < 8; i++)
747 std::cout <<
"Change the control mode from velocity to position control.\n";
757 std::cout <<
"Change the control mode from stop to velocity control.\n";
843 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
845 "Cannot send a velocity to the robot "
846 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
851 double scale_trans_sat = 1;
852 double scale_rot_sat = 1;
853 double scale_sat = 1;
868 vpERROR_TRACE (
"The velocity vector must have a size of 6 !!!!");
872 for (
unsigned int i = 0 ; i < 3; ++ i)
874 vel_abs = fabs (vel[i]);
877 vel_trans_max = vel_abs;
879 "(axis nr. %d).", vel[i], i+1);
882 vel_abs = fabs (vel[i+3]);
884 vel_rot_max = vel_abs;
886 "(axis nr. %d).", vel[i+3], i+4);
896 if ( (scale_trans_sat < 1) || (scale_rot_sat < 1) )
898 if (scale_trans_sat < scale_rot_sat)
899 scale_sat = scale_trans_sat;
901 scale_sat = scale_rot_sat;
911 vpERROR_TRACE (
"The velocity vector must have a size of 6 !!!!");
914 for (
unsigned int i = 0 ; i < 6; ++ i)
916 vel_abs = fabs (vel[i]);
919 vel_rot_max = vel_abs;
921 "(axis nr. %d).", vel[i], i+1);
926 if ( scale_rot_sat < 1 )
927 scale_sat = scale_rot_sat;
950 double scale_rot_sat = 1;
951 double scale_sat = 1;
969 articularVelocity = eJe*eVc*velocityframe;
980 articularVelocity = fJe*velocityframe;
986 articularVelocity = velocityframe;
1003 for (
unsigned int i = 0 ; i < 6; ++ i)
1005 vel_abs = fabs (articularVelocity[i]);
1008 vel_rot_max = vel_abs;
1010 "(axis nr. %d).", articularVelocity[i], i+1);
1015 if ( scale_rot_sat < 1 )
1016 scale_sat = scale_rot_sat;
1091 vel = cVe*eJe*articularVelocity;
1096 vel = articularVelocity;
1103 vel = fJe*articularVelocity;
1113 "Case not taken in account.");
1175 double velmax = fabs(q[0]);
1176 for (
unsigned int i = 1; i < 6; i++)
1178 if (velmax < fabs(q[i]))
1179 velmax = fabs(q[i]);
1266 "Modification of the robot state");
1283 for (
unsigned int i=0; i < 3; i++)
1300 qdes = articularCoordinates;
1305 error = qdes - articularCoordinates;
1322 "Position out of range.");
1324 }
while (errsqr > 1e-8 && nbSol > 0);
1334 error = q - articularCoordinates;
1347 }
while (errsqr > 1e-8);
1358 for (
unsigned int i=0; i < 3; i++)
1370 qdes = articularCoordinates;
1375 error = qdes - articularCoordinates;
1390 }
while (errsqr > 1e-8 && nbSol > 0);
1395 vpERROR_TRACE (
"Positionning error. Mixt frame not implemented");
1397 "Positionning error: "
1398 "Mixt frame not implemented.");
1477 position[0] = pos1 ;
1478 position[1] = pos2 ;
1479 position[2] = pos3 ;
1480 position[3] = pos4 ;
1481 position[4] = pos5 ;
1482 position[5] = pos6 ;
1538 "Bad position in filename.");
1634 for (
unsigned int i=0; i <3; i++)
1644 vpERROR_TRACE (
"Positionning error. Mixt frame not implemented");
1646 "Positionning error: "
1647 "Mixt frame not implemented.");
1676 for(
unsigned int j=0;j<3;j++)
1678 position[j]=posRxyz[j];
1679 position[j+3]=RtuVect[j];
1694 vpTRACE(
"Joint limit vector has not a size of 6 !");
1724 bool cond = fabs(q5-M_PI/2) < 1e-1;
1754 for (
unsigned int i = 0; i < 6; i++)
1756 if (articularCoordinates[i] <=
_joint_min[i])
1758 difft =
_joint_min[i] - articularCoordinates[i];
1762 artNumb = -(int)i-1;
1767 for (
unsigned int i = 0; i < 6; i++)
1769 if (articularCoordinates[i] >=
_joint_max[i])
1771 difft = articularCoordinates[i] -
_joint_max[i];
1775 artNumb = (int)(i+1);
1781 std::cout <<
"\nWarning: Velocity control stopped: axis " << fabs((
float)artNumb) <<
" on joint limit!" <<std::endl;
1798 vpSimulatorAfma6::getCameraDisplacement(
vpColVector &displacement)
1813 vpSimulatorAfma6::getArticularDisplacement(
vpColVector &displacement)
1846 if ( ! first_time_getdis )
1852 std::cout <<
"getDisplacement() CAMERA_FRAME not implemented\n";
1859 displacement = q_cur - q_prev_getdis;
1865 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
1872 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
1880 first_time_getdis =
false;
1884 q_prev_getdis = q_cur;
1938 fd = fopen(filename,
"r") ;
1942 char line[FILENAME_MAX];
1943 char dummy[FILENAME_MAX];
1945 bool sortie =
false;
1949 if (fgets (line, FILENAME_MAX, fd) != NULL) {
1950 if ( strncmp (line,
"#", 1) != 0) {
1952 if ( strncmp (line, head,
sizeof(head)-1) == 0) {
1964 while ( sortie !=
true );
1968 sscanf(line,
"%s %lf %lf %lf %lf %lf %lf",
1970 &q[0], &q[1], &q[2],
1971 &q[3], &q[4], &q[5]);
2011 fd = fopen(filename,
"w") ;
2016 #AFMA6 - Position - Version 2.01\n\
2019 # Joint position: X, Y, Z: translations in meters\n\
2020 # A, B, C: rotations in degrees\n\
2025 fprintf(fd,
"R: %lf %lf %lf %lf %lf %lf\n",
2175 std::string scene_dir;
2177 scene_dir = VISP_SCENES_DIR;
2181 std::cout <<
"The simulator uses data from VISP_SCENES_DIR=" << scene_dir << std::endl;
2184 std::cout <<
"Cannot get VISP_SCENES_DIR environment variable" << std::endl;
2188 char name_cam[FILENAME_MAX];
2190 strcpy(name_cam, scene_dir.c_str());
2191 strcat(name_cam,
"/camera.bnd");
2194 char name_arm[FILENAME_MAX];
2195 strcpy(name_arm, arm_dir.c_str());
2196 strcat(name_arm,
"/afma6_gate.bnd");
2198 strcpy(name_arm, arm_dir.c_str());
2199 strcat(name_arm,
"/afma6_arm1.bnd");
2201 strcpy(name_arm, arm_dir.c_str());
2202 strcat(name_arm,
"/afma6_arm2.bnd");
2204 strcpy(name_arm, arm_dir.c_str());
2205 strcat(name_arm,
"/afma6_arm3.bnd");
2207 strcpy(name_arm, arm_dir.c_str());
2208 strcat(name_arm,
"/afma6_arm4.bnd");
2213 strcpy(name_arm, arm_dir.c_str());
2216 strcat(name_arm,
"/afma6_tool_ccmop.bnd");
2220 strcat(name_arm,
"/afma6_tool_gripper.bnd");
2224 strcat(name_arm,
"/afma6_tool_vacuum.bnd");
2228 std::cout <<
"The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
2234 add_rfstack(IS_BACK);
2236 add_vwstack (
"start",
"depth", 0.0, 100.0);
2237 add_vwstack (
"start",
"window", -0.1,0.1,-0.1,0.1);
2238 add_vwstack (
"start",
"type", PERSPECTIVE);
2249 bool changed =
false;
2253 if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
2276 float w44o[4][4],w44cext[4][4],x,y,z;
2280 add_vwstack (
"start",
"cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
2281 x = w44cext[2][0] + w44cext[3][0];
2282 y = w44cext[2][1] + w44cext[3][1];
2283 z = w44cext[2][2] + w44cext[3][2];
2284 add_vwstack (
"start",
"vrp", x,y,z);
2285 add_vwstack (
"start",
"vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
2286 add_vwstack (
"start",
"vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
2287 add_vwstack (
"start",
"window", -u, u, -v, v);
2295 vp2jlc_matrix(fMit[0],w44o);
2298 vp2jlc_matrix(fMit[2],w44o);
2301 vp2jlc_matrix(fMit[3],w44o);
2304 vp2jlc_matrix(fMit[4],w44o);
2307 vp2jlc_matrix(fMit[5],w44o);
2315 cMe = fMit[6] * cMe;
2316 vp2jlc_matrix(cMe,w44o);
2322 vp2jlc_matrix(
fMo,w44o);
2397 const double lambda = 5.;
2403 unsigned int i,iter=0;
2423 v = -lambda*cdRc.
t()*cdTc;
2430 err[i+3] = cdTUc[i];