40 #include <visp3/robot/vpSimulatorViper850.h> 42 #if defined(VISP_HAVE_MODULE_GUI) && ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD)) 44 #include <visp3/core/vpTime.h> 45 #include <visp3/core/vpImagePoint.h> 46 #include <visp3/core/vpPoint.h> 47 #include <visp3/core/vpMeterPixelConversion.h> 48 #include <visp3/core/vpIoTools.h> 53 #include "../wireframe-simulator/vpBound.h" 54 #include "../wireframe-simulator/vpVwstack.h" 55 #include "../wireframe-simulator/vpRfstack.h" 56 #include "../wireframe-simulator/vpScene.h" 65 q_prev_getdis(), first_time_getdis(true), positioningVelocity(defaultPositioningVelocity),
66 zeroPos(), reposPos(), toolCustom(false), arm_dir()
75 mutex_fMi = CreateMutexEx(NULL, NULL, 0, NULL);
81 mutex_fMi = CreateMutex(NULL, FALSE, NULL);
89 DWORD dwThreadIdArray;
90 hThread = CreateThread(
97 #elif defined (VISP_HAVE_PTHREAD) 104 pthread_attr_init(&
attr);
105 pthread_attr_setdetachstate(&
attr, PTHREAD_CREATE_JOINABLE);
122 zeroPos(), reposPos(), toolCustom(false), arm_dir()
131 mutex_fMi = CreateMutexEx(NULL, NULL, 0, NULL);
137 mutex_fMi = CreateMutex(NULL,FALSE,NULL);
144 DWORD dwThreadIdArray;
145 hThread = CreateThread(
152 #elif defined(VISP_HAVE_PTHREAD) 159 pthread_attr_init(&
attr);
160 pthread_attr_setdetachstate(&
attr, PTHREAD_CREATE_JOINABLE);
176 # if defined(WINRT_8_1) 177 WaitForSingleObjectEx(hThread, INFINITE, FALSE);
179 WaitForSingleObject(hThread, INFINITE);
181 CloseHandle(hThread);
187 #elif defined(VISP_HAVE_PTHREAD) 188 pthread_attr_destroy(&
attr);
189 pthread_join(
thread, NULL);
200 for(
int i = 0; i < 6; i++)
221 std::vector<std::string> arm_dirs =
vpIoTools::splitChain(std::string(VISP_ROBOT_ARMS_DIR), std::string(
";"));
222 bool armDirExists =
false;
223 for(
size_t i=0; i < arm_dirs.size(); i++)
225 arm_dir = arm_dirs[i];
229 if (! armDirExists) {
232 std::cout <<
"The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
235 std::cout <<
"Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
249 zeroPos[1] = -M_PI/2; zeroPos[2] = M_PI;
252 reposPos[1] = -M_PI/2; reposPos[2] = M_PI; reposPos[4] = M_PI/2;
259 first_time_getdis =
true;
341 etc[0] = -0.04558630174;
342 etc[1] = -0.00134326752;
343 etc[2] = 0.001000828017;
351 std::cout <<
"This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
375 const unsigned int &image_width,
376 const unsigned int &image_height)
386 if (image_width == 640 && image_height == 480)
388 std::cout <<
"Get default camera parameters for camera \"" 393 vpTRACE(
"Cannot get default intrinsic camera parameters for this image resolution");
399 if (image_width == 640 && image_height == 480) {
400 std::cout <<
"Get default camera parameters for camera \"" 405 vpTRACE(
"Cannot get default intrinsic camera parameters for this image resolution");
412 std::cout <<
"This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
470 double tcur_1 =
tcur;
483 double ellapsedTime = (
tcur -
tprev) * 1e-3;
499 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];
563 for (
int k = 1; k < 7; k++)
616 double c23 = cos(q2+q3);
617 double s23 = sin(q2+q3);
634 fMit[0][0][3] =
a1*c1;
635 fMit[0][1][3] =
a1*s1;
638 fMit[1][0][0] = c1*c2;
639 fMit[1][1][0] = s1*c2;
641 fMit[1][0][1] = -c1*s2;
642 fMit[1][1][1] = -s1*s2;
647 fMit[1][0][3] = c1*(
a2*c2+
a1);
648 fMit[1][1][3] = s1*(
a2*c2+
a1);
649 fMit[1][2][3] =
d1-
a2*s2;
651 double quickcomp1 =
a3*c23-
a2*c2-
a1;
653 fMit[2][0][0] = -c1*c23;
654 fMit[2][1][0] = -s1*c23;
659 fMit[2][0][2] = c1*s23;
660 fMit[2][1][2] = s1*s23;
662 fMit[2][0][3] = -c1*quickcomp1;
663 fMit[2][1][3] = -s1*quickcomp1;
664 fMit[2][2][3] =
a3*s23-
a2*s2+
d1;
666 double quickcomp2 = c1*(s23*
d4 - quickcomp1);
667 double quickcomp3 = s1*(s23*
d4 - quickcomp1);
669 fMit[3][0][0] = -c1*c23*c4+s1*s4;
670 fMit[3][1][0] = -s1*c23*c4-c1*s4;
671 fMit[3][2][0] = s23*c4;
672 fMit[3][0][1] = c1*s23;
673 fMit[3][1][1] = s1*s23;
675 fMit[3][0][2] = -c1*c23*s4-s1*c4;
676 fMit[3][1][2] = -s1*c23*s4+c1*c4;
677 fMit[3][2][2] = s23*s4;
678 fMit[3][0][3] = quickcomp2;
679 fMit[3][1][3] = quickcomp3;
680 fMit[3][2][3] = c23*
d4+
a3*s23-
a2*s2+
d1;
682 fMit[4][0][0] = c1*(s23*s5-c5*c23*c4)+s1*c5*s4;
683 fMit[4][1][0] = s1*(s23*s5-c5*c23*c4)-c1*c5*s4;
684 fMit[4][2][0] = s23*c4*c5+c23*s5;
685 fMit[4][0][1] = c1*c23*s4+s1*c4;
686 fMit[4][1][1] = s1*c23*s4-c1*c4;
687 fMit[4][2][1] = -s23*s4;
688 fMit[4][0][2] = c1*(s23*c5+s5*c23*c4)-s1*s5*s4;
689 fMit[4][1][2] = s1*(s23*c5+s5*c23*c4)+c1*s5*s4;
690 fMit[4][2][2] = -s23*c4*s5+c23*c5;
691 fMit[4][0][3] = quickcomp2;
692 fMit[4][1][3] = quickcomp3;
693 fMit[4][2][3] = c23*
d4+
a3*s23-
a2*s2+
d1;
695 fMit[5][0][0] = c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-s1*(s4*c5*c6+c4*s6);
696 fMit[5][1][0] = -s1*(c23*(-c4*c5*c6+s4*s6)+s23*s5*c6)+c1*(s4*c5*c6+c4*s6);
697 fMit[5][2][0] = s23*(s4*s6-c4*c5*c6)-c23*s5*c6;
698 fMit[5][0][1] = -c1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)+s1*(s4*c5*s6-c4*c6);
699 fMit[5][1][1] = -s1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)-c1*(s4*c5*s6-c4*c6);
700 fMit[5][2][1] = s23*(c4*c5*s6+s4*c6)+c23*s5*s6;
701 fMit[5][0][2] = c1*(c23*c4*s5+s23*c5)-s1*s4*s5;
702 fMit[5][1][2] = s1*(c23*c4*s5+s23*c5)+c1*s4*s5;
703 fMit[5][2][2] = -s23*c4*s5+c23*c5;
704 fMit[5][0][3] = quickcomp2;
705 fMit[5][1][3] = quickcomp3;
706 fMit[5][2][3] = s23*
a3+c23*
d4-
a2*s2+
d1;
708 fMit[6][0][0] = c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-s1*(s4*c5*c6+c4*s6);
709 fMit[6][1][0] = -s1*(c23*(-c4*c5*c6+s4*s6)+s23*s5*c6)+c1*(s4*c5*c6+c4*s6);
710 fMit[6][2][0] = s23*(s4*s6-c4*c5*c6)-c23*s5*c6;
711 fMit[6][0][1] = -c1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)+s1*(s4*c5*s6-c4*c6);
712 fMit[6][1][1] = -s1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)-c1*(s4*c5*s6-c4*c6);
713 fMit[6][2][1] = s23*(c4*c5*s6+s4*c6)+c23*s5*s6;
714 fMit[6][0][2] = c1*(c23*c4*s5+s23*c5)-s1*s4*s5;
715 fMit[6][1][2] = s1*(c23*c4*s5+s23*c5)+c1*s4*s5;
716 fMit[6][2][2] = -s23*c4*s5+c23*c5;
717 fMit[6][0][3] = c1*(c23*(c4*s5*
d6-
a3)+s23*(c5*
d6+
d4)+a1+
a2*c2)-s1*s4*s5*
d6;
718 fMit[6][1][3] = s1*(c23*(c4*s5*
d6-
a3)+s23*(c5*
d6+
d4)+a1+
a2*c2)+c1*s4*s5*
d6;
719 fMit[6][2][3] = s23*(
a3-c4*s5*
d6)+c23*(c5*
d6+
d4)-
a2*s2+
d1;
728 # if defined(WINRT_8_1) 729 WaitForSingleObjectEx(
mutex_fMi, INFINITE, FALSE);
731 WaitForSingleObject(
mutex_fMi, INFINITE);
733 for (
int i = 0; i < 8; i++)
736 #elif defined(VISP_HAVE_PTHREAD) 738 for (
int i = 0; i < 8; i++)
763 std::cout <<
"Change the control mode from velocity to position control.\n";
773 std::cout <<
"Change the control mode from stop to velocity control.\n";
859 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
861 "Cannot send a velocity to the robot " 862 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
867 double scale_sat = 1;
882 vpERROR_TRACE (
"The velocity vector must have a size of 6 !!!!");
886 for (
unsigned int i = 0 ; i < 3; ++ i)
888 vel_abs = fabs (vel[i]);
891 vel_trans_max = vel_abs;
893 "(axis nr. %d).", vel[i], i+1);
896 vel_abs = fabs (vel[i+3]);
898 vel_rot_max = vel_abs;
900 "(axis nr. %d).", vel[i+3], i+4);
904 double scale_trans_sat = 1;
905 double scale_rot_sat = 1;
912 if ( (scale_trans_sat < 1) || (scale_rot_sat < 1) )
914 if (scale_trans_sat < scale_rot_sat)
915 scale_sat = scale_trans_sat;
917 scale_sat = scale_rot_sat;
927 vpERROR_TRACE (
"The velocity vector must have a size of 6 !!!!");
930 for (
unsigned int i = 0 ; i < 6; ++ i)
932 vel_abs = fabs (vel[i]);
935 vel_rot_max = vel_abs;
937 "(axis nr. %d).", vel[i], i+1);
940 double scale_rot_sat = 1;
943 if ( scale_rot_sat < 1 )
944 scale_sat = scale_rot_sat;
983 articularVelocity = eJe_*eVc*velocityframe;
994 articularVelocity = fJe_*velocityframe;
1000 articularVelocity = velocityframe;
1015 for (
unsigned int i = 0 ; i < 6; ++ i)
1017 double vel_abs = fabs (articularVelocity[i]);
1020 vel_rot_max = vel_abs;
1022 "(axis nr. %d).", articularVelocity[i], i+1);
1025 double scale_rot_sat = 1;
1026 double scale_sat = 1;
1030 if ( scale_rot_sat < 1 )
1031 scale_sat = scale_rot_sat;
1106 vel = cVe*eJe_*articularVelocity;
1111 vel = articularVelocity;
1118 vel = fJe_*articularVelocity;
1128 "Case not taken in account.");
1234 double velmax = fabs(q[0]);
1235 for (
unsigned int i = 1; i < 6; i++)
1237 if (velmax < fabs(q[i]))
1238 velmax = fabs(q[i]);
1325 "Modification of the robot state");
1342 for (
unsigned int i=0; i < 3; i++)
1359 qdes = articularCoordinates;
1364 error = qdes - articularCoordinates;
1381 "Position out of range.");
1383 }
while (errsqr > 1e-8 && nbSol > 0);
1393 error = q - articularCoordinates;
1406 }
while (errsqr > 1e-8);
1417 for (
unsigned int i=0; i < 3; i++)
1429 qdes = articularCoordinates;
1433 error = qdes - articularCoordinates;
1449 }
while (errsqr > 1e-8 && nbSol > 0);
1454 vpERROR_TRACE (
"Positionning error. Mixt frame not implemented");
1456 "Positionning error: " 1457 "Mixt frame not implemented.");
1535 position[0] = pos1 ;
1536 position[1] = pos2 ;
1537 position[2] = pos3 ;
1538 position[3] = pos4 ;
1539 position[4] = pos5 ;
1540 position[5] = pos6 ;
1596 "Bad position in filename.");
1692 for (
unsigned int i=0; i <3; i++)
1702 vpERROR_TRACE (
"Positionning error. Mixt frame not implemented");
1704 "Positionning error: " 1705 "Mixt frame not implemented.");
1766 for(
unsigned int j=0;j<3;j++)
1768 position[j]=posRxyz[j];
1769 position[j+3]=RtuVect[j];
1802 vpTRACE(
"Joint limit vector has not a size of 6 !");
1834 double c2 = cos(q2);
1835 double c3 = cos(q3);
1836 double s3 = sin(q3);
1837 double c23 = cos(q2+q3);
1838 double s23 = sin(q2+q3);
1839 double s5 = sin(q5);
1841 bool cond1 = fabs(s5) < 1e-1;
1842 bool cond2 = fabs(
a3*s3+c3*
d4) < 1e-1;
1843 bool cond3 = fabs(
a2*c2-
a3*c23+s23*d4+
a1) < 1e-1;
1864 J[1][0] = 0; J[2][0] = 0; J[3][0] = 0; J[4][0] = 0; J[5][0] = 0;
1865 J[1][1] = 0; J[2][1] = 0; J[3][1] = 0; J[4][1] = 0; J[5][1] = 0;
1866 J[1][2] = 0; J[2][2] = 0; J[3][2] = 0; J[4][2] = 0; J[5][2] = 0;
1872 J[0][0] = 0; J[3][0] = 0; J[4][0] = 0; J[5][0] = 0;
1873 J[0][1] = 0; J[3][1] = 0; J[4][1] = 0; J[5][1] = 0;
1891 for (
unsigned int i = 0; i < 6; i++)
1893 if (articularCoordinates[i] <=
joint_min[i])
1895 difft =
joint_min[i] - articularCoordinates[i];
1899 artNumb = -(int)i-1;
1904 for (
unsigned int i = 0; i < 6; i++)
1906 if (articularCoordinates[i] >=
joint_max[i])
1908 difft = articularCoordinates[i] -
joint_max[i];
1912 artNumb = (int)(i+1);
1918 std::cout <<
"\nWarning: Velocity control stopped: axis " << fabs((
float)artNumb) <<
" on joint limit!" <<std::endl;
1935 vpSimulatorViper850::getCameraDisplacement(
vpColVector &displacement)
1950 vpSimulatorViper850::getArticularDisplacement(
vpColVector &displacement)
1983 if ( ! first_time_getdis )
1989 std::cout <<
"getDisplacement() CAMERA_FRAME not implemented\n";
1995 displacement = q_cur - q_prev_getdis;
2001 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
2007 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
2014 first_time_getdis =
false;
2018 q_prev_getdis = q_cur;
2085 std::ifstream fd(filename.c_str(), std::ios::in);
2087 if(! fd.is_open()) {
2092 std::string key(
"R:");
2093 std::string id(
"#Viper850 - Position");
2094 bool pos_found =
false;
2099 while(std::getline(fd, line)) {
2102 if(! (line.compare(0,
id.size(), id) == 0)) {
2103 std::cout <<
"Error: this position file " << filename <<
" is not for Viper850 robot" << std::endl;
2107 if((line.compare(0, 1,
"#") == 0)) {
2110 if((line.compare(0, key.size(), key) == 0)) {
2113 if (chain.size() <
njoint+1)
2115 if(chain.size() <
njoint+1)
2118 std::istringstream ss(line);
2121 for (
unsigned int i=0; i<
njoint; i++)
2134 std::cout <<
"Error: unable to find a position for Viper850 robot in " << filename << std::endl;
2167 fd = fopen(filename.c_str(),
"w") ;
2172 #Viper - Position - Version 1.0\n\ 2175 # Joint position in degrees\n\ 2180 fprintf(fd,
"R: %lf %lf %lf %lf %lf %lf\n",
2331 std::string scene_dir_;
2332 std::vector<std::string> scene_dirs =
vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(
";"));
2333 bool sceneDirExists =
false;
2334 for(
size_t i=0; i < scene_dirs.size(); i++)
2336 scene_dir_ = scene_dirs[i];
2337 sceneDirExists =
true;
2340 if (! sceneDirExists) {
2343 std::cout <<
"The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
2346 std::cout <<
"Cannot get VISP_SCENES_DIR environment variable" << std::endl;
2350 unsigned int name_length = 30;
2351 if (scene_dir_.size() > FILENAME_MAX)
2354 unsigned int full_length = (
unsigned int)scene_dir_.size() + name_length;
2355 if (full_length > FILENAME_MAX)
2358 char *name_cam =
new char [full_length];
2360 strcpy(name_cam, scene_dir_.c_str());
2361 strcat(name_cam,
"/camera.bnd");
2364 if (arm_dir.size() > FILENAME_MAX)
2366 full_length = (
unsigned int)arm_dir.size() + name_length;
2367 if (full_length > FILENAME_MAX)
2370 char *name_arm =
new char [full_length];
2371 strcpy(name_arm, arm_dir.c_str());
2372 strcat(name_arm,
"/viper850_arm1.bnd");
2374 strcpy(name_arm, arm_dir.c_str());
2375 strcat(name_arm,
"/viper850_arm2.bnd");
2377 strcpy(name_arm, arm_dir.c_str());
2378 strcat(name_arm,
"/viper850_arm3.bnd");
2380 strcpy(name_arm, arm_dir.c_str());
2381 strcat(name_arm,
"/viper850_arm4.bnd");
2383 strcpy(name_arm, arm_dir.c_str());
2384 strcat(name_arm,
"/viper850_arm5.bnd");
2386 strcpy(name_arm, arm_dir.c_str());
2387 strcat(name_arm,
"/viper850_arm6.bnd");
2396 add_rfstack(IS_BACK);
2398 add_vwstack (
"start",
"depth", 0.0, 100.0);
2399 add_vwstack (
"start",
"window", -0.1,0.1,-0.1,0.1);
2400 add_vwstack (
"start",
"type", PERSPECTIVE);
2414 bool changed =
false;
2418 if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
2441 float w44o[4][4],w44cext[4][4],x,y,z;
2445 add_vwstack (
"start",
"cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
2446 x = w44cext[2][0] + w44cext[3][0];
2447 y = w44cext[2][1] + w44cext[3][1];
2448 z = w44cext[2][2] + w44cext[3][2];
2449 add_vwstack (
"start",
"vrp", x,y,z);
2450 add_vwstack (
"start",
"vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
2451 add_vwstack (
"start",
"vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
2452 add_vwstack (
"start",
"window", -u, u, -v, v);
2460 vp2jlc_matrix(fMit[0],w44o);
2463 vp2jlc_matrix(fMit[1],w44o);
2466 vp2jlc_matrix(fMit[2],w44o);
2469 vp2jlc_matrix(fMit[3],w44o);
2472 vp2jlc_matrix(fMit[6],w44o);
2480 cMe = fMit[6] * cMe;
2481 vp2jlc_matrix(cMe,w44o);
2487 vp2jlc_matrix(
fMo,w44o);
2527 std::cout <<
"Used joint coordinates (rad): " << articularCoordinates.
t() << std::endl;
2557 fMo = fMit[7] * cMo_;
2560 #elif !defined(VISP_BUILD_SHARED_LIBS) 2562 void dummy_vpSimulatorViper850() {};
static void displayCamera(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness)
Implementation of a matrix and operations on matrices.
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Compute the pseudo inverse of the matrix using the SVD.
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
VISP_EXPORT int wait(double t0, double t)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
bool singularityTest(const vpColVector q, vpMatrix &J)
vpHomogeneousMatrix eMc
End effector to camera transformation.
Error that can be emited by the vpRobot class and its derivates.
vpColVector get_artCoord()
virtual ~vpSimulatorViper850()
vpHomogeneousMatrix getExternalCameraPosition() const
void get_fMi(vpHomogeneousMatrix *fMit)
void computeArticularVelocity()
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
static const vpToolType defaultTool
Default tool attached to the robot end effector.
void get_cMe(vpHomogeneousMatrix &cMe)
Implementation of an homogeneous matrix and operations on such kind of matrices.
unsigned int jointLimitArt
static void * launcher(void *arg)
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Point coordinates conversion from normalized coordinates in meter to pixel coordinates ...
VISP_EXPORT double measureTimeSecond()
double getMaxTranslationVelocity(void) const
void getExternalImage(vpImage< vpRGBa > &I)
bool constantSamplingTimeMode
Flag used to force the sampling time in the thread computing the robot's displacement to a constant v...
bool singularityManagement
static const vpColor none
Initialize the position controller.
error that can be emited by ViSP classes.
unsigned int getRows() const
Return the number of rows of the 2D array.
void track(const vpHomogeneousMatrix &cMo)
vpHomogeneousMatrix inverse() const
void get_fJe(vpMatrix &fJe)
vpColVector get_velocity()
static const char *const CONST_MARLIN_F033C_CAMERA_NAME
double getMaxRotationVelocity(void) const
void extract(vpRotationMatrix &R) const
static const vpColor green
static void flush(const vpImage< unsigned char > &I)
static bool savePosFile(const std::string &filename, const vpColVector &q)
VISP_EXPORT double measureTimeMs()
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Class that defines what is a point.
static Type maximum(const Type &a, const Type &b)
Implementation of a rotation matrix and operations on such kind of matrices.
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
pthread_mutex_t mutex_artCoord
pthread_mutex_t mutex_display
void move(const char *filename)
vpDisplayRobotType displayType
Initialize the velocity controller.
virtual vpRobotStateType getRobotState(void) const
vpCameraParameters cameraParam
void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
void setToolType(vpViper850::vpToolType tool)
Set the current tool type.
void setExternalCameraPosition(const vpHomogeneousMatrix camMf_)
Initialize the acceleration controller.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void set_displayBusy(const bool &status)
vpCameraParametersProjType
void initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo)
static void display(const vpImage< unsigned char > &I)
vpToolType
List of possible tools that can be attached to the robot end-effector.
Generic class defining intrinsic camera parameters.
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &displacement)
Modelisation of the ADEPT Viper 850 robot.
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false) const
void get_cMe(vpHomogeneousMatrix &cMe) const
static Type minimum(const Type &a, const Type &b)
static bool readPosFile(const std::string &filename, vpColVector &q)
Implementation of a velocity twist matrix and operations on such kind of matrices.
double getSamplingTime() const
void set_velocity(const vpColVector &vel)
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static double rad(double deg)
void setExternalCameraParameters(const vpCameraParameters &cam)
vpToolType getToolType() const
Get the current tool type.
This class aims to be a basis used to create all the simulators of robots.
vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
vpHomogeneousMatrix * fMi
pthread_mutex_t mutex_fMi
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, vpImagePoint offset=vpImagePoint(0, 0))
void updateArticularPosition()
bool setVelocityCalled
Flag used to specify to the thread managing the robot displacements that the setVelocity() method has...
void display_scene(Matrix mat, Bound_scene &sc, const vpImage< vpRGBa > &I, const vpColor &color)
vpHomogeneousMatrix camMf
vpHomogeneousMatrix navigation(const vpImage< vpRGBa > &I, bool &changed)
static double deg(double rad)
unsigned int getHeight() const
Implementation of column vector and the associated operations.
double get_x() const
Get the point x coordinate in the image plane.
void setCameraParameters(const vpCameraParameters &cam)
Implementation of a pose vector and operations on poses.
double get_y() const
Get the point y coordinate in the image plane.
Implementation of a rotation vector as Euler angle minimal representation.
vpControlFrameType setRobotFrame(vpRobot::vpControlFrameType newFrame)
vpControlFrameType getRobotFrame(void) const
void findHighestPositioningSpeed(vpColVector &q)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
void set_artCoord(const vpColVector &coord)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)
void get_cVe(vpVelocityTwistMatrix &cVe)
vpCameraParameters::vpCameraParametersProjType projModel
VISP_EXPORT double getMinTimeForUsleepCall()
void set_artVel(const vpColVector &vel)
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q)
unsigned int getWidth() const
pthread_mutex_t mutex_velocity
static const double defaultPositioningVelocity
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
double getPositioningVelocity(void)
pthread_mutex_t mutex_artVel
static const unsigned int njoint
Number of joint.
vpHomogeneousMatrix camMf2
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo)
static const char *const CONST_PTGREY_FLEA2_CAMERA_NAME
void get_eJe(vpMatrix &eJe)
void resize(const unsigned int i, const bool flagNullify=true)