36 #include <visp3/core/vpConfig.h>
37 #if defined(VISP_HAVE_MODULE_GUI) && ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD))
41 #include <visp3/core/vpImagePoint.h>
42 #include <visp3/core/vpIoTools.h>
43 #include <visp3/core/vpMeterPixelConversion.h>
44 #include <visp3/core/vpPoint.h>
45 #include <visp3/core/vpTime.h>
46 #include <visp3/robot/vpRobotException.h>
47 #include <visp3/robot/vpSimulatorAfma6.h>
49 #include "../wireframe-simulator/vpBound.h"
50 #include "../wireframe-simulator/vpRfstack.h"
51 #include "../wireframe-simulator/vpScene.h"
52 #include "../wireframe-simulator/vpVwstack.h"
61 positioningVelocity(defaultPositioningVelocity), zeroPos(), reposPos(), toolCustom(false), arm_dir()
69 DWORD dwThreadIdArray;
70 hThread = CreateThread(NULL,
76 #elif defined(VISP_HAVE_PTHREAD)
77 pthread_attr_init(&
attr);
78 pthread_attr_setdetachstate(&
attr, PTHREAD_CREATE_JOINABLE);
94 positioningVelocity(defaultPositioningVelocity), zeroPos(), reposPos(), toolCustom(false), arm_dir()
102 DWORD dwThreadIdArray;
103 hThread = CreateThread(NULL,
109 #elif defined(VISP_HAVE_PTHREAD)
110 pthread_attr_init(&
attr);
111 pthread_attr_setdetachstate(&
attr, PTHREAD_CREATE_JOINABLE);
129 #if defined(WINRT_8_1)
130 WaitForSingleObjectEx(hThread, INFINITE, FALSE);
132 WaitForSingleObject(hThread, INFINITE);
134 CloseHandle(hThread);
135 #elif defined(VISP_HAVE_PTHREAD)
136 pthread_attr_destroy(&
attr);
137 pthread_join(
thread, NULL);
141 for (
int i = 0; i < 6; i++)
161 std::vector<std::string> arm_dirs =
vpIoTools::splitChain(std::string(VISP_ROBOT_ARMS_DIR), std::string(
";"));
162 bool armDirExists =
false;
163 for (
size_t i = 0; i < arm_dirs.size(); i++)
165 arm_dir = arm_dirs[i];
172 std::cout <<
"The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
174 std::cout <<
"Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
190 reposPos[1] = -M_PI / 2;
192 reposPos[4] = M_PI / 2;
199 first_time_getdis =
true;
260 unsigned int name_length = 30;
261 if (arm_dir.size() > FILENAME_MAX)
263 unsigned int full_length = (
unsigned int)arm_dir.size() + name_length;
264 if (full_length > FILENAME_MAX)
283 char *name_arm =
new char[full_length];
284 strcpy(name_arm, arm_dir.c_str());
285 strcat(name_arm,
"/afma6_tool_ccmop.bnd");
306 char *name_arm =
new char[full_length];
307 strcpy(name_arm, arm_dir.c_str());
308 strcat(name_arm,
"/afma6_tool_gripper.bnd");
330 char *name_arm =
new char[full_length];
332 strcpy(name_arm, arm_dir.c_str());
333 strcat(name_arm,
"/afma6_tool_vacuum.bnd");
341 std::cout <<
"The custom tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
345 std::cout <<
"The Intel D435 camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
349 std::cout <<
"The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
375 const unsigned int &image_height)
384 if (image_width == 640 && image_height == 480) {
389 vpTRACE(
"Cannot get default intrinsic camera parameters for this image "
396 if (image_width == 640 && image_height == 480) {
401 vpTRACE(
"Cannot get default intrinsic camera parameters for this image "
407 std::cout <<
"The generic tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
411 std::cout <<
"The Intel D435 camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
415 std::cout <<
"The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
419 std::cout <<
"The vacuum tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
473 double tcur_1 =
tcur;
476 bool setVelocityCalled_ =
false;
493 double ellapsedTime = (
tcur -
tprev) * 1e-3;
512 articularVelocities = 0.0;
517 articularCoordinates[0] = articularCoordinates[0] + ellapsedTime * articularVelocities[0];
518 articularCoordinates[1] = articularCoordinates[1] + ellapsedTime * articularVelocities[1];
519 articularCoordinates[2] = articularCoordinates[2] + ellapsedTime * articularVelocities[2];
520 articularCoordinates[3] = articularCoordinates[3] + ellapsedTime * articularVelocities[3];
521 articularCoordinates[4] = articularCoordinates[4] + ellapsedTime * articularVelocities[4];
522 articularCoordinates[5] = articularCoordinates[5] + ellapsedTime * articularVelocities[5];
528 ellapsedTime = (
_joint_min[(
unsigned int)(-jl - 1)] - articularCoordinates[(
unsigned int)(-jl - 1)]) /
529 (articularVelocities[(
unsigned int)(-jl - 1)]);
531 ellapsedTime = (
_joint_max[(
unsigned int)(jl - 1)] - articularCoordinates[(
unsigned int)(jl - 1)]) /
532 (articularVelocities[(
unsigned int)(jl - 1)]);
534 for (
unsigned int i = 0; i < 6; i++)
535 articularCoordinates[i] = articularCoordinates[i] + ellapsedTime * articularVelocities[i];
575 for (
unsigned int k = 1; k < 7; k++) {
689 fMit[4][0][0] = s4 * s5;
690 fMit[4][1][0] = -c4 * s5;
692 fMit[4][0][1] = s4 * c5;
693 fMit[4][1][1] = -c4 * c5;
698 fMit[4][0][3] = c4 * this->
_long_56 + q1;
699 fMit[4][1][3] = s4 * this->
_long_56 + q2;
702 fMit[5][0][0] = s4 * s5 * c6 + c4 * s6;
703 fMit[5][1][0] = -c4 * s5 * c6 + s4 * s6;
704 fMit[5][2][0] = c5 * c6;
705 fMit[5][0][1] = -s4 * s5 * s6 + c4 * c6;
706 fMit[5][1][1] = c4 * s5 * s6 + s4 * c6;
707 fMit[5][2][1] = -c5 * s6;
708 fMit[5][0][2] = -s4 * c5;
709 fMit[5][1][2] = c4 * c5;
711 fMit[5][0][3] = c4 * this->
_long_56 + q1;
712 fMit[5][1][3] = s4 * this->
_long_56 + q2;
715 fMit[6][0][0] = fMit[5][0][0];
716 fMit[6][1][0] = fMit[5][1][0];
717 fMit[6][2][0] = fMit[5][2][0];
718 fMit[6][0][1] = fMit[5][0][1];
719 fMit[6][1][1] = fMit[5][1][1];
720 fMit[6][2][1] = fMit[5][2][1];
721 fMit[6][0][2] = fMit[5][0][2];
722 fMit[6][1][2] = fMit[5][1][2];
723 fMit[6][2][2] = fMit[5][2][2];
724 fMit[6][0][3] = fMit[5][0][3];
725 fMit[6][1][3] = fMit[5][1][3];
726 fMit[6][2][3] = fMit[5][2][3];
737 for (
int i = 0; i < 8; i++) {
760 std::cout <<
"Change the control mode from velocity to position control.\n";
770 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;
879 vpERROR_TRACE(
"The velocity vector must have a size of 6 !!!!");
883 for (
unsigned int i = 0; i < 3; ++i) {
884 vel_abs = fabs(vel[i]);
886 vel_trans_max = vel_abs;
892 vel_abs = fabs(vel[i + 3]);
894 vel_rot_max = vel_abs;
901 double scale_trans_sat = 1;
902 double scale_rot_sat = 1;
909 if ((scale_trans_sat < 1) || (scale_rot_sat < 1)) {
910 if (scale_trans_sat < scale_rot_sat)
911 scale_sat = scale_trans_sat;
913 scale_sat = scale_rot_sat;
921 vpERROR_TRACE(
"The velocity vector must have a size of 6 !!!!");
924 for (
unsigned int i = 0; i < 6; ++i) {
925 vel_abs = fabs(vel[i]);
927 vel_rot_max = vel_abs;
933 double scale_rot_sat = 1;
936 if (scale_rot_sat < 1)
937 scale_sat = scale_rot_sat;
942 "functionality not implemented");
946 "functionality not implemented");
986 articularVelocity = eJe_ * eVc * velocityframe;
996 articularVelocity = fJe_ * velocityframe;
1001 articularVelocity = velocityframe;
1014 for (
unsigned int i = 0; i < 6; ++i) {
1015 double vel_abs = fabs(articularVelocity[i]);
1017 vel_rot_max = vel_abs;
1020 articularVelocity[i], i + 1);
1023 double scale_rot_sat = 1;
1024 double scale_sat = 1;
1027 if (scale_rot_sat < 1)
1028 scale_sat = scale_rot_sat;
1098 vel = cVe * eJe_ * articularVelocity;
1102 vel = articularVelocity;
1108 vel = fJe_ * articularVelocity;
1117 "Case not taken in account.");
1218 double velmax = fabs(q[0]);
1219 for (
unsigned int i = 1; i < 6; i++) {
1220 if (velmax < fabs(q[i]))
1221 velmax = fabs(q[i]);
1306 "Modification of the robot state");
1321 for (
unsigned int i = 0; i < 3; i++) {
1336 qdes = articularCoordinates;
1344 error = qdes - articularCoordinates;
1348 if (errsqr < 1e-4) {
1359 }
while (errsqr > 1e-8 && nbSol > 0);
1367 error = q - articularCoordinates;
1374 if (errsqr < 1e-4) {
1381 }
while (errsqr > 1e-8);
1391 for (
unsigned int i = 0; i < 3; i++) {
1401 qdes = articularCoordinates;
1407 error = qdes - articularCoordinates;
1411 if (errsqr < 1e-4) {
1420 }
while (errsqr > 1e-8 && nbSol > 0);
1424 vpERROR_TRACE(
"Positioning error. Mixt frame not implemented");
1426 "MIXT_FRAME not implemented.");
1429 vpERROR_TRACE(
"Positioning error. Mixt frame not implemented");
1431 "END_EFFECTOR_FRAME not implemented.");
1499 double pos4,
double pos5,
double pos6)
1651 for (
unsigned int i = 0; i < 3; i++) {
1660 "Mixt frame not implemented.");
1664 "End-effector frame not implemented.");
1722 for (
unsigned int j = 0; j < 3; j++) {
1723 position[j] = posRxyz[j];
1724 position[j + 3] = RtuVect[j];
1758 vpTRACE(
"Joint limit vector has not a size of 6 !");
1786 bool cond = fabs(q5 - M_PI / 2) < 1e-1;
1814 for (
unsigned int i = 0; i < 6; i++) {
1815 if (articularCoordinates[i] <=
_joint_min[i]) {
1816 difft =
_joint_min[i] - articularCoordinates[i];
1819 artNumb = -(int)i - 1;
1824 for (
unsigned int i = 0; i < 6; i++) {
1825 if (articularCoordinates[i] >=
_joint_max[i]) {
1826 difft = articularCoordinates[i] -
_joint_max[i];
1829 artNumb = (int)(i + 1);
1835 std::cout <<
"\nWarning: Velocity control stopped: axis " << fabs((
float)artNumb) <<
" on joint limit!"
1866 if (!first_time_getdis) {
1869 std::cout <<
"getDisplacement() CAMERA_FRAME not implemented\n";
1873 displacement = q_cur - q_prev_getdis;
1877 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
1881 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
1885 std::cout <<
"getDisplacement() END_EFFECTOR_FRAME not implemented\n";
1890 first_time_getdis =
false;
1894 q_prev_getdis = q_cur;
1946 std::ifstream fd(filename.c_str(), std::ios::in);
1948 if (!fd.is_open()) {
1953 std::string key(
"R:");
1954 std::string id(
"#AFMA6 - Position");
1955 bool pos_found =
false;
1960 while (std::getline(fd, line)) {
1963 if (!(line.compare(0,
id.size(),
id) == 0)) {
1964 std::cout <<
"Error: this position file " << filename <<
" is not for Afma6 robot" << std::endl;
1968 if ((line.compare(0, 1,
"#") == 0)) {
1971 if ((line.compare(0, key.size(), key) == 0)) {
1974 if (chain.size() <
njoint + 1)
1976 if (chain.size() <
njoint + 1)
1979 std::istringstream ss(line);
1982 for (
unsigned int i = 0; i <
njoint; i++)
1997 std::cout <<
"Error: unable to find a position for Afma6 robot in " << filename << std::endl;
2029 fd = fopen(filename.c_str(),
"w");
2034 #AFMA6 - Position - Version 2.01\n\
2037 # Joint position: X, Y, Z: translations in meters\n\
2038 # A, B, C: rotations in degrees\n\
2168 std::string scene_dir_;
2169 std::vector<std::string> scene_dirs =
vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(
";"));
2170 bool sceneDirExists =
false;
2171 for (
size_t i = 0; i < scene_dirs.size(); i++)
2173 scene_dir_ = scene_dirs[i];
2174 sceneDirExists =
true;
2177 if (!sceneDirExists) {
2180 std::cout <<
"The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
2182 std::cout <<
"Cannot get VISP_SCENES_DIR environment variable" << std::endl;
2186 unsigned int name_length = 30;
2187 if (scene_dir_.size() > FILENAME_MAX)
2189 unsigned int full_length = (
unsigned int)scene_dir_.size() + name_length;
2190 if (full_length > FILENAME_MAX)
2193 char *name_cam =
new char[full_length];
2195 strcpy(name_cam, scene_dir_.c_str());
2196 strcat(name_cam,
"/camera.bnd");
2199 if (arm_dir.size() > FILENAME_MAX)
2201 full_length = (
unsigned int)arm_dir.size() + name_length;
2202 if (full_length > FILENAME_MAX)
2205 char *name_arm =
new char[full_length];
2206 strcpy(name_arm, arm_dir.c_str());
2207 strcat(name_arm,
"/afma6_gate.bnd");
2208 std::cout <<
"name arm: " << name_arm << std::endl;
2210 strcpy(name_arm, arm_dir.c_str());
2211 strcat(name_arm,
"/afma6_arm1.bnd");
2212 set_scene(name_arm,
robotArms + 1, 1.0);
2213 strcpy(name_arm, arm_dir.c_str());
2214 strcat(name_arm,
"/afma6_arm2.bnd");
2215 set_scene(name_arm,
robotArms + 2, 1.0);
2216 strcpy(name_arm, arm_dir.c_str());
2217 strcat(name_arm,
"/afma6_arm3.bnd");
2218 set_scene(name_arm,
robotArms + 3, 1.0);
2219 strcpy(name_arm, arm_dir.c_str());
2220 strcat(name_arm,
"/afma6_arm4.bnd");
2221 set_scene(name_arm,
robotArms + 4, 1.0);
2225 strcpy(name_arm, arm_dir.c_str());
2228 strcat(name_arm,
"/afma6_tool_ccmop.bnd");
2232 strcat(name_arm,
"/afma6_tool_gripper.bnd");
2236 strcat(name_arm,
"/afma6_tool_vacuum.bnd");
2240 std::cout <<
"The custom tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
2244 std::cout <<
"The Intel D435 camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
2248 std::cout <<
"The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
2252 set_scene(name_arm,
robotArms + 5, 1.0);
2254 add_rfstack(IS_BACK);
2256 add_vwstack(
"start",
"depth", 0.0, 100.0);
2257 add_vwstack(
"start",
"window", -0.1, 0.1, -0.1, 0.1);
2258 add_vwstack(
"start",
"type", PERSPECTIVE);
2271 bool changed =
false;
2275 if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
2295 float w44o[4][4], w44cext[4][4], x, y, z;
2299 add_vwstack(
"start",
"cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
2300 x = w44cext[2][0] + w44cext[3][0];
2301 y = w44cext[2][1] + w44cext[3][1];
2302 z = w44cext[2][2] + w44cext[3][2];
2303 add_vwstack(
"start",
"vrp", x, y, z);
2304 add_vwstack(
"start",
"vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
2305 add_vwstack(
"start",
"vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
2306 add_vwstack(
"start",
"window", -u, u, -v, v);
2314 vp2jlc_matrix(fMit[0], w44o);
2317 vp2jlc_matrix(fMit[2], w44o);
2320 vp2jlc_matrix(fMit[3], w44o);
2323 vp2jlc_matrix(fMit[4], w44o);
2326 vp2jlc_matrix(fMit[5], w44o);
2333 cMe = fMit[6] * cMe;
2334 vp2jlc_matrix(cMe, w44o);
2339 vp2jlc_matrix(
fMo, w44o);
2382 std::cout <<
"Used joint coordinates (rad): " << articularCoordinates.
t() << std::endl;
2414 fMo = fMit[7] * cMo_;
2444 const double lambda = 5.;
2448 unsigned int i, iter = 0;
2466 v = -lambda * cdRc.
t() * cdTc;
2467 w = -lambda * cdTUc;
2468 for (i = 0; i < 3; ++i) {
2472 err[i + 3] = cdTUc[i];
2491 #elif !defined(VISP_BUILD_SHARED_LIBS)
2494 void dummy_vpSimulatorAfma6(){};
Modelisation of Irisa's gantry robot named Afma6.
static const char *const CONST_CCMOP_CAMERA_NAME
static const unsigned int njoint
Number of joint.
int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest=true, const bool &verbose=false) const
void get_cMe(vpHomogeneousMatrix &cMe) const
void setToolType(vpAfma6::vpAfma6ToolType tool)
Set the current tool type.
vpAfma6ToolType getToolType() const
Get the current tool type.
static const char *const CONST_GRIPPER_CAMERA_NAME
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
vpCameraParameters::vpCameraParametersProjType projModel
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
vpAfma6ToolType
List of possible tools that can be attached to the robot end-effector.
unsigned int getRows() const
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
vpCameraParametersProjType
Implementation of column vector and the associated operations.
double frobeniusNorm() const
void resize(unsigned int i, bool flagNullify=true)
static const vpColor none
static const vpColor green
static void display(const vpImage< unsigned char > &I)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
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, const vpImagePoint &offset=vpImagePoint(0, 0), const std::string &frameName="", const vpColor &textColor=vpColor::black, const vpImagePoint &textOffset=vpImagePoint(15, 15))
static void flush(const vpImage< unsigned char > &I)
static void displayCamera(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness)
error that can be emitted by ViSP classes.
@ dimensionError
Bad dimension.
void track(const vpHomogeneousMatrix &cMo)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void extract(vpRotationMatrix &R) const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
unsigned int getWidth() const
unsigned int getHeight() const
static double rad(double deg)
static Type maximum(const Type &a, const Type &b)
static Type minimum(const Type &a, const Type &b)
static double deg(double rad)
Implementation of a matrix and operations on matrices.
vpMatrix pseudoInverse(double svThreshold=1e-6) const
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
double get_y() const
Get the point y coordinate in the image plane.
double get_x() const
Get the point x coordinate in the image plane.
Implementation of a pose vector and operations on poses.
Error that can be emitted by the vpRobot class and its derivatives.
@ wrongStateError
Wrong robot state.
@ positionOutOfRangeError
Position is out of range.
@ lowLevelError
Error thrown by the low level sdk.
double getSamplingTime() const
This class aims to be a basis used to create all the simulators of robots.
vpHomogeneousMatrix get_cMo()
vpColVector get_velocity()
vpColVector get_artCoord()
void set_velocity(const vpColVector &vel)
void set_displayBusy(const bool &status)
vpHomogeneousMatrix * fMi
void getInternalView(vpImage< vpRGBa > &I)
vpCameraParameters cameraParam
vpHomogeneousMatrix getExternalCameraPosition() const
static void * launcher(void *arg)
vpDisplayRobotType displayType
void set_artCoord(const vpColVector &coord)
unsigned int jointLimitArt
vpMutex m_mutex_setVelocityCalled
bool constantSamplingTimeMode
vpMutex m_mutex_robotStop
void setExternalCameraPosition(const vpHomogeneousMatrix &camMf_)
void set_artVel(const vpColVector &vel)
bool singularityManagement
virtual vpRobotStateType getRobotState(void) const
vpControlFrameType getRobotFrame(void) const
@ STATE_POSITION_CONTROL
Initialize the position controller.
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
@ STATE_ACCELERATION_CONTROL
Initialize the acceleration controller.
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
double getMaxRotationVelocity(void) const
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
double getMaxTranslationVelocity(void) const
void setMaxRotationVelocity(double maxVr)
vpControlFrameType setRobotFrame(vpRobot::vpControlFrameType newFrame)
void setMaxTranslationVelocity(double maxVt)
Implementation of a rotation matrix and operations on such kind of matrices.
vpRotationMatrix t() const
Implementation of a rotation vector as Euler angle minimal representation.
void get_eJe(vpMatrix &eJe)
void updateArticularPosition()
static bool savePosFile(const std::string &filename, const vpColVector &q)
void setCameraParameters(const vpCameraParameters &cam)
vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
void get_cVe(vpVelocityTwistMatrix &cVe)
void initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo)
static bool readPosFile(const std::string &filename, vpColVector &q)
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height)
bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo)
void computeArticularVelocity()
void get_fMi(vpHomogeneousMatrix *fMit)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
void findHighestPositioningSpeed(vpColVector &q)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
void move(const char *filename)
static const double defaultPositioningVelocity
bool singularityTest(const vpColVector &q, vpMatrix &J)
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &displacement)
void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
void get_fJe(vpMatrix &fJe)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q)
void getExternalImage(vpImage< vpRGBa > &I)
double getPositioningVelocity(void)
void get_cMe(vpHomogeneousMatrix &cMe)
virtual ~vpSimulatorAfma6()
Implementation of a rotation vector as axis-angle minimal representation.
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
Class that consider the case of a translation vector.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpHomogeneousMatrix camMf2
void display_scene(Matrix mat, Bound_scene &sc, const vpImage< vpRGBa > &I, const vpColor &color)
vpHomogeneousMatrix camMf
void setExternalCameraParameters(const vpCameraParameters &cam)
vpHomogeneousMatrix navigation(const vpImage< vpRGBa > &I, bool &changed)
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double getMinTimeForUsleepCall()
VISP_EXPORT double measureTimeSecond()
VISP_EXPORT double measureTimeMs()