44 #include <visp/vpSimulatorViper850.h>
45 #include <visp/vpTime.h>
46 #include <visp/vpImagePoint.h>
47 #include <visp/vpPoint.h>
48 #include <visp/vpMeterPixelConversion.h>
49 #include <visp/vpIoTools.h>
53 #if defined(_WIN32) || defined(VISP_HAVE_PTHREAD)
62 q_prev_getdis(), first_time_getdis(true), positioningVelocity(defaultPositioningVelocity),
63 zeroPos(), reposPos(), toolCustom(false), arm_dir()
78 DWORD dwThreadIdArray;
79 hThread = CreateThread(
86 #elif defined (VISP_HAVE_PTHREAD)
93 pthread_attr_init(&
attr);
94 pthread_attr_setdetachstate(&
attr, PTHREAD_CREATE_JOINABLE);
110 q_prev_getdis(), first_time_getdis(true), positioningVelocity(defaultPositioningVelocity),
111 zeroPos(), reposPos(), toolCustom(false), arm_dir()
119 mutex_fMi = CreateMutex(NULL,FALSE,NULL);
126 DWORD dwThreadIdArray;
127 hThread = CreateThread(
134 #elif defined(VISP_HAVE_PTHREAD)
141 pthread_attr_init(&
attr);
142 pthread_attr_setdetachstate(&
attr, PTHREAD_CREATE_JOINABLE);
158 WaitForSingleObject(hThread,INFINITE);
159 CloseHandle(hThread);
165 #elif defined(VISP_HAVE_PTHREAD)
166 pthread_attr_destroy(&
attr);
167 pthread_join(
thread, NULL);
178 for(
int i = 0; i < 6; i++)
199 std::vector<std::string> arm_dirs =
vpIoTools::splitChain(std::string(VISP_ROBOT_ARMS_DIR), std::string(
";"));
200 bool armDirExists =
false;
201 for(
size_t i=0; i < arm_dirs.size(); i++)
203 arm_dir = arm_dirs[i];
207 if (! armDirExists) {
210 std::cout <<
"The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
213 std::cout <<
"Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
227 zeroPos[1] = -M_PI/2; zeroPos[2] = M_PI;
230 reposPos[1] = -M_PI/2; reposPos[2] = M_PI; reposPos[4] = M_PI/2;
237 first_time_getdis =
true;
319 etc[0] = -0.04558630174;
320 etc[1] = -0.00134326752;
321 etc[2] = 0.001000828017;
328 std::cout <<
"This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
352 const unsigned int &image_width,
353 const unsigned int &image_height)
363 if (image_width == 640 && image_height == 480)
365 std::cout <<
"Get default camera parameters for camera \""
370 vpTRACE(
"Cannot get default intrinsic camera parameters for this image resolution");
376 if (image_width == 640 && image_height == 480) {
377 std::cout <<
"Get default camera parameters for camera \""
382 vpTRACE(
"Cannot get default intrinsic camera parameters for this image resolution");
388 std::cout <<
"This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
446 double tcur_1 =
tcur;
459 double ellapsedTime = (
tcur -
tprev) * 1e-3;
476 articularVelocities = 0.0;
482 articularCoordinates[0] = articularCoordinates[0] + ellapsedTime*articularVelocities[0];
483 articularCoordinates[1] = articularCoordinates[1] + ellapsedTime*articularVelocities[1];
484 articularCoordinates[2] = articularCoordinates[2] + ellapsedTime*articularVelocities[2];
485 articularCoordinates[3] = articularCoordinates[3] + ellapsedTime*articularVelocities[3];
486 articularCoordinates[4] = articularCoordinates[4] + ellapsedTime*articularVelocities[4];
487 articularCoordinates[5] = articularCoordinates[5] + ellapsedTime*articularVelocities[5];
494 ellapsedTime = (
joint_min[(
unsigned int)(-jl-1)] - articularCoordinates[(
unsigned int)(-jl-1)])/(articularVelocities[(
unsigned int)(-jl-1)]);
496 ellapsedTime = (
joint_max[(
unsigned int)(jl-1)] - articularCoordinates[(
unsigned int)(jl-1)])/(articularVelocities[(
unsigned int)(jl-1)]);
498 for (
unsigned int i = 0; i < 6; i++)
499 articularCoordinates[i] = articularCoordinates[i] + ellapsedTime*articularVelocities[i];
541 for (
int k = 1; k < 7; k++)
594 double c23 = cos(q2+q3);
595 double s23 = sin(q2+q3);
612 fMit[0][0][3] =
a1*c1;
613 fMit[0][1][3] =
a1*s1;
616 fMit[1][0][0] = c1*c2;
617 fMit[1][1][0] = s1*c2;
619 fMit[1][0][1] = -c1*s2;
620 fMit[1][1][1] = -s1*s2;
625 fMit[1][0][3] = c1*(
a2*c2+
a1);
626 fMit[1][1][3] = s1*(
a2*c2+
a1);
627 fMit[1][2][3] =
d1-
a2*s2;
629 double quickcomp1 =
a3*c23-
a2*c2-
a1;
631 fMit[2][0][0] = -c1*c23;
632 fMit[2][1][0] = -s1*c23;
637 fMit[2][0][2] = c1*s23;
638 fMit[2][1][2] = s1*s23;
640 fMit[2][0][3] = -c1*quickcomp1;
641 fMit[2][1][3] = -s1*quickcomp1;
642 fMit[2][2][3] =
a3*s23-
a2*s2+
d1;
644 double quickcomp2 = c1*(s23*
d4 - quickcomp1);
645 double quickcomp3 = s1*(s23*
d4 - quickcomp1);
647 fMit[3][0][0] = -c1*c23*c4+s1*s4;
648 fMit[3][1][0] = -s1*c23*c4-c1*s4;
649 fMit[3][2][0] = s23*c4;
650 fMit[3][0][1] = c1*s23;
651 fMit[3][1][1] = s1*s23;
653 fMit[3][0][2] = -c1*c23*s4-s1*c4;
654 fMit[3][1][2] = -s1*c23*s4+c1*c4;
655 fMit[3][2][2] = s23*s4;
656 fMit[3][0][3] = quickcomp2;
657 fMit[3][1][3] = quickcomp3;
658 fMit[3][2][3] = c23*
d4+
a3*s23-
a2*s2+
d1;
660 fMit[4][0][0] = c1*(s23*s5-c5*c23*c4)+s1*c5*s4;
661 fMit[4][1][0] = s1*(s23*s5-c5*c23*c4)-c1*c5*s4;
662 fMit[4][2][0] = s23*c4*c5+c23*s5;
663 fMit[4][0][1] = c1*c23*s4+s1*c4;
664 fMit[4][1][1] = s1*c23*s4-c1*c4;
665 fMit[4][2][1] = -s23*s4;
666 fMit[4][0][2] = c1*(s23*c5+s5*c23*c4)-s1*s5*s4;
667 fMit[4][1][2] = s1*(s23*c5+s5*c23*c4)+c1*s5*s4;
668 fMit[4][2][2] = -s23*c4*s5+c23*c5;
669 fMit[4][0][3] = quickcomp2;
670 fMit[4][1][3] = quickcomp3;
671 fMit[4][2][3] = c23*
d4+
a3*s23-
a2*s2+
d1;
673 fMit[5][0][0] = c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-s1*(s4*c5*c6+c4*s6);
674 fMit[5][1][0] = -s1*(c23*(-c4*c5*c6+s4*s6)+s23*s5*c6)+c1*(s4*c5*c6+c4*s6);
675 fMit[5][2][0] = s23*(s4*s6-c4*c5*c6)-c23*s5*c6;
676 fMit[5][0][1] = -c1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)+s1*(s4*c5*s6-c4*c6);
677 fMit[5][1][1] = -s1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)-c1*(s4*c5*s6-c4*c6);
678 fMit[5][2][1] = s23*(c4*c5*s6+s4*c6)+c23*s5*s6;
679 fMit[5][0][2] = c1*(c23*c4*s5+s23*c5)-s1*s4*s5;
680 fMit[5][1][2] = s1*(c23*c4*s5+s23*c5)+c1*s4*s5;
681 fMit[5][2][2] = -s23*c4*s5+c23*c5;
682 fMit[5][0][3] = quickcomp2;
683 fMit[5][1][3] = quickcomp3;
684 fMit[5][2][3] = s23*
a3+c23*
d4-
a2*s2+
d1;
686 fMit[6][0][0] = c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-s1*(s4*c5*c6+c4*s6);
687 fMit[6][1][0] = -s1*(c23*(-c4*c5*c6+s4*s6)+s23*s5*c6)+c1*(s4*c5*c6+c4*s6);
688 fMit[6][2][0] = s23*(s4*s6-c4*c5*c6)-c23*s5*c6;
689 fMit[6][0][1] = -c1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)+s1*(s4*c5*s6-c4*c6);
690 fMit[6][1][1] = -s1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)-c1*(s4*c5*s6-c4*c6);
691 fMit[6][2][1] = s23*(c4*c5*s6+s4*c6)+c23*s5*s6;
692 fMit[6][0][2] = c1*(c23*c4*s5+s23*c5)-s1*s4*s5;
693 fMit[6][1][2] = s1*(c23*c4*s5+s23*c5)+c1*s4*s5;
694 fMit[6][2][2] = -s23*c4*s5+c23*c5;
695 fMit[6][0][3] = c1*(c23*(c4*s5*
d6-
a3)+s23*(c5*
d6+
d4)+a1+
a2*c2)-s1*s4*s5*
d6;
696 fMit[6][1][3] = s1*(c23*(c4*s5*
d6-
a3)+s23*(c5*
d6+
d4)+a1+
a2*c2)+c1*s4*s5*
d6;
697 fMit[6][2][3] = s23*(
a3-c4*s5*
d6)+c23*(c5*
d6+
d4)-
a2*s2+
d1;
707 for (
int i = 0; i < 8; i++)
710 #elif defined(VISP_HAVE_PTHREAD)
712 for (
int i = 0; i < 8; i++)
737 std::cout <<
"Change the control mode from velocity to position control.\n";
747 std::cout <<
"Change the control mode from stop to velocity control.\n";
833 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
835 "Cannot send a velocity to the robot "
836 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
841 double scale_trans_sat = 1;
842 double scale_rot_sat = 1;
843 double scale_sat = 1;
858 vpERROR_TRACE (
"The velocity vector must have a size of 6 !!!!");
862 for (
unsigned int i = 0 ; i < 3; ++ i)
864 vel_abs = fabs (vel[i]);
867 vel_trans_max = vel_abs;
869 "(axis nr. %d).", vel[i], i+1);
872 vel_abs = fabs (vel[i+3]);
874 vel_rot_max = vel_abs;
876 "(axis nr. %d).", vel[i+3], i+4);
886 if ( (scale_trans_sat < 1) || (scale_rot_sat < 1) )
888 if (scale_trans_sat < scale_rot_sat)
889 scale_sat = scale_trans_sat;
891 scale_sat = scale_rot_sat;
901 vpERROR_TRACE (
"The velocity vector must have a size of 6 !!!!");
904 for (
unsigned int i = 0 ; i < 6; ++ i)
906 vel_abs = fabs (vel[i]);
909 vel_rot_max = vel_abs;
911 "(axis nr. %d).", vel[i], i+1);
916 if ( scale_rot_sat < 1 )
917 scale_sat = scale_rot_sat;
940 double scale_rot_sat = 1;
941 double scale_sat = 1;
959 articularVelocity = eJe_*eVc*velocityframe;
970 articularVelocity = fJe_*velocityframe;
976 articularVelocity = velocityframe;
993 for (
unsigned int i = 0 ; i < 6; ++ i)
995 vel_abs = fabs (articularVelocity[i]);
998 vel_rot_max = vel_abs;
1000 "(axis nr. %d).", articularVelocity[i], i+1);
1005 if ( scale_rot_sat < 1 )
1006 scale_sat = scale_rot_sat;
1081 vel = cVe*eJe_*articularVelocity;
1086 vel = articularVelocity;
1093 vel = fJe_*articularVelocity;
1103 "Case not taken in account.");
1209 double velmax = fabs(q[0]);
1210 for (
unsigned int i = 1; i < 6; i++)
1212 if (velmax < fabs(q[i]))
1213 velmax = fabs(q[i]);
1300 "Modification of the robot state");
1317 for (
unsigned int i=0; i < 3; i++)
1334 qdes = articularCoordinates;
1339 error = qdes - articularCoordinates;
1356 "Position out of range.");
1358 }
while (errsqr > 1e-8 && nbSol > 0);
1368 error = q - articularCoordinates;
1381 }
while (errsqr > 1e-8);
1392 for (
unsigned int i=0; i < 3; i++)
1404 qdes = articularCoordinates;
1408 error = qdes - articularCoordinates;
1424 }
while (errsqr > 1e-8 && nbSol > 0);
1429 vpERROR_TRACE (
"Positionning error. Mixt frame not implemented");
1431 "Positionning error: "
1432 "Mixt frame not implemented.");
1511 position[0] = pos1 ;
1512 position[1] = pos2 ;
1513 position[2] = pos3 ;
1514 position[3] = pos4 ;
1515 position[4] = pos5 ;
1516 position[5] = pos6 ;
1572 "Bad position in filename.");
1668 for (
unsigned int i=0; i <3; i++)
1678 vpERROR_TRACE (
"Positionning error. Mixt frame not implemented");
1680 "Positionning error: "
1681 "Mixt frame not implemented.");
1743 for(
unsigned int j=0;j<3;j++)
1745 position[j]=posRxyz[j];
1746 position[j+3]=RtuVect[j];
1779 vpTRACE(
"Joint limit vector has not a size of 6 !");
1811 double c2 = cos(q2);
1812 double c3 = cos(q3);
1813 double s3 = sin(q3);
1814 double c23 = cos(q2+q3);
1815 double s23 = sin(q2+q3);
1816 double s5 = sin(q5);
1818 bool cond1 = fabs(s5) < 1e-1;
1819 bool cond2 = fabs(
a3*s3+c3*
d4) < 1e-1;
1820 bool cond3 = fabs(
a2*c2-
a3*c23+s23*d4+
a1) < 1e-1;
1841 J[1][0] = 0; J[2][0] = 0; J[3][0] = 0; J[4][0] = 0; J[5][0] = 0;
1842 J[1][1] = 0; J[2][1] = 0; J[3][1] = 0; J[4][1] = 0; J[5][1] = 0;
1843 J[1][2] = 0; J[2][2] = 0; J[3][2] = 0; J[4][2] = 0; J[5][2] = 0;
1849 J[0][0] = 0; J[3][0] = 0; J[4][0] = 0; J[5][0] = 0;
1850 J[0][1] = 0; J[3][1] = 0; J[4][1] = 0; J[5][1] = 0;
1868 for (
unsigned int i = 0; i < 6; i++)
1870 if (articularCoordinates[i] <=
joint_min[i])
1872 difft =
joint_min[i] - articularCoordinates[i];
1876 artNumb = -(int)i-1;
1881 for (
unsigned int i = 0; i < 6; i++)
1883 if (articularCoordinates[i] >=
joint_max[i])
1885 difft = articularCoordinates[i] -
joint_max[i];
1889 artNumb = (int)(i+1);
1895 std::cout <<
"\nWarning: Velocity control stopped: axis " << fabs((
float)artNumb) <<
" on joint limit!" <<std::endl;
1912 vpSimulatorViper850::getCameraDisplacement(
vpColVector &displacement)
1927 vpSimulatorViper850::getArticularDisplacement(
vpColVector &displacement)
1960 if ( ! first_time_getdis )
1966 std::cout <<
"getDisplacement() CAMERA_FRAME not implemented\n";
1973 displacement = q_cur - q_prev_getdis;
1979 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
1986 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
1994 first_time_getdis =
false;
1998 q_prev_getdis = q_cur;
2066 fd = fopen(filename,
"r") ;
2070 char line[FILENAME_MAX];
2071 char dummy[FILENAME_MAX];
2073 bool sortie =
false;
2077 if (fgets (line, FILENAME_MAX, fd) != NULL) {
2078 if ( strncmp (line,
"#", 1) != 0) {
2080 if ( strncmp (line, head,
sizeof(head)-1) == 0) {
2092 while ( sortie !=
true );
2096 int ret = sscanf(line,
"%s %lf %lf %lf %lf %lf %lf",
2098 &q[0], &q[1], &q[2], &q[3], &q[4], &q[5]);
2137 fd = fopen(filename,
"w") ;
2142 #Viper - Position - Version 1.0\n\
2145 # Joint position in degrees\n\
2150 fprintf(fd,
"R: %lf %lf %lf %lf %lf %lf\n",
2301 std::string scene_dir_;
2302 std::vector<std::string> scene_dirs =
vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(
";"));
2303 bool sceneDirExists =
false;
2304 for(
size_t i=0; i < scene_dirs.size(); i++)
2306 scene_dir_ = scene_dirs[i];
2307 sceneDirExists =
true;
2310 if (! sceneDirExists) {
2313 std::cout <<
"The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
2316 std::cout <<
"Cannot get VISP_SCENES_DIR environment variable" << std::endl;
2320 unsigned int name_length = 30;
2321 if (scene_dir_.size() > FILENAME_MAX)
2324 unsigned int full_length = (
unsigned int)scene_dir_.size() + name_length;
2325 if (full_length > FILENAME_MAX)
2328 char *name_cam =
new char [full_length];
2330 strcpy(name_cam, scene_dir_.c_str());
2331 strcat(name_cam,
"/camera.bnd");
2334 if (arm_dir.size() > FILENAME_MAX)
2336 full_length = (
unsigned int)arm_dir.size() + name_length;
2337 if (full_length > FILENAME_MAX)
2340 char *name_arm =
new char [full_length];
2341 strcpy(name_arm, arm_dir.c_str());
2342 strcat(name_arm,
"/viper850_arm1.bnd");
2344 strcpy(name_arm, arm_dir.c_str());
2345 strcat(name_arm,
"/viper850_arm2.bnd");
2347 strcpy(name_arm, arm_dir.c_str());
2348 strcat(name_arm,
"/viper850_arm3.bnd");
2350 strcpy(name_arm, arm_dir.c_str());
2351 strcat(name_arm,
"/viper850_arm4.bnd");
2353 strcpy(name_arm, arm_dir.c_str());
2354 strcat(name_arm,
"/viper850_arm5.bnd");
2356 strcpy(name_arm, arm_dir.c_str());
2357 strcat(name_arm,
"/viper850_arm6.bnd");
2366 add_rfstack(IS_BACK);
2368 add_vwstack (
"start",
"depth", 0.0, 100.0);
2369 add_vwstack (
"start",
"window", -0.1,0.1,-0.1,0.1);
2370 add_vwstack (
"start",
"type", PERSPECTIVE);
2384 bool changed =
false;
2388 if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
2411 float w44o[4][4],w44cext[4][4],x,y,z;
2415 add_vwstack (
"start",
"cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
2416 x = w44cext[2][0] + w44cext[3][0];
2417 y = w44cext[2][1] + w44cext[3][1];
2418 z = w44cext[2][2] + w44cext[3][2];
2419 add_vwstack (
"start",
"vrp", x,y,z);
2420 add_vwstack (
"start",
"vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
2421 add_vwstack (
"start",
"vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
2422 add_vwstack (
"start",
"window", -u, u, -v, v);
2430 vp2jlc_matrix(fMit[0],w44o);
2433 vp2jlc_matrix(fMit[1],w44o);
2436 vp2jlc_matrix(fMit[2],w44o);
2439 vp2jlc_matrix(fMit[3],w44o);
2442 vp2jlc_matrix(fMit[6],w44o);
2450 cMe = fMit[6] * cMe;
2451 vp2jlc_matrix(cMe,w44o);
2457 vp2jlc_matrix(
fMo,w44o);
2497 std::cout <<
"Used joint coordinates (rad): " << articularCoordinates.
t() << std::endl;
2527 fMo = fMit[7] * cMo_;
static void displayCamera(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness)
Definition of the vpMatrix class.
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
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()
void get_fMi(vpHomogeneousMatrix *fMit)
void computeArticularVelocity()
static bool readPosFile(const char *filename, vpColVector &q)
static const vpToolType defaultTool
Default tool attached to the robot end effector.
unsigned int getWidth() const
void get_cMe(vpHomogeneousMatrix &cMe)
double getSamplingTime() const
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
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 ...
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
double getMaxTranslationVelocity(void) const
static const vpColor none
Initialize the position controller.
error that can be emited by ViSP classes.
void track(const vpHomogeneousMatrix &cMo)
void get_fJe(vpMatrix &fJe)
vpColVector get_velocity()
static double measureTimeMs()
double get_y() const
Get the point y coordinate in the image plane.
static const char *const CONST_MARLIN_F033C_CAMERA_NAME
static int wait(double t0, double t)
static const vpColor green
static void flush(const vpImage< unsigned char > &I)
double getMaxRotationVelocity(void) const
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Class that defines what is a point.
static Type maximum(const Type &a, const Type &b)
The vpRotationMatrix considers the particular case of a rotation matrix.
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.
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)
vpRowVector t() const
Transpose of a vector.
vpToolType
List of possible tools that can be attached to the robot end-effector.
Generic class defining intrinsic camera parameters.
vpHomogeneousMatrix getExternalCameraPosition() const
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &displacement)
Modelisation of the ADEPT Viper 850 robot.
void extract(vpRotationMatrix &R) const
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
double get_x() const
Get the point x coordinate in the image plane.
vpToolType getToolType() const
Get the current tool type.
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)
static Type minimum(const Type &a, const Type &b)
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false) const
void set_velocity(const vpColVector &vel)
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Construction from translation vector and rotation matrix.
static double rad(double deg)
void setExternalCameraParameters(const vpCameraParameters &cam)
vpControlFrameType getRobotFrame(void) const
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
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
void get_cMe(vpHomogeneousMatrix &cMe) const
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)
Class that provides a data structure for the column vectors as well as a set of operations on these v...
void setCameraParameters(const vpCameraParameters &cam)
The pose is a complete representation of every rigid motion in the euclidian space.
vpHomogeneousMatrix inverse() const
virtual vpRobotStateType getRobotState(void) const
unsigned int getHeight() const
Class that consider the case of the Euler angle using the x-y-z convention, where are respectively ...
vpControlFrameType setRobotFrame(vpRobot::vpControlFrameType newFrame)
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Compute the pseudo inverse of the matrix using the SVD.
static double measureTimeSecond()
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)
virtual void displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)=0
void get_cVe(vpVelocityTwistMatrix &cVe)
vpCameraParameters::vpCameraParametersProjType projModel
void set_artVel(const vpColVector &vel)
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height)
unsigned int getRows() const
Return the number of rows of the matrix.
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q)
pthread_mutex_t mutex_velocity
static const double defaultPositioningVelocity
static bool savePosFile(const char *filename, const vpColVector &q)
Class that consider the case of a translation vector.
Class that consider the case of the parameterization for the rotation.
double getPositioningVelocity(void)
pthread_mutex_t mutex_artVel
static double minTimeForUsleepCall
static const unsigned int njoint
Number of joint.
vpHomogeneousMatrix camMf2
bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo)
static const char *const CONST_PTGREY_FLEA2_CAMERA_NAME
void setWorldCoordinates(const double ox, const double oy, const double oz)
Set the point world coordinates. We mean here the coordinates of the point in the object frame...
void get_eJe(vpMatrix &eJe)
void resize(const unsigned int i, const bool flagNullify=true)