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;
500 articularVelocities = 0.0;
505 articularCoordinates[0] = articularCoordinates[0] + ellapsedTime*articularVelocities[0]*1e-3;
506 articularCoordinates[1] = articularCoordinates[1] + ellapsedTime*articularVelocities[1]*1e-3;
507 articularCoordinates[2] = articularCoordinates[2] + ellapsedTime*articularVelocities[2]*1e-3;
508 articularCoordinates[3] = articularCoordinates[3] + ellapsedTime*articularVelocities[3]*1e-3;
509 articularCoordinates[4] = articularCoordinates[4] + ellapsedTime*articularVelocities[4]*1e-3;
510 articularCoordinates[5] = articularCoordinates[5] + ellapsedTime*articularVelocities[5]*1e-3;
517 ellapsedTime = (
_joint_min[(
unsigned int)(-jl-1)] - articularCoordinates[(
unsigned int)(-jl-1)])/(articularVelocities[(
unsigned int)(-jl-1)]*1e-3);
519 ellapsedTime = (
_joint_max[(
unsigned int)(jl-1)] - articularCoordinates[(
unsigned int)(jl-1)])/(articularVelocities[(
unsigned int)(jl-1)]*1e-3);
521 for (
unsigned int i = 0; i < 6; i++)
522 articularCoordinates[i] = articularCoordinates[i] + ellapsedTime*articularVelocities[i]*1e-3;
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;
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];
unsigned int jointLimitArt
vpHomogeneousMatrix get_cMo()
Modelisation of Irisa's gantry robot named Afma6.
Definition of the vpMatrix class.
void initialiseObjectRelativeToCamera(vpHomogeneousMatrix cMo)
vpHomogeneousMatrix * fMi
static const unsigned int njoint
Number of joint.
void initialiseCameraRelativeToObject(vpHomogeneousMatrix cMo)
Error that can be emited by the vpRobot class and its derivates.
vpColVector get_velocity()
unsigned int getWidth() const
void setMaxTranslationVelocity(const double maxVt)
void get_eJe(vpMatrix &eJe)
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
virtual ~vpSimulatorAfma6()
void get_cVe(vpVelocityTwistMatrix &cVe)
void move(const char *filename)
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 ...
vpHomogeneousMatrix get_fMc(const vpColVector &q)
static void displayCamera(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color)
virtual vpRobotStateType getRobotState(void)
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.
vpAfma6ToolType
List of possible tools that can be attached to the robot end-effector.
void updateArticularPosition()
void track(const vpHomogeneousMatrix &cMo)
vpHomogeneousMatrix getExternalCameraPosition() const
static const double defaultPositioningVelocity
void getCameraDisplacement(vpColVector &displacement)
vpRotationMatrix t() const
transpose
void getExternalImage(vpImage< vpRGBa > &I)
vpCameraParameters::vpCameraParametersProjType projModel
static double measureTimeMs()
double get_y() const
Get the point y coordinate in the image plane.
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
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
void set_artCoord(const vpColVector &coord)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
vpAfma6ToolType getToolType()
Get the current tool type.
vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
static DWORD WINAPI launcher(LPVOID lpParam)
static bool readPosFile(const char *filename, vpColVector &q)
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Class that defines what is a point.
void setToolType(vpAfma6::vpAfma6ToolType tool)
Set the current tool type.
static const char *const CONST_GRIPPER_CAMERA_NAME
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)
void get_cMe(vpHomogeneousMatrix &cMe)
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &displacement)
vpControlFrameType getRobotFrame(void)
bool singularityTest(const vpColVector q, vpMatrix &J)
This class aims to be a basis used to create all the simulators of robots.
Initialize the velocity controller.
void get_eJe(const vpColVector &q, vpMatrix &eJe)
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 getArticularDisplacement(vpColVector &displacement)
vpCameraParametersProjType
static void display(const vpImage< unsigned char > &I)
void set_velocity(const vpColVector &vel)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
Generic class defining intrinsic camera parameters.
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
void setExternalCameraPosition(const vpHomogeneousMatrix camMf)
bool singularityManagement
void extract(vpRotationMatrix &R) const
double get_x() const
Get the point x coordinate in the image plane.
void setCameraParameters(const vpCameraParameters cam)
void getInternalView(vpImage< vpRGBa > &I)
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...
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q)
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 display_scene(Matrix mat, Bound_scene &sc, vpImage< vpRGBa > &I, vpColor color)
int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest=true)
void setMaxRotationVelocity(const double maxVr)
double getPositioningVelocity(void)
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...
double euclideanNorm() const
void get_fJe(vpMatrix &fJe)
The pose is a complete representation of every rigid motion in the euclidian space.
void computeArticularVelocity()
vpHomogeneousMatrix inverse() const
void get_fJe(const vpColVector &q, vpMatrix &fJe)
unsigned int getHeight() const
Class that consider the case of the Euler angle using the x-y-z convention, where are respectively ...
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height)
vpControlFrameType setRobotFrame(vpRobot::vpControlFrameType newFrame)
void get_cMe(vpHomogeneousMatrix &cMe)
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Compute the pseudo inverse of the matrix using the SVD.
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
vpDisplayRobotType displayType
void setJointLimit(vpColVector limitMin, vpColVector limitMax)
void get_fMi(vpHomogeneousMatrix *fMit)
unsigned int getRows() const
Return the number of rows of the matrix.
static const char *const CONST_CCMOP_CAMERA_NAME
void setExternalCameraParameters(const vpCameraParameters cam)
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Class that consider the case of a translation vector.
Class that consider the case of the parameterization for the rotation.
void set_displayBusy(const bool &status)
vpHomogeneousMatrix camMf2
static bool savePosFile(const char *filename, const vpColVector &q)
void findHighestPositioningSpeed(vpColVector &q)
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 resize(const unsigned int i, const bool flagNullify=true)