40 #include <visp3/core/vpConfig.h> 41 #if defined(VISP_HAVE_MODULE_GUI) && ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD)) 42 #include <visp3/robot/vpSimulatorAfma6.h> 43 #include <visp3/core/vpTime.h> 44 #include <visp3/core/vpImagePoint.h> 45 #include <visp3/core/vpPoint.h> 46 #include <visp3/core/vpMeterPixelConversion.h> 47 #include <visp3/robot/vpRobotException.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);
88 DWORD dwThreadIdArray;
89 hThread = CreateThread(
96 #elif defined (VISP_HAVE_PTHREAD) 103 pthread_attr_init(&
attr);
104 pthread_attr_setdetachstate(&
attr, PTHREAD_CREATE_JOINABLE);
121 zeroPos(), reposPos(), toolCustom(false), arm_dir()
130 mutex_fMi = CreateMutexEx(NULL, NULL, 0, NULL);
136 mutex_fMi = CreateMutex(NULL, FALSE, NULL);
143 DWORD dwThreadIdArray;
144 hThread = CreateThread(
151 #elif defined(VISP_HAVE_PTHREAD) 158 pthread_attr_init(&
attr);
159 pthread_attr_setdetachstate(&
attr, PTHREAD_CREATE_JOINABLE);
175 # if defined(WINRT_8_1) 176 WaitForSingleObjectEx(hThread, INFINITE, FALSE);
178 WaitForSingleObject(hThread, INFINITE);
180 CloseHandle(hThread);
186 #elif defined(VISP_HAVE_PTHREAD) 187 pthread_attr_destroy(&
attr);
188 pthread_join(
thread, NULL);
198 for(
int i = 0; i < 6; i++)
219 std::vector<std::string> arm_dirs =
vpIoTools::splitChain(std::string(VISP_ROBOT_ARMS_DIR), std::string(
";"));
220 bool armDirExists =
false;
221 for(
size_t i=0; i < arm_dirs.size(); i++)
223 arm_dir = arm_dirs[i];
227 if (! armDirExists) {
230 std::cout <<
"The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
233 std::cout <<
"Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
249 reposPos[1] = -M_PI/2; reposPos[2] = M_PI; reposPos[4] = M_PI/2;
256 first_time_getdis =
true;
289 setExternalCameraPosition(
vpHomogeneousMatrix(0,0,0,0,0,
vpMath::rad(180))*
vpHomogeneousMatrix(-0.1,0,4,
vpMath::rad(90),0,0));
320 unsigned int name_length = 30;
321 if (arm_dir.size() > FILENAME_MAX)
323 unsigned int full_length = (
unsigned int)arm_dir.size() + name_length;
324 if (full_length > FILENAME_MAX)
343 char *name_arm =
new char [full_length];
344 strcpy(name_arm, arm_dir.c_str());
345 strcat(name_arm,
"/afma6_tool_ccmop.bnd");
366 char *name_arm =
new char [full_length];
367 strcpy(name_arm, arm_dir.c_str());
368 strcat(name_arm,
"/afma6_tool_gripper.bnd");
390 char *name_arm =
new char [full_length];
392 strcpy(name_arm, arm_dir.c_str());
393 strcat(name_arm,
"/afma6_tool_vacuum.bnd");
402 std::cout <<
"The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
425 const unsigned int &image_width,
426 const unsigned int &image_height)
436 if (image_width == 640 && image_height == 480)
438 std::cout <<
"Get default camera parameters for camera \"" 443 vpTRACE(
"Cannot get default intrinsic camera parameters for this image resolution");
449 if (image_width == 640 && image_height == 480) {
450 std::cout <<
"Get default camera parameters for camera \"" 455 vpTRACE(
"Cannot get default intrinsic camera parameters for this image resolution");
462 std::cout <<
"The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
523 double tcur_1 =
tcur;
536 double ellapsedTime = (
tcur -
tprev) * 1e-3;
554 articularVelocities = 0.0;
560 articularCoordinates[0] = articularCoordinates[0] + ellapsedTime*articularVelocities[0];
561 articularCoordinates[1] = articularCoordinates[1] + ellapsedTime*articularVelocities[1];
562 articularCoordinates[2] = articularCoordinates[2] + ellapsedTime*articularVelocities[2];
563 articularCoordinates[3] = articularCoordinates[3] + ellapsedTime*articularVelocities[3];
564 articularCoordinates[4] = articularCoordinates[4] + ellapsedTime*articularVelocities[4];
565 articularCoordinates[5] = articularCoordinates[5] + ellapsedTime*articularVelocities[5];
572 ellapsedTime = (
_joint_min[(
unsigned int)(-jl-1)] - articularCoordinates[(
unsigned int)(-jl-1)])/(articularVelocities[(
unsigned int)(-jl-1)]);
574 ellapsedTime = (
_joint_max[(
unsigned int)(jl-1)] - articularCoordinates[(
unsigned int)(jl-1)])/(articularVelocities[(
unsigned int)(jl-1)]);
576 for (
unsigned int i = 0; i < 6; i++)
577 articularCoordinates[i] = articularCoordinates[i] + ellapsedTime*articularVelocities[i];
618 for (
unsigned int k = 1; k < 7; k++)
724 fMit[4][0][0] = s4*s5;
725 fMit[4][1][0] = -c4*s5;
727 fMit[4][0][1] = s4*c5;
728 fMit[4][1][1] = -c4*c5;
733 fMit[4][0][3] = c4*this->
_long_56+q1;
734 fMit[4][1][3] = s4*this->
_long_56+q2;
737 fMit[5][0][0] = s4*s5*c6+c4*s6;
738 fMit[5][1][0] = -c4*s5*c6+s4*s6;
739 fMit[5][2][0] = c5*c6;
740 fMit[5][0][1] = -s4*s5*s6+c4*c6;
741 fMit[5][1][1] = c4*s5*s6+s4*c6;
742 fMit[5][2][1] = -c5*s6;
743 fMit[5][0][2] = -s4*c5;
744 fMit[5][1][2] = c4*c5;
746 fMit[5][0][3] = c4*this->
_long_56+q1;
747 fMit[5][1][3] = s4*this->
_long_56+q2;
750 fMit[6][0][0] = fMit[5][0][0];
751 fMit[6][1][0] = fMit[5][1][0];
752 fMit[6][2][0] = fMit[5][2][0];
753 fMit[6][0][1] = fMit[5][0][1];
754 fMit[6][1][1] = fMit[5][1][1];
755 fMit[6][2][1] = fMit[5][2][1];
756 fMit[6][0][2] = fMit[5][0][2];
757 fMit[6][1][2] = fMit[5][1][2];
758 fMit[6][2][2] = fMit[5][2][2];
759 fMit[6][0][3] = fMit[5][0][3];
760 fMit[6][1][3] = fMit[5][1][3];
761 fMit[6][2][3] = fMit[5][2][3];
770 # if defined(WINRT_8_1) 771 WaitForSingleObjectEx(
mutex_fMi, INFINITE, FALSE);
773 WaitForSingleObject(
mutex_fMi, INFINITE);
775 for (
int i = 0; i < 8; i++)
778 #elif defined(VISP_HAVE_PTHREAD) 780 for (
int i = 0; i < 8; i++)
805 std::cout <<
"Change the control mode from velocity to position control.\n";
815 std::cout <<
"Change the control mode from stop to velocity control.\n";
901 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
903 "Cannot send a velocity to the robot " 904 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
909 double scale_sat = 1;
924 vpERROR_TRACE (
"The velocity vector must have a size of 6 !!!!");
928 for (
unsigned int i = 0 ; i < 3; ++ i)
930 vel_abs = fabs (vel[i]);
933 vel_trans_max = vel_abs;
935 "(axis nr. %d).", vel[i], i+1);
938 vel_abs = fabs (vel[i+3]);
940 vel_rot_max = vel_abs;
942 "(axis nr. %d).", vel[i+3], i+4);
946 double scale_trans_sat = 1;
947 double scale_rot_sat = 1;
954 if ( (scale_trans_sat < 1) || (scale_rot_sat < 1) )
956 if (scale_trans_sat < scale_rot_sat)
957 scale_sat = scale_trans_sat;
959 scale_sat = scale_rot_sat;
969 vpERROR_TRACE (
"The velocity vector must have a size of 6 !!!!");
972 for (
unsigned int i = 0 ; i < 6; ++ i)
974 vel_abs = fabs (vel[i]);
977 vel_rot_max = vel_abs;
979 "(axis nr. %d).", vel[i], i+1);
982 double scale_rot_sat = 1;
985 if ( scale_rot_sat < 1 )
986 scale_sat = scale_rot_sat;
1025 articularVelocity = eJe_*eVc*velocityframe;
1036 articularVelocity = fJe_*velocityframe;
1042 articularVelocity = velocityframe;
1057 for (
unsigned int i = 0 ; i < 6; ++ i)
1059 double vel_abs = fabs (articularVelocity[i]);
1062 vel_rot_max = vel_abs;
1064 "(axis nr. %d).", articularVelocity[i], i+1);
1067 double scale_rot_sat = 1;
1068 double scale_sat = 1;
1071 if ( scale_rot_sat < 1 )
1072 scale_sat = scale_rot_sat;
1147 vel = cVe*eJe_*articularVelocity;
1152 vel = articularVelocity;
1159 vel = fJe_*articularVelocity;
1169 "Case not taken in account.");
1275 double velmax = fabs(q[0]);
1276 for (
unsigned int i = 1; i < 6; i++)
1278 if (velmax < fabs(q[i]))
1279 velmax = fabs(q[i]);
1366 "Modification of the robot state");
1383 for (
unsigned int i=0; i < 3; i++)
1400 qdes = articularCoordinates;
1405 error = qdes - articularCoordinates;
1422 "Position out of range.");
1424 }
while (errsqr > 1e-8 && nbSol > 0);
1434 error = q - articularCoordinates;
1447 }
while (errsqr > 1e-8);
1458 for (
unsigned int i=0; i < 3; i++)
1470 qdes = articularCoordinates;
1475 error = qdes - articularCoordinates;
1490 }
while (errsqr > 1e-8 && nbSol > 0);
1495 vpERROR_TRACE (
"Positionning error. Mixt frame not implemented");
1497 "Positionning error: " 1498 "Mixt frame not implemented.");
1576 position[0] = pos1 ;
1577 position[1] = pos2 ;
1578 position[2] = pos3 ;
1579 position[3] = pos4 ;
1580 position[4] = pos5 ;
1581 position[5] = pos6 ;
1637 "Bad position in filename.");
1733 for (
unsigned int i=0; i <3; i++)
1743 vpERROR_TRACE (
"Positionning error. Mixt frame not implemented");
1745 "Positionning error: " 1746 "Mixt frame not implemented.");
1806 for(
unsigned int j=0;j<3;j++)
1808 position[j]=posRxyz[j];
1809 position[j+3]=RtuVect[j];
1842 vpTRACE(
"Joint limit vector has not a size of 6 !");
1872 bool cond = fabs(q5-M_PI/2) < 1e-1;
1902 for (
unsigned int i = 0; i < 6; i++)
1904 if (articularCoordinates[i] <=
_joint_min[i])
1906 difft =
_joint_min[i] - articularCoordinates[i];
1910 artNumb = -(int)i-1;
1915 for (
unsigned int i = 0; i < 6; i++)
1917 if (articularCoordinates[i] >=
_joint_max[i])
1919 difft = articularCoordinates[i] -
_joint_max[i];
1923 artNumb = (int)(i+1);
1929 std::cout <<
"\nWarning: Velocity control stopped: axis " << fabs((
float)artNumb) <<
" on joint limit!" <<std::endl;
1946 vpSimulatorAfma6::getCameraDisplacement(
vpColVector &displacement)
1961 vpSimulatorAfma6::getArticularDisplacement(
vpColVector &displacement)
1994 if ( ! first_time_getdis )
2000 std::cout <<
"getDisplacement() CAMERA_FRAME not implemented\n";
2006 displacement = q_cur - q_prev_getdis;
2012 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
2018 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
2025 first_time_getdis =
false;
2029 q_prev_getdis = q_cur;
2081 std::ifstream fd(filename.c_str(), std::ios::in);
2083 if(! fd.is_open()) {
2088 std::string key(
"R:");
2089 std::string id(
"#AFMA6 - Position");
2090 bool pos_found =
false;
2095 while(std::getline(fd, line)) {
2098 if(! (line.compare(0,
id.size(), id) == 0)) {
2099 std::cout <<
"Error: this position file " << filename <<
" is not for Afma6 robot" << std::endl;
2103 if((line.compare(0, 1,
"#") == 0)) {
2106 if((line.compare(0, key.size(), key) == 0)) {
2109 if (chain.size() <
njoint+1)
2111 if(chain.size() <
njoint+1)
2114 std::istringstream ss(line);
2117 for (
unsigned int i=0; i<
njoint; i++)
2132 std::cout <<
"Error: unable to find a position for Afma6 robot in " << filename << std::endl;
2165 fd = fopen(filename.c_str(),
"w") ;
2170 #AFMA6 - Position - Version 2.01\n\ 2173 # Joint position: X, Y, Z: translations in meters\n\ 2174 # A, B, C: rotations in degrees\n\ 2179 fprintf(fd,
"R: %lf %lf %lf %lf %lf %lf\n",
2330 std::string scene_dir_;
2331 std::vector<std::string> scene_dirs =
vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(
";"));
2332 bool sceneDirExists =
false;
2333 for(
size_t i=0; i < scene_dirs.size(); i++)
2335 scene_dir_ = scene_dirs[i];
2336 sceneDirExists =
true;
2339 if (! sceneDirExists) {
2342 std::cout <<
"The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
2345 std::cout <<
"Cannot get VISP_SCENES_DIR environment variable" << std::endl;
2350 unsigned int name_length = 30;
2351 if (scene_dir_.size() > FILENAME_MAX)
2353 unsigned int full_length = (
unsigned int)scene_dir_.size() + name_length;
2354 if (full_length > FILENAME_MAX)
2357 char *name_cam =
new char [full_length];
2359 strcpy(name_cam, scene_dir_.c_str());
2360 strcat(name_cam,
"/camera.bnd");
2363 if (arm_dir.size() > FILENAME_MAX)
2365 full_length = (
unsigned int)arm_dir.size() + name_length;
2366 if (full_length > FILENAME_MAX)
2369 char *name_arm =
new char [full_length];
2370 strcpy(name_arm, arm_dir.c_str());
2371 strcat(name_arm,
"/afma6_gate.bnd");
2372 std::cout <<
"name arm: " << name_arm << std::endl;
2374 strcpy(name_arm, arm_dir.c_str());
2375 strcat(name_arm,
"/afma6_arm1.bnd");
2377 strcpy(name_arm, arm_dir.c_str());
2378 strcat(name_arm,
"/afma6_arm2.bnd");
2380 strcpy(name_arm, arm_dir.c_str());
2381 strcat(name_arm,
"/afma6_arm3.bnd");
2383 strcpy(name_arm, arm_dir.c_str());
2384 strcat(name_arm,
"/afma6_arm4.bnd");
2389 strcpy(name_arm, arm_dir.c_str());
2392 strcat(name_arm,
"/afma6_tool_ccmop.bnd");
2396 strcat(name_arm,
"/afma6_tool_gripper.bnd");
2400 strcat(name_arm,
"/afma6_tool_vacuum.bnd");
2404 std::cout <<
"The custom tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
2408 std::cout <<
"The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
2414 add_rfstack(IS_BACK);
2416 add_vwstack (
"start",
"depth", 0.0, 100.0);
2417 add_vwstack (
"start",
"window", -0.1,0.1,-0.1,0.1);
2418 add_vwstack (
"start",
"type", PERSPECTIVE);
2432 bool changed =
false;
2436 if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
2459 float w44o[4][4],w44cext[4][4],x,y,z;
2463 add_vwstack (
"start",
"cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
2464 x = w44cext[2][0] + w44cext[3][0];
2465 y = w44cext[2][1] + w44cext[3][1];
2466 z = w44cext[2][2] + w44cext[3][2];
2467 add_vwstack (
"start",
"vrp", x,y,z);
2468 add_vwstack (
"start",
"vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
2469 add_vwstack (
"start",
"vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
2470 add_vwstack (
"start",
"window", -u, u, -v, v);
2478 vp2jlc_matrix(fMit[0],w44o);
2481 vp2jlc_matrix(fMit[2],w44o);
2484 vp2jlc_matrix(fMit[3],w44o);
2487 vp2jlc_matrix(fMit[4],w44o);
2490 vp2jlc_matrix(fMit[5],w44o);
2498 cMe = fMit[6] * cMe;
2499 vp2jlc_matrix(cMe,w44o);
2505 vp2jlc_matrix(
fMo,w44o);
2546 std::cout <<
"Used joint coordinates (rad): " << articularCoordinates.
t() << std::endl;
2576 fMo = fMit[7] * cMo_;
2602 const double lambda = 5.;
2606 unsigned int i,iter=0;
2626 v = -lambda*cdRc.
t()*cdTc;
2633 err[i+3] = cdTUc[i];
2652 #elif !defined(VISP_BUILD_SHARED_LIBS) 2654 void dummy_vpSimulatorAfma6() {};
static void displayCamera(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness)
Modelisation of Irisa's gantry robot named Afma6.
double euclideanNorm() const
Implementation of a matrix and operations on matrices.
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Compute the pseudo inverse of the matrix using the SVD.
VISP_EXPORT int wait(double t0, double t)
static const unsigned int njoint
Number of joint.
Error that can be emited by the vpRobot class and its derivates.
vpColVector get_artCoord()
vpHomogeneousMatrix getExternalCameraPosition() const
void setMaxTranslationVelocity(const double maxVt)
void get_eJe(vpMatrix &eJe)
Implementation of an homogeneous matrix and operations on such kind of matrices.
virtual ~vpSimulatorAfma6()
unsigned int jointLimitArt
void get_cVe(vpVelocityTwistMatrix &cVe)
void move(const char *filename)
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
int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest=true, const bool &verbose=false) const
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.
vpAfma6ToolType
List of possible tools that can be attached to the robot end-effector.
void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
void updateArticularPosition()
unsigned int getRows() const
Return the number of rows of the 2D array.
void track(const vpHomogeneousMatrix &cMo)
vpHomogeneousMatrix inverse() const
static const double defaultPositioningVelocity
vpColVector get_velocity()
static bool savePosFile(const std::string &filename, const vpColVector &q)
void getExternalImage(vpImage< vpRGBa > &I)
vpCameraParameters::vpCameraParametersProjType projModel
vpAfma6ToolType getToolType() const
Get the current tool type.
double getMaxRotationVelocity(void) const
void extract(vpRotationMatrix &R) const
static const vpColor green
static void flush(const vpImage< unsigned char > &I)
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
vpRotationMatrix t() const
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
VISP_EXPORT double measureTimeMs()
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Class that defines what is a point.
void setToolType(vpAfma6::vpAfma6ToolType tool)
Set the current tool type.
bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo)
static const char *const CONST_GRIPPER_CAMERA_NAME
static Type maximum(const Type &a, const Type &b)
Implementation of a rotation matrix and operations on such kind of matrices.
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &displacement)
pthread_mutex_t mutex_artCoord
pthread_mutex_t mutex_display
bool singularityTest(const vpColVector q, vpMatrix &J)
vpDisplayRobotType displayType
Initialize the velocity controller.
static bool readPosFile(const std::string &filename, vpColVector &q)
virtual vpRobotStateType getRobotState(void) const
vpCameraParameters cameraParam
void initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo)
void setExternalCameraPosition(const vpHomogeneousMatrix camMf_)
Initialize the acceleration controller.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void set_displayBusy(const bool &status)
vpCameraParametersProjType
static void display(const vpImage< unsigned char > &I)
void getInternalView(vpImage< vpRGBa > &I)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Generic class defining intrinsic camera parameters.
vpHomogeneousMatrix get_cMo()
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
static Type minimum(const Type &a, const Type &b)
void setCameraParameters(const vpCameraParameters &cam)
Implementation of a velocity twist matrix and operations on such kind of matrices.
double getSamplingTime() const
void set_velocity(const vpColVector &vel)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q)
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
static double rad(double deg)
void setExternalCameraParameters(const vpCameraParameters &cam)
This class aims to be a basis used to create all the simulators of robots.
vpHomogeneousMatrix * fMi
void setMaxRotationVelocity(const double maxVr)
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))
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)
double getPositioningVelocity(void)
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 get_fJe(vpMatrix &fJe)
Implementation of a pose vector and operations on poses.
void computeArticularVelocity()
void get_cMe(vpHomogeneousMatrix &cMe) const
double get_y() const
Get the point y coordinate in the image plane.
Implementation of a rotation vector as Euler angle minimal representation.
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height)
vpControlFrameType setRobotFrame(vpRobot::vpControlFrameType newFrame)
vpControlFrameType getRobotFrame(void) const
void get_cMe(vpHomogeneousMatrix &cMe)
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)
VISP_EXPORT double getMinTimeForUsleepCall()
void set_artVel(const vpColVector &vel)
void get_fMi(vpHomogeneousMatrix *fMit)
static const char *const CONST_CCMOP_CAMERA_NAME
unsigned int getWidth() const
pthread_mutex_t mutex_velocity
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
pthread_mutex_t mutex_artVel
vpHomogeneousMatrix camMf2
void findHighestPositioningSpeed(vpColVector &q)
void resize(const unsigned int i, const bool flagNullify=true)