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)
76 DWORD dwThreadIdArray;
84 #elif defined (VISP_HAVE_PTHREAD)
91 pthread_attr_init(&attr);
92 pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
94 pthread_create(&thread, NULL,
launcher, (
void *)
this);
111 mutex_fMi = CreateMutex(NULL,FALSE,NULL);
118 DWORD dwThreadIdArray;
126 #elif defined(VISP_HAVE_PTHREAD)
133 pthread_attr_init(&attr);
134 pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
136 pthread_create(&thread, NULL,
launcher, (
void *)
this);
150 WaitForSingleObject(
hThread,INFINITE);
157 #elif defined(VISP_HAVE_PTHREAD)
158 pthread_attr_destroy(&attr);
159 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;
212 zeroPos[1] = -M_PI/2; zeroPos[2] = M_PI;
215 reposPos[1] = -M_PI/2; reposPos[2] = M_PI; reposPos[4] = M_PI/2;
222 first_time_getdis =
true;
304 etc[0] = -0.04558630174;
305 etc[1] = -0.00134326752;
306 etc[2] = 0.001000828017;
313 std::cout <<
"This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
337 const unsigned int &image_width,
338 const unsigned int &image_height)
348 if (image_width == 640 && image_height == 480)
350 std::cout <<
"Get default camera parameters for camera \""
355 vpTRACE(
"Cannot get default intrinsic camera parameters for this image resolution");
361 if (image_width == 640 && image_height == 480) {
362 std::cout <<
"Get default camera parameters for camera \""
367 vpTRACE(
"Cannot get default intrinsic camera parameters for this image resolution");
373 std::cout <<
"This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
431 double tcur_1 =
tcur;
457 articularVelocities = 0.0;
462 articularCoordinates[0] = articularCoordinates[0] + ellapsedTime*articularVelocities[0]*1e-3;
463 articularCoordinates[1] = articularCoordinates[1] + ellapsedTime*articularVelocities[1]*1e-3;
464 articularCoordinates[2] = articularCoordinates[2] + ellapsedTime*articularVelocities[2]*1e-3;
465 articularCoordinates[3] = articularCoordinates[3] + ellapsedTime*articularVelocities[3]*1e-3;
466 articularCoordinates[4] = articularCoordinates[4] + ellapsedTime*articularVelocities[4]*1e-3;
467 articularCoordinates[5] = articularCoordinates[5] + ellapsedTime*articularVelocities[5]*1e-3;
474 ellapsedTime = (
joint_min[(
unsigned int)(-jl-1)] - articularCoordinates[(
unsigned int)(-jl-1)])/(articularVelocities[(
unsigned int)(-jl-1)]*1e-3);
476 ellapsedTime = (
joint_max[(
unsigned int)(jl-1)] - articularCoordinates[(
unsigned int)(jl-1)])/(articularVelocities[(
unsigned int)(jl-1)]*1e-3);
478 for (
unsigned int i = 0; i < 6; i++)
479 articularCoordinates[i] = articularCoordinates[i] + ellapsedTime*articularVelocities[i]*1e-3;
521 for (
int k = 1; k < 7; k++)
572 double c23 = cos(q2+q3);
573 double s23 = sin(q2+q3);
590 fMit[0][0][3] =
a1*c1;
591 fMit[0][1][3] =
a1*s1;
594 fMit[1][0][0] = c1*c2;
595 fMit[1][1][0] = s1*c2;
597 fMit[1][0][1] = -c1*s2;
598 fMit[1][1][1] = -s1*s2;
603 fMit[1][0][3] = c1*(
a2*c2+
a1);
604 fMit[1][1][3] = s1*(
a2*c2+
a1);
605 fMit[1][2][3] =
d1-
a2*s2;
607 double quickcomp1 =
a3*c23-
a2*c2-
a1;
609 fMit[2][0][0] = -c1*c23;
610 fMit[2][1][0] = -s1*c23;
615 fMit[2][0][2] = c1*s23;
616 fMit[2][1][2] = s1*s23;
618 fMit[2][0][3] = -c1*quickcomp1;
619 fMit[2][1][3] = -s1*quickcomp1;
620 fMit[2][2][3] =
a3*s23-
a2*s2+
d1;
622 double quickcomp2 = c1*(s23*
d4 - quickcomp1);
623 double quickcomp3 = s1*(s23*
d4 - quickcomp1);
625 fMit[3][0][0] = -c1*c23*c4+s1*s4;
626 fMit[3][1][0] = -s1*c23*c4-c1*s4;
627 fMit[3][2][0] = s23*c4;
628 fMit[3][0][1] = c1*s23;
629 fMit[3][1][1] = s1*s23;
631 fMit[3][0][2] = -c1*c23*s4-s1*c4;
632 fMit[3][1][2] = -s1*c23*s4+c1*c4;
633 fMit[3][2][2] = s23*s4;
634 fMit[3][0][3] = quickcomp2;
635 fMit[3][1][3] = quickcomp3;
636 fMit[3][2][3] = c23*
d4+
a3*s23-
a2*s2+
d1;
638 fMit[4][0][0] = c1*(s23*s5-c5*c23*c4)+s1*c5*s4;
639 fMit[4][1][0] = s1*(s23*s5-c5*c23*c4)-c1*c5*s4;
640 fMit[4][2][0] = s23*c4*c5+c23*s5;
641 fMit[4][0][1] = c1*c23*s4+s1*c4;
642 fMit[4][1][1] = s1*c23*s4-c1*c4;
643 fMit[4][2][1] = -s23*s4;
644 fMit[4][0][2] = c1*(s23*c5+s5*c23*c4)-s1*s5*s4;
645 fMit[4][1][2] = s1*(s23*c5+s5*c23*c4)+c1*s5*s4;
646 fMit[4][2][2] = -s23*c4*s5+c23*c5;
647 fMit[4][0][3] = quickcomp2;
648 fMit[4][1][3] = quickcomp3;
649 fMit[4][2][3] = c23*
d4+
a3*s23-
a2*s2+
d1;
651 fMit[5][0][0] = c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-s1*(s4*c5*c6+c4*s6);
652 fMit[5][1][0] = -s1*(c23*(-c4*c5*c6+s4*s6)+s23*s5*c6)+c1*(s4*c5*c6+c4*s6);
653 fMit[5][2][0] = s23*(s4*s6-c4*c5*c6)-c23*s5*c6;
654 fMit[5][0][1] = -c1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)+s1*(s4*c5*s6-c4*c6);
655 fMit[5][1][1] = -s1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)-c1*(s4*c5*s6-c4*c6);
656 fMit[5][2][1] = s23*(c4*c5*s6+s4*c6)+c23*s5*s6;
657 fMit[5][0][2] = c1*(c23*c4*s5+s23*c5)-s1*s4*s5;
658 fMit[5][1][2] = s1*(c23*c4*s5+s23*c5)+c1*s4*s5;
659 fMit[5][2][2] = -s23*c4*s5+c23*c5;
660 fMit[5][0][3] = quickcomp2;
661 fMit[5][1][3] = quickcomp3;
662 fMit[5][2][3] = s23*
a3+c23*
d4-
a2*s2+
d1;
664 fMit[6][0][0] = c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-s1*(s4*c5*c6+c4*s6);
665 fMit[6][1][0] = -s1*(c23*(-c4*c5*c6+s4*s6)+s23*s5*c6)+c1*(s4*c5*c6+c4*s6);
666 fMit[6][2][0] = s23*(s4*s6-c4*c5*c6)-c23*s5*c6;
667 fMit[6][0][1] = -c1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)+s1*(s4*c5*s6-c4*c6);
668 fMit[6][1][1] = -s1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)-c1*(s4*c5*s6-c4*c6);
669 fMit[6][2][1] = s23*(c4*c5*s6+s4*c6)+c23*s5*s6;
670 fMit[6][0][2] = c1*(c23*c4*s5+s23*c5)-s1*s4*s5;
671 fMit[6][1][2] = s1*(c23*c4*s5+s23*c5)+c1*s4*s5;
672 fMit[6][2][2] = -s23*c4*s5+c23*c5;
673 fMit[6][0][3] = c1*(c23*(c4*s5*
d6-
a3)+s23*(c5*
d6+
d4)+a1+
a2*c2)-s1*s4*s5*
d6;
674 fMit[6][1][3] = s1*(c23*(c4*s5*
d6-
a3)+s23*(c5*
d6+
d4)+a1+
a2*c2)+c1*s4*s5*
d6;
675 fMit[6][2][3] = s23*(
a3-c4*s5*
d6)+c23*(c5*
d6+
d4)-
a2*s2+
d1;
685 for (
int i = 0; i < 8; i++)
688 #elif defined(VISP_HAVE_PTHREAD)
690 for (
int i = 0; i < 8; i++)
715 std::cout <<
"Change the control mode from velocity to position control.\n";
725 std::cout <<
"Change the control mode from stop to velocity control.\n";
811 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
813 "Cannot send a velocity to the robot "
814 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
819 double scale_trans_sat = 1;
820 double scale_rot_sat = 1;
821 double scale_sat = 1;
836 vpERROR_TRACE (
"The velocity vector must have a size of 6 !!!!");
840 for (
unsigned int i = 0 ; i < 3; ++ i)
842 vel_abs = fabs (vel[i]);
845 vel_trans_max = vel_abs;
847 "(axis nr. %d).", vel[i], i+1);
850 vel_abs = fabs (vel[i+3]);
852 vel_rot_max = vel_abs;
854 "(axis nr. %d).", vel[i+3], i+4);
864 if ( (scale_trans_sat < 1) || (scale_rot_sat < 1) )
866 if (scale_trans_sat < scale_rot_sat)
867 scale_sat = scale_trans_sat;
869 scale_sat = scale_rot_sat;
879 vpERROR_TRACE (
"The velocity vector must have a size of 6 !!!!");
882 for (
unsigned int i = 0 ; i < 6; ++ i)
884 vel_abs = fabs (vel[i]);
887 vel_rot_max = vel_abs;
889 "(axis nr. %d).", vel[i], i+1);
894 if ( scale_rot_sat < 1 )
895 scale_sat = scale_rot_sat;
918 double scale_rot_sat = 1;
919 double scale_sat = 1;
937 articularVelocity = eJe*eVc*velocityframe;
948 articularVelocity = fJe*velocityframe;
954 articularVelocity = velocityframe;
971 for (
unsigned int i = 0 ; i < 6; ++ i)
973 vel_abs = fabs (articularVelocity[i]);
976 vel_rot_max = vel_abs;
978 "(axis nr. %d).", articularVelocity[i], i+1);
983 if ( scale_rot_sat < 1 )
984 scale_sat = scale_rot_sat;
1059 vel = cVe*eJe*articularVelocity;
1064 vel = articularVelocity;
1071 vel = fJe*articularVelocity;
1081 "Case not taken in account.");
1143 double velmax = fabs(q[0]);
1144 for (
unsigned int i = 1; i < 6; i++)
1146 if (velmax < fabs(q[i]))
1147 velmax = fabs(q[i]);
1234 "Modification of the robot state");
1251 for (
unsigned int i=0; i < 3; i++)
1268 qdes = articularCoordinates;
1273 error = qdes - articularCoordinates;
1290 "Position out of range.");
1292 }
while (errsqr > 1e-8 && nbSol > 0);
1302 error = q - articularCoordinates;
1315 }
while (errsqr > 1e-8);
1326 for (
unsigned int i=0; i < 3; i++)
1338 qdes = articularCoordinates;
1342 error = qdes - articularCoordinates;
1358 }
while (errsqr > 1e-8 && nbSol > 0);
1363 vpERROR_TRACE (
"Positionning error. Mixt frame not implemented");
1365 "Positionning error: "
1366 "Mixt frame not implemented.");
1445 position[0] = pos1 ;
1446 position[1] = pos2 ;
1447 position[2] = pos3 ;
1448 position[3] = pos4 ;
1449 position[4] = pos5 ;
1450 position[5] = pos6 ;
1506 "Bad position in filename.");
1602 for (
unsigned int i=0; i <3; i++)
1612 vpERROR_TRACE (
"Positionning error. Mixt frame not implemented");
1614 "Positionning error: "
1615 "Mixt frame not implemented.");
1644 for(
unsigned int j=0;j<3;j++)
1646 position[j]=posRxyz[j];
1647 position[j+3]=RtuVect[j];
1662 vpTRACE(
"Joint limit vector has not a size of 6 !");
1694 double c2 = cos(q2);
1695 double c3 = cos(q3);
1696 double s3 = sin(q3);
1697 double c23 = cos(q2+q3);
1698 double s23 = sin(q2+q3);
1699 double s5 = sin(q5);
1701 bool cond1 = fabs(s5) < 1e-1;
1702 bool cond2 = fabs(
a3*s3+c3*
d4) < 1e-1;
1703 bool cond3 = fabs(
a2*c2-
a3*c23+s23*d4+
a1) < 1e-1;
1724 J[1][0] = 0; J[2][0] = 0; J[3][0] = 0; J[4][0] = 0; J[5][0] = 0;
1725 J[1][1] = 0; J[2][1] = 0; J[3][1] = 0; J[4][1] = 0; J[5][1] = 0;
1726 J[1][2] = 0; J[2][2] = 0; J[3][2] = 0; J[4][2] = 0; J[5][2] = 0;
1732 J[0][0] = 0; J[3][0] = 0; J[4][0] = 0; J[5][0] = 0;
1733 J[0][1] = 0; J[3][1] = 0; J[4][1] = 0; J[5][1] = 0;
1751 for (
unsigned int i = 0; i < 6; i++)
1753 if (articularCoordinates[i] <=
joint_min[i])
1755 difft =
joint_min[i] - articularCoordinates[i];
1759 artNumb = -(int)i-1;
1764 for (
unsigned int i = 0; i < 6; i++)
1766 if (articularCoordinates[i] >=
joint_max[i])
1768 difft = articularCoordinates[i] -
joint_max[i];
1772 artNumb = (int)(i+1);
1778 std::cout <<
"\nWarning: Velocity control stopped: axis " << fabs((
float)artNumb) <<
" on joint limit!" <<std::endl;
1843 if ( ! first_time_getdis )
1849 std::cout <<
"getDisplacement() CAMERA_FRAME not implemented\n";
1856 displacement = q_cur - q_prev_getdis;
1862 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
1869 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
1877 first_time_getdis =
false;
1881 q_prev_getdis = q_cur;
1950 fd = fopen(filename,
"r") ;
1954 char line[FILENAME_MAX];
1955 char dummy[FILENAME_MAX];
1957 bool sortie =
false;
1961 if (fgets (line, FILENAME_MAX, fd) != NULL) {
1962 if ( strncmp (line,
"#", 1) != 0) {
1964 if ( strncmp (line, head,
sizeof(head)-1) == 0) {
1976 while ( sortie !=
true );
1980 sscanf(line,
"%s %lf %lf %lf %lf %lf %lf",
1982 &q[0], &q[1], &q[2],
1983 &q[3], &q[4], &q[5]);
2018 fd = fopen(filename,
"w") ;
2023 #Viper - Position - Version 1.0\n\
2026 # Joint position in degrees\n\
2031 fprintf(fd,
"R: %lf %lf %lf %lf %lf %lf\n",
2181 std::string scene_dir;
2183 scene_dir = VISP_SCENES_DIR;
2187 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 char name_cam[FILENAME_MAX];
2196 strcpy(name_cam, scene_dir.c_str());
2197 strcat(name_cam,
"/camera.bnd");
2200 char name_arm[FILENAME_MAX];
2201 strcpy(name_arm, arm_dir.c_str());
2202 strcat(name_arm,
"/viper850_arm1.bnd");
2204 strcpy(name_arm, arm_dir.c_str());
2205 strcat(name_arm,
"/viper850_arm2.bnd");
2207 strcpy(name_arm, arm_dir.c_str());
2208 strcat(name_arm,
"/viper850_arm3.bnd");
2210 strcpy(name_arm, arm_dir.c_str());
2211 strcat(name_arm,
"/viper850_arm4.bnd");
2213 strcpy(name_arm, arm_dir.c_str());
2214 strcat(name_arm,
"/viper850_arm5.bnd");
2216 strcpy(name_arm, arm_dir.c_str());
2217 strcat(name_arm,
"/viper850_arm6.bnd");
2226 add_rfstack(IS_BACK);
2228 add_vwstack (
"start",
"depth", 0.0, 100.0);
2229 add_vwstack (
"start",
"window", -0.1,0.1,-0.1,0.1);
2230 add_vwstack (
"start",
"type", PERSPECTIVE);
2241 bool changed =
false;
2245 if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
2268 float w44o[4][4],w44cext[4][4],x,y,z;
2272 add_vwstack (
"start",
"cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
2273 x = w44cext[2][0] + w44cext[3][0];
2274 y = w44cext[2][1] + w44cext[3][1];
2275 z = w44cext[2][2] + w44cext[3][2];
2276 add_vwstack (
"start",
"vrp", x,y,z);
2277 add_vwstack (
"start",
"vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
2278 add_vwstack (
"start",
"vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
2279 add_vwstack (
"start",
"window", -u, u, -v, v);
2287 vp2jlc_matrix(fMit[0],w44o);
2290 vp2jlc_matrix(fMit[1],w44o);
2293 vp2jlc_matrix(fMit[2],w44o);
2296 vp2jlc_matrix(fMit[3],w44o);
2299 vp2jlc_matrix(fMit[6],w44o);
2307 cMe = fMit[6] * cMe;
2308 vp2jlc_matrix(cMe,w44o);
2314 vp2jlc_matrix(
fMo,w44o);
unsigned int jointLimitArt
vpToolType getToolType()
Get the current tool type.
Definition of the vpMatrix class.
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
vpHomogeneousMatrix * fMi
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.
virtual ~vpSimulatorViper850()
void get_fMi(vpHomogeneousMatrix *fMit)
vpColVector get_velocity()
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)
void setJointLimit(vpColVector limitMin, vpColVector limitMax)
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
void get_eJe(const vpColVector &q, vpMatrix &eJe)
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 ...
static void displayCamera(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color)
virtual vpRobotStateType getRobotState(void)
void getExternalImage(vpImage< vpRGBa > &I)
vpHomogeneousMatrix navigation(vpImage< vpRGBa > &I, bool &changed)
void set_artVel(const vpColVector &vel)
double getMaxTranslationVelocity(void) const
static const vpColor none
Initialize the position controller.
void getCameraDisplacement(vpColVector &displacement)
void track(const vpHomogeneousMatrix &cMo)
vpHomogeneousMatrix getExternalCameraPosition() const
void get_fJe(vpMatrix &fJe)
static double measureTimeMs()
double get_y() const
Get the point y coordinate in the image plane.
void getArticularDisplacement(vpColVector &displacement)
static const char *const CONST_MARLIN_F033C_CAMERA_NAME
static int wait(double t0, double t)
double sumSquare() const
return sum of the Aij^2 (for all i, for all j)
static const vpColor green
static void flush(const vpImage< unsigned char > &I)
double getMaxRotationVelocity(void) const
void set_artCoord(const vpColVector &coord)
static DWORD WINAPI launcher(LPVOID lpParam)
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Class that defines what is a point.
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q)
static Type maximum(const Type &a, const Type &b)
The vpRotationMatrix considers the particular case of a rotation matrix.
vpColVector get_artCoord()
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
vpControlFrameType getRobotFrame(void)
void move(const char *filename)
This class aims to be a basis used to create all the simulators of robots.
Initialize the velocity controller.
void setToolType(vpViper850::vpToolType tool)
Set the current tool type.
Initialize the acceleration controller.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
bool constantSamplingTimeMode
Flag used to force the sampling time in the thread computing the robot's displacement to a constant v...
void setCameraParameters(const vpCameraParameters cam)
vpCameraParametersProjType
static void display(const vpImage< unsigned char > &I)
void set_velocity(const vpColVector &vel)
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)
void setExternalCameraPosition(const vpHomogeneousMatrix camMf)
Modelisation of the ADEPT Viper 850 robot.
bool singularityManagement
void extract(vpRotationMatrix &R) const
double get_x() const
Get the point x coordinate in the image plane.
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...
bool setVelocityCalled
Flag used to specify to the thread managing the robot displacements that the setVelocity() method has...
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness=1)
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Construction from translation vector and rotation matrix.
static double rad(double deg)
void get_fJe(const vpColVector &q, vpMatrix &fJe)
void display_scene(Matrix mat, Bound_scene &sc, vpImage< vpRGBa > &I, vpColor color)
vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
void updateArticularPosition()
void initialiseObjectRelativeToCamera(vpHomogeneousMatrix cMo)
vpHomogeneousMatrix camMf
vpCameraParameters cameraParam
static double deg(double rad)
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Class that provides a data structure for the column vectors as well as a set of operations on these v...
The pose is a complete representation of every rigid motion in the euclidian space.
vpHomogeneousMatrix inverse() const
void initialiseCameraRelativeToObject(vpHomogeneousMatrix cMo)
void get_cMe(vpHomogeneousMatrix &cMe)
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.
void findHighestPositioningSpeed(vpColVector &q)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
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
vpDisplayRobotType displayType
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)
void setExternalCameraParameters(const vpCameraParameters cam)
static const double defaultPositioningVelocity
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
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)
static const unsigned int njoint
Number of joint.
vpHomogeneousMatrix get_fMc(const vpColVector &q)
void set_displayBusy(const bool &status)
vpHomogeneousMatrix camMf2
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)