39 #include <visp3/core/vpConfig.h>
40 #if defined(VISP_HAVE_MODULE_GUI) && ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD))
44 #include <visp3/core/vpImagePoint.h>
45 #include <visp3/core/vpIoTools.h>
46 #include <visp3/core/vpMeterPixelConversion.h>
47 #include <visp3/core/vpPoint.h>
48 #include <visp3/core/vpTime.h>
49 #include <visp3/robot/vpRobotException.h>
50 #include <visp3/robot/vpSimulatorAfma6.h>
52 #include "../wireframe-simulator/vpBound.h"
53 #include "../wireframe-simulator/vpRfstack.h"
54 #include "../wireframe-simulator/vpScene.h"
55 #include "../wireframe-simulator/vpVwstack.h"
64 positioningVelocity(defaultPositioningVelocity), zeroPos(), reposPos(), toolCustom(false), arm_dir()
72 DWORD dwThreadIdArray;
73 hThread = CreateThread(NULL,
79 #elif defined(VISP_HAVE_PTHREAD)
80 pthread_attr_init(&
attr);
81 pthread_attr_setdetachstate(&
attr, PTHREAD_CREATE_JOINABLE);
97 positioningVelocity(defaultPositioningVelocity), zeroPos(), reposPos(), toolCustom(false), arm_dir()
105 DWORD dwThreadIdArray;
106 hThread = CreateThread(NULL,
112 #elif defined(VISP_HAVE_PTHREAD)
113 pthread_attr_init(&
attr);
114 pthread_attr_setdetachstate(&
attr, PTHREAD_CREATE_JOINABLE);
132 #if defined(WINRT_8_1)
133 WaitForSingleObjectEx(hThread, INFINITE, FALSE);
135 WaitForSingleObject(hThread, INFINITE);
137 CloseHandle(hThread);
138 #elif defined(VISP_HAVE_PTHREAD)
139 pthread_attr_destroy(&
attr);
140 pthread_join(
thread, NULL);
144 for (
int i = 0; i < 6; i++)
164 std::vector<std::string> arm_dirs =
vpIoTools::splitChain(std::string(VISP_ROBOT_ARMS_DIR), std::string(
";"));
165 bool armDirExists =
false;
166 for (
size_t i = 0; i < arm_dirs.size(); i++)
168 arm_dir = arm_dirs[i];
175 std::cout <<
"The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
177 std::cout <<
"Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
193 reposPos[1] = -M_PI / 2;
195 reposPos[4] = M_PI / 2;
202 first_time_getdis =
true;
263 unsigned int name_length = 30;
264 if (arm_dir.size() > FILENAME_MAX)
266 unsigned int full_length = (
unsigned int)arm_dir.size() + name_length;
267 if (full_length > FILENAME_MAX)
286 char *name_arm =
new char[full_length];
287 strcpy(name_arm, arm_dir.c_str());
288 strcat(name_arm,
"/afma6_tool_ccmop.bnd");
309 char *name_arm =
new char[full_length];
310 strcpy(name_arm, arm_dir.c_str());
311 strcat(name_arm,
"/afma6_tool_gripper.bnd");
333 char *name_arm =
new char[full_length];
335 strcpy(name_arm, arm_dir.c_str());
336 strcat(name_arm,
"/afma6_tool_vacuum.bnd");
344 std::cout <<
"The custom tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
348 std::cout <<
"The Intel D435 camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
352 std::cout <<
"The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
378 const unsigned int &image_height)
387 if (image_width == 640 && image_height == 480) {
392 vpTRACE(
"Cannot get default intrinsic camera parameters for this image "
399 if (image_width == 640 && image_height == 480) {
404 vpTRACE(
"Cannot get default intrinsic camera parameters for this image "
410 std::cout <<
"The generic tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
414 std::cout <<
"The Intel D435 camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
418 std::cout <<
"The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
422 std::cout <<
"The vacuum tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
476 double tcur_1 =
tcur;
479 bool setVelocityCalled_ =
false;
496 double ellapsedTime = (
tcur -
tprev) * 1e-3;
515 articularVelocities = 0.0;
520 articularCoordinates[0] = articularCoordinates[0] + ellapsedTime * articularVelocities[0];
521 articularCoordinates[1] = articularCoordinates[1] + ellapsedTime * articularVelocities[1];
522 articularCoordinates[2] = articularCoordinates[2] + ellapsedTime * articularVelocities[2];
523 articularCoordinates[3] = articularCoordinates[3] + ellapsedTime * articularVelocities[3];
524 articularCoordinates[4] = articularCoordinates[4] + ellapsedTime * articularVelocities[4];
525 articularCoordinates[5] = articularCoordinates[5] + ellapsedTime * articularVelocities[5];
531 ellapsedTime = (
_joint_min[(
unsigned int)(-jl - 1)] - articularCoordinates[(
unsigned int)(-jl - 1)]) /
532 (articularVelocities[(
unsigned int)(-jl - 1)]);
534 ellapsedTime = (
_joint_max[(
unsigned int)(jl - 1)] - articularCoordinates[(
unsigned int)(jl - 1)]) /
535 (articularVelocities[(
unsigned int)(jl - 1)]);
537 for (
unsigned int i = 0; i < 6; i++)
538 articularCoordinates[i] = articularCoordinates[i] + ellapsedTime * articularVelocities[i];
578 for (
unsigned int k = 1; k < 7; k++) {
692 fMit[4][0][0] = s4 * s5;
693 fMit[4][1][0] = -c4 * s5;
695 fMit[4][0][1] = s4 * c5;
696 fMit[4][1][1] = -c4 * c5;
701 fMit[4][0][3] = c4 * this->
_long_56 + q1;
702 fMit[4][1][3] = s4 * this->
_long_56 + q2;
705 fMit[5][0][0] = s4 * s5 * c6 + c4 * s6;
706 fMit[5][1][0] = -c4 * s5 * c6 + s4 * s6;
707 fMit[5][2][0] = c5 * c6;
708 fMit[5][0][1] = -s4 * s5 * s6 + c4 * c6;
709 fMit[5][1][1] = c4 * s5 * s6 + s4 * c6;
710 fMit[5][2][1] = -c5 * s6;
711 fMit[5][0][2] = -s4 * c5;
712 fMit[5][1][2] = c4 * c5;
714 fMit[5][0][3] = c4 * this->
_long_56 + q1;
715 fMit[5][1][3] = s4 * this->
_long_56 + q2;
718 fMit[6][0][0] = fMit[5][0][0];
719 fMit[6][1][0] = fMit[5][1][0];
720 fMit[6][2][0] = fMit[5][2][0];
721 fMit[6][0][1] = fMit[5][0][1];
722 fMit[6][1][1] = fMit[5][1][1];
723 fMit[6][2][1] = fMit[5][2][1];
724 fMit[6][0][2] = fMit[5][0][2];
725 fMit[6][1][2] = fMit[5][1][2];
726 fMit[6][2][2] = fMit[5][2][2];
727 fMit[6][0][3] = fMit[5][0][3];
728 fMit[6][1][3] = fMit[5][1][3];
729 fMit[6][2][3] = fMit[5][2][3];
740 for (
int i = 0; i < 8; i++) {
763 std::cout <<
"Change the control mode from velocity to position control.\n";
773 std::cout <<
"Change the control mode from stop to velocity control.\n";
862 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
864 "Cannot send a velocity to the robot "
865 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
870 double scale_sat = 1;
882 vpERROR_TRACE(
"The velocity vector must have a size of 6 !!!!");
886 for (
unsigned int i = 0; i < 3; ++i) {
887 vel_abs = fabs(vel[i]);
889 vel_trans_max = vel_abs;
895 vel_abs = fabs(vel[i + 3]);
897 vel_rot_max = vel_abs;
904 double scale_trans_sat = 1;
905 double scale_rot_sat = 1;
912 if ((scale_trans_sat < 1) || (scale_rot_sat < 1)) {
913 if (scale_trans_sat < scale_rot_sat)
914 scale_sat = scale_trans_sat;
916 scale_sat = scale_rot_sat;
924 vpERROR_TRACE(
"The velocity vector must have a size of 6 !!!!");
927 for (
unsigned int i = 0; i < 6; ++i) {
928 vel_abs = fabs(vel[i]);
930 vel_rot_max = vel_abs;
936 double scale_rot_sat = 1;
939 if (scale_rot_sat < 1)
940 scale_sat = scale_rot_sat;
945 "functionality not implemented");
949 "functionality not implemented");
989 articularVelocity = eJe_ * eVc * velocityframe;
999 articularVelocity = fJe_ * velocityframe;
1004 articularVelocity = velocityframe;
1017 for (
unsigned int i = 0; i < 6; ++i) {
1018 double vel_abs = fabs(articularVelocity[i]);
1020 vel_rot_max = vel_abs;
1023 articularVelocity[i], i + 1);
1026 double scale_rot_sat = 1;
1027 double scale_sat = 1;
1030 if (scale_rot_sat < 1)
1031 scale_sat = scale_rot_sat;
1101 vel = cVe * eJe_ * articularVelocity;
1105 vel = articularVelocity;
1111 vel = fJe_ * articularVelocity;
1120 "Case not taken in account.");
1221 double velmax = fabs(q[0]);
1222 for (
unsigned int i = 1; i < 6; i++) {
1223 if (velmax < fabs(q[i]))
1224 velmax = fabs(q[i]);
1309 "Modification of the robot state");
1324 for (
unsigned int i = 0; i < 3; i++) {
1339 qdes = articularCoordinates;
1347 error = qdes - articularCoordinates;
1351 if (errsqr < 1e-4) {
1362 }
while (errsqr > 1e-8 && nbSol > 0);
1370 error = q - articularCoordinates;
1377 if (errsqr < 1e-4) {
1384 }
while (errsqr > 1e-8);
1394 for (
unsigned int i = 0; i < 3; i++) {
1404 qdes = articularCoordinates;
1410 error = qdes - articularCoordinates;
1414 if (errsqr < 1e-4) {
1423 }
while (errsqr > 1e-8 && nbSol > 0);
1427 vpERROR_TRACE(
"Positioning error. Mixt frame not implemented");
1429 "MIXT_FRAME not implemented.");
1432 vpERROR_TRACE(
"Positioning error. Mixt frame not implemented");
1434 "END_EFFECTOR_FRAME not implemented.");
1502 double pos4,
double pos5,
double pos6)
1654 for (
unsigned int i = 0; i < 3; i++) {
1663 "Mixt frame not implemented.");
1667 "End-effector frame not implemented.");
1725 for (
unsigned int j = 0; j < 3; j++) {
1726 position[j] = posRxyz[j];
1727 position[j + 3] = RtuVect[j];
1761 vpTRACE(
"Joint limit vector has not a size of 6 !");
1789 bool cond = fabs(q5 - M_PI / 2) < 1e-1;
1817 for (
unsigned int i = 0; i < 6; i++) {
1818 if (articularCoordinates[i] <=
_joint_min[i]) {
1819 difft =
_joint_min[i] - articularCoordinates[i];
1822 artNumb = -(int)i - 1;
1827 for (
unsigned int i = 0; i < 6; i++) {
1828 if (articularCoordinates[i] >=
_joint_max[i]) {
1829 difft = articularCoordinates[i] -
_joint_max[i];
1832 artNumb = (int)(i + 1);
1838 std::cout <<
"\nWarning: Velocity control stopped: axis " << fabs((
float)artNumb) <<
" on joint limit!"
1869 if (!first_time_getdis) {
1872 std::cout <<
"getDisplacement() CAMERA_FRAME not implemented\n";
1876 displacement = q_cur - q_prev_getdis;
1880 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
1884 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
1888 std::cout <<
"getDisplacement() END_EFFECTOR_FRAME not implemented\n";
1893 first_time_getdis =
false;
1897 q_prev_getdis = q_cur;
1949 std::ifstream fd(filename.c_str(), std::ios::in);
1951 if (!fd.is_open()) {
1956 std::string key(
"R:");
1957 std::string id(
"#AFMA6 - Position");
1958 bool pos_found =
false;
1963 while (std::getline(fd, line)) {
1966 if (!(line.compare(0,
id.size(),
id) == 0)) {
1967 std::cout <<
"Error: this position file " << filename <<
" is not for Afma6 robot" << std::endl;
1971 if ((line.compare(0, 1,
"#") == 0)) {
1974 if ((line.compare(0, key.size(), key) == 0)) {
1977 if (chain.size() <
njoint + 1)
1979 if (chain.size() <
njoint + 1)
1982 std::istringstream ss(line);
1985 for (
unsigned int i = 0; i <
njoint; i++)
2000 std::cout <<
"Error: unable to find a position for Afma6 robot in " << filename << std::endl;
2032 fd = fopen(filename.c_str(),
"w");
2037 #AFMA6 - Position - Version 2.01\n\
2040 # Joint position: X, Y, Z: translations in meters\n\
2041 # A, B, C: rotations in degrees\n\
2171 std::string scene_dir_;
2172 std::vector<std::string> scene_dirs =
vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(
";"));
2173 bool sceneDirExists =
false;
2174 for (
size_t i = 0; i < scene_dirs.size(); i++)
2176 scene_dir_ = scene_dirs[i];
2177 sceneDirExists =
true;
2180 if (!sceneDirExists) {
2183 std::cout <<
"The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
2185 std::cout <<
"Cannot get VISP_SCENES_DIR environment variable" << std::endl;
2189 unsigned int name_length = 30;
2190 if (scene_dir_.size() > FILENAME_MAX)
2192 unsigned int full_length = (
unsigned int)scene_dir_.size() + name_length;
2193 if (full_length > FILENAME_MAX)
2196 char *name_cam =
new char[full_length];
2198 strcpy(name_cam, scene_dir_.c_str());
2199 strcat(name_cam,
"/camera.bnd");
2202 if (arm_dir.size() > FILENAME_MAX)
2204 full_length = (
unsigned int)arm_dir.size() + name_length;
2205 if (full_length > FILENAME_MAX)
2208 char *name_arm =
new char[full_length];
2209 strcpy(name_arm, arm_dir.c_str());
2210 strcat(name_arm,
"/afma6_gate.bnd");
2211 std::cout <<
"name arm: " << name_arm << std::endl;
2213 strcpy(name_arm, arm_dir.c_str());
2214 strcat(name_arm,
"/afma6_arm1.bnd");
2215 set_scene(name_arm,
robotArms + 1, 1.0);
2216 strcpy(name_arm, arm_dir.c_str());
2217 strcat(name_arm,
"/afma6_arm2.bnd");
2218 set_scene(name_arm,
robotArms + 2, 1.0);
2219 strcpy(name_arm, arm_dir.c_str());
2220 strcat(name_arm,
"/afma6_arm3.bnd");
2221 set_scene(name_arm,
robotArms + 3, 1.0);
2222 strcpy(name_arm, arm_dir.c_str());
2223 strcat(name_arm,
"/afma6_arm4.bnd");
2224 set_scene(name_arm,
robotArms + 4, 1.0);
2228 strcpy(name_arm, arm_dir.c_str());
2231 strcat(name_arm,
"/afma6_tool_ccmop.bnd");
2235 strcat(name_arm,
"/afma6_tool_gripper.bnd");
2239 strcat(name_arm,
"/afma6_tool_vacuum.bnd");
2243 std::cout <<
"The custom tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
2247 std::cout <<
"The Intel D435 camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
2251 std::cout <<
"The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
2255 set_scene(name_arm,
robotArms + 5, 1.0);
2257 add_rfstack(IS_BACK);
2259 add_vwstack(
"start",
"depth", 0.0, 100.0);
2260 add_vwstack(
"start",
"window", -0.1, 0.1, -0.1, 0.1);
2261 add_vwstack(
"start",
"type", PERSPECTIVE);
2274 bool changed =
false;
2278 if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
2298 float w44o[4][4], w44cext[4][4], x, y, z;
2302 add_vwstack(
"start",
"cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
2303 x = w44cext[2][0] + w44cext[3][0];
2304 y = w44cext[2][1] + w44cext[3][1];
2305 z = w44cext[2][2] + w44cext[3][2];
2306 add_vwstack(
"start",
"vrp", x, y, z);
2307 add_vwstack(
"start",
"vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
2308 add_vwstack(
"start",
"vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
2309 add_vwstack(
"start",
"window", -u, u, -v, v);
2317 vp2jlc_matrix(fMit[0], w44o);
2320 vp2jlc_matrix(fMit[2], w44o);
2323 vp2jlc_matrix(fMit[3], w44o);
2326 vp2jlc_matrix(fMit[4], w44o);
2329 vp2jlc_matrix(fMit[5], w44o);
2336 cMe = fMit[6] * cMe;
2337 vp2jlc_matrix(cMe, w44o);
2342 vp2jlc_matrix(
fMo, w44o);
2385 std::cout <<
"Used joint coordinates (rad): " << articularCoordinates.
t() << std::endl;
2417 fMo = fMit[7] * cMo_;
2447 const double lambda = 5.;
2451 unsigned int i, iter = 0;
2469 v = -lambda * cdRc.
t() * cdTc;
2470 w = -lambda * cdTUc;
2471 for (i = 0; i < 3; ++i) {
2475 err[i + 3] = cdTUc[i];
2494 #elif !defined(VISP_BUILD_SHARED_LIBS)
2497 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 emited 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 emited by the vpRobot class and its derivates.
@ positionOutOfRangeError
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()