39 #include <visp3/robot/vpSimulatorViper850.h>
41 #if defined(VISP_HAVE_MODULE_GUI) && ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD))
46 #include <visp3/core/vpImagePoint.h>
47 #include <visp3/core/vpIoTools.h>
48 #include <visp3/core/vpMeterPixelConversion.h>
49 #include <visp3/core/vpPoint.h>
50 #include <visp3/core/vpTime.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()
73 DWORD dwThreadIdArray;
74 hThread = CreateThread(NULL,
80 #elif defined(VISP_HAVE_PTHREAD)
81 pthread_attr_init(&
attr);
82 pthread_attr_setdetachstate(&
attr, PTHREAD_CREATE_JOINABLE);
98 positioningVelocity(defaultPositioningVelocity), zeroPos(), reposPos(), toolCustom(false), arm_dir()
106 DWORD dwThreadIdArray;
107 hThread = CreateThread(NULL,
113 #elif defined(VISP_HAVE_PTHREAD)
114 pthread_attr_init(&
attr);
115 pthread_attr_setdetachstate(&
attr, PTHREAD_CREATE_JOINABLE);
133 #if defined(WINRT_8_1)
134 WaitForSingleObjectEx(hThread, INFINITE, FALSE);
136 WaitForSingleObject(hThread, INFINITE);
138 CloseHandle(hThread);
139 #elif defined(VISP_HAVE_PTHREAD)
140 pthread_attr_destroy(&
attr);
141 pthread_join(
thread, NULL);
146 for (
int i = 0; i < 6; i++)
166 std::vector<std::string> arm_dirs =
vpIoTools::splitChain(std::string(VISP_ROBOT_ARMS_DIR), std::string(
";"));
167 bool armDirExists =
false;
168 for (
size_t i = 0; i < arm_dirs.size(); i++)
170 arm_dir = arm_dirs[i];
177 std::cout <<
"The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
179 std::cout <<
"Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
193 zeroPos[1] = -M_PI / 2;
197 reposPos[1] = -M_PI / 2;
199 reposPos[4] = M_PI / 2;
206 first_time_getdis =
true;
284 etc[0] = -0.04558630174;
285 etc[1] = -0.00134326752;
286 etc[2] = 0.001000828017;
294 std::cout <<
"This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
320 const unsigned int &image_height)
329 if (image_width == 640 && image_height == 480) {
334 vpTRACE(
"Cannot get default intrinsic camera parameters for this image "
341 if (image_width == 640 && image_height == 480) {
346 vpTRACE(
"Cannot get default intrinsic camera parameters for this image "
354 std::cout <<
"This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
405 double tcur_1 =
tcur;
409 bool setVelocityCalled_ =
false;
425 double ellapsedTime = (
tcur -
tprev) * 1e-3;
443 articularVelocities = 0.0;
448 articularCoordinates[0] = articularCoordinates[0] + ellapsedTime * articularVelocities[0];
449 articularCoordinates[1] = articularCoordinates[1] + ellapsedTime * articularVelocities[1];
450 articularCoordinates[2] = articularCoordinates[2] + ellapsedTime * articularVelocities[2];
451 articularCoordinates[3] = articularCoordinates[3] + ellapsedTime * articularVelocities[3];
452 articularCoordinates[4] = articularCoordinates[4] + ellapsedTime * articularVelocities[4];
453 articularCoordinates[5] = articularCoordinates[5] + ellapsedTime * articularVelocities[5];
459 ellapsedTime = (
joint_min[(
unsigned int)(-jl - 1)] - articularCoordinates[(
unsigned int)(-jl - 1)]) /
460 (articularVelocities[(
unsigned int)(-jl - 1)]);
462 ellapsedTime = (
joint_max[(
unsigned int)(jl - 1)] - articularCoordinates[(
unsigned int)(jl - 1)]) /
463 (articularVelocities[(
unsigned int)(jl - 1)]);
465 for (
unsigned int i = 0; i < 6; i++)
466 articularCoordinates[i] = articularCoordinates[i] + ellapsedTime * articularVelocities[i];
506 for (
int k = 1; k < 7; k++) {
563 double c23 = cos(q2 + q3);
564 double s23 = sin(q2 + q3);
581 fMit[0][0][3] =
a1 * c1;
582 fMit[0][1][3] =
a1 * s1;
585 fMit[1][0][0] = c1 * c2;
586 fMit[1][1][0] = s1 * c2;
588 fMit[1][0][1] = -c1 * s2;
589 fMit[1][1][1] = -s1 * s2;
594 fMit[1][0][3] = c1 * (
a2 * c2 +
a1);
595 fMit[1][1][3] = s1 * (
a2 * c2 +
a1);
596 fMit[1][2][3] =
d1 -
a2 * s2;
598 double quickcomp1 =
a3 * c23 -
a2 * c2 -
a1;
600 fMit[2][0][0] = -c1 * c23;
601 fMit[2][1][0] = -s1 * c23;
606 fMit[2][0][2] = c1 * s23;
607 fMit[2][1][2] = s1 * s23;
609 fMit[2][0][3] = -c1 * quickcomp1;
610 fMit[2][1][3] = -s1 * quickcomp1;
611 fMit[2][2][3] =
a3 * s23 -
a2 * s2 +
d1;
613 double quickcomp2 = c1 * (s23 *
d4 - quickcomp1);
614 double quickcomp3 = s1 * (s23 *
d4 - quickcomp1);
616 fMit[3][0][0] = -c1 * c23 * c4 + s1 * s4;
617 fMit[3][1][0] = -s1 * c23 * c4 - c1 * s4;
618 fMit[3][2][0] = s23 * c4;
619 fMit[3][0][1] = c1 * s23;
620 fMit[3][1][1] = s1 * s23;
622 fMit[3][0][2] = -c1 * c23 * s4 - s1 * c4;
623 fMit[3][1][2] = -s1 * c23 * s4 + c1 * c4;
624 fMit[3][2][2] = s23 * s4;
625 fMit[3][0][3] = quickcomp2;
626 fMit[3][1][3] = quickcomp3;
627 fMit[3][2][3] = c23 *
d4 +
a3 * s23 -
a2 * s2 +
d1;
629 fMit[4][0][0] = c1 * (s23 * s5 - c5 * c23 * c4) + s1 * c5 * s4;
630 fMit[4][1][0] = s1 * (s23 * s5 - c5 * c23 * c4) - c1 * c5 * s4;
631 fMit[4][2][0] = s23 * c4 * c5 + c23 * s5;
632 fMit[4][0][1] = c1 * c23 * s4 + s1 * c4;
633 fMit[4][1][1] = s1 * c23 * s4 - c1 * c4;
634 fMit[4][2][1] = -s23 * s4;
635 fMit[4][0][2] = c1 * (s23 * c5 + s5 * c23 * c4) - s1 * s5 * s4;
636 fMit[4][1][2] = s1 * (s23 * c5 + s5 * c23 * c4) + c1 * s5 * s4;
637 fMit[4][2][2] = -s23 * c4 * s5 + c23 * c5;
638 fMit[4][0][3] = quickcomp2;
639 fMit[4][1][3] = quickcomp3;
640 fMit[4][2][3] = c23 *
d4 +
a3 * s23 -
a2 * s2 +
d1;
642 fMit[5][0][0] = c1 * (c23 * (c4 * c5 * c6 - s4 * s6) - s23 * s5 * c6) - s1 * (s4 * c5 * c6 + c4 * s6);
643 fMit[5][1][0] = -s1 * (c23 * (-c4 * c5 * c6 + s4 * s6) + s23 * s5 * c6) + c1 * (s4 * c5 * c6 + c4 * s6);
644 fMit[5][2][0] = s23 * (s4 * s6 - c4 * c5 * c6) - c23 * s5 * c6;
645 fMit[5][0][1] = -c1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) + s1 * (s4 * c5 * s6 - c4 * c6);
646 fMit[5][1][1] = -s1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) - c1 * (s4 * c5 * s6 - c4 * c6);
647 fMit[5][2][1] = s23 * (c4 * c5 * s6 + s4 * c6) + c23 * s5 * s6;
648 fMit[5][0][2] = c1 * (c23 * c4 * s5 + s23 * c5) - s1 * s4 * s5;
649 fMit[5][1][2] = s1 * (c23 * c4 * s5 + s23 * c5) + c1 * s4 * s5;
650 fMit[5][2][2] = -s23 * c4 * s5 + c23 * c5;
651 fMit[5][0][3] = quickcomp2;
652 fMit[5][1][3] = quickcomp3;
653 fMit[5][2][3] = s23 *
a3 + c23 *
d4 -
a2 * s2 +
d1;
655 fMit[6][0][0] = c1 * (c23 * (c4 * c5 * c6 - s4 * s6) - s23 * s5 * c6) - s1 * (s4 * c5 * c6 + c4 * s6);
656 fMit[6][1][0] = -s1 * (c23 * (-c4 * c5 * c6 + s4 * s6) + s23 * s5 * c6) + c1 * (s4 * c5 * c6 + c4 * s6);
657 fMit[6][2][0] = s23 * (s4 * s6 - c4 * c5 * c6) - c23 * s5 * c6;
658 fMit[6][0][1] = -c1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) + s1 * (s4 * c5 * s6 - c4 * c6);
659 fMit[6][1][1] = -s1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) - c1 * (s4 * c5 * s6 - c4 * c6);
660 fMit[6][2][1] = s23 * (c4 * c5 * s6 + s4 * c6) + c23 * s5 * s6;
661 fMit[6][0][2] = c1 * (c23 * c4 * s5 + s23 * c5) - s1 * s4 * s5;
662 fMit[6][1][2] = s1 * (c23 * c4 * s5 + s23 * c5) + c1 * s4 * s5;
663 fMit[6][2][2] = -s23 * c4 * s5 + c23 * c5;
664 fMit[6][0][3] = c1 * (c23 * (c4 * s5 *
d6 -
a3) + s23 * (c5 *
d6 +
d4) +
a1 +
a2 * c2) - s1 * s4 * s5 *
d6;
665 fMit[6][1][3] = s1 * (c23 * (c4 * s5 *
d6 -
a3) + s23 * (c5 *
d6 +
d4) +
a1 +
a2 * c2) + c1 * s4 * s5 *
d6;
666 fMit[6][2][3] = s23 * (
a3 - c4 * s5 *
d6) + c23 * (c5 *
d6 +
d4) -
a2 * s2 +
d1;
677 for (
int i = 0; i < 8; i++) {
700 std::cout <<
"Change the control mode from velocity to position control.\n";
710 std::cout <<
"Change the control mode from stop to velocity control.\n";
799 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
801 "Cannot send a velocity to the robot "
802 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
807 double scale_sat = 1;
819 vpERROR_TRACE(
"The velocity vector must have a size of 6 !!!!");
823 for (
unsigned int i = 0; i < 3; ++i) {
824 vel_abs = fabs(vel[i]);
826 vel_trans_max = vel_abs;
832 vel_abs = fabs(vel[i + 3]);
834 vel_rot_max = vel_abs;
841 double scale_trans_sat = 1;
842 double scale_rot_sat = 1;
849 if ((scale_trans_sat < 1) || (scale_rot_sat < 1)) {
850 if (scale_trans_sat < scale_rot_sat)
851 scale_sat = scale_trans_sat;
853 scale_sat = scale_rot_sat;
861 vpERROR_TRACE(
"The velocity vector must have a size of 6 !!!!");
864 for (
unsigned int i = 0; i < 6; ++i) {
865 vel_abs = fabs(vel[i]);
867 vel_rot_max = vel_abs;
873 double scale_rot_sat = 1;
876 if (scale_rot_sat < 1)
877 scale_sat = scale_rot_sat;
922 articularVelocity = eJe_ * eVc * velocityframe;
932 articularVelocity = fJe_ * velocityframe;
937 articularVelocity = velocityframe;
950 for (
unsigned int i = 0; i < 6; ++i) {
951 double vel_abs = fabs(articularVelocity[i]);
953 vel_rot_max = vel_abs;
956 articularVelocity[i], i + 1);
959 double scale_rot_sat = 1;
960 double scale_sat = 1;
964 if (scale_rot_sat < 1)
965 scale_sat = scale_rot_sat;
1035 vel = cVe * eJe_ * articularVelocity;
1039 vel = articularVelocity;
1045 vel = fJe_ * articularVelocity;
1054 "Case not taken in account.");
1155 double velmax = fabs(q[0]);
1156 for (
unsigned int i = 1; i < 6; i++) {
1157 if (velmax < fabs(q[i]))
1158 velmax = fabs(q[i]);
1243 "Modification of the robot state");
1258 for (
unsigned int i = 0; i < 3; i++) {
1273 qdes = articularCoordinates;
1279 error = qdes - articularCoordinates;
1283 if (errsqr < 1e-4) {
1294 }
while (errsqr > 1e-8 && nbSol > 0);
1302 error = q - articularCoordinates;
1309 if (errsqr < 1e-4) {
1316 }
while (errsqr > 1e-8);
1326 for (
unsigned int i = 0; i < 3; i++) {
1336 qdes = articularCoordinates;
1339 error = qdes - articularCoordinates;
1346 if (errsqr < 1e-4) {
1355 }
while (errsqr > 1e-8 && nbSol > 0);
1360 "Mixt frame not implemented.");
1364 "End-effector frame not implemented.");
1432 double pos4,
double pos5,
double pos6)
1584 for (
unsigned int i = 0; i < 3; i++) {
1592 vpERROR_TRACE(
"Positioning error. Mixt frame not implemented");
1594 "Mixt frame not implemented.");
1597 vpERROR_TRACE(
"Positioning error. Mixt frame not implemented");
1599 "End-effector frame not implemented.");
1657 for (
unsigned int j = 0; j < 3; j++) {
1658 position[j] = posRxyz[j];
1659 position[j + 3] = RtuVect[j];
1692 vpTRACE(
"Joint limit vector has not a size of 6 !");
1722 double c2 = cos(q2);
1723 double c3 = cos(q3);
1724 double s3 = sin(q3);
1725 double c23 = cos(q2 + q3);
1726 double s23 = sin(q2 + q3);
1727 double s5 = sin(q5);
1729 bool cond1 = fabs(s5) < 1e-1;
1730 bool cond2 = fabs(
a3 * s3 + c3 *
d4) < 1e-1;
1731 bool cond3 = fabs(
a2 * c2 -
a3 * c23 + s23 *
d4 +
a1) < 1e-1;
1793 for (
unsigned int i = 0; i < 6; i++) {
1794 if (articularCoordinates[i] <=
joint_min[i]) {
1795 difft =
joint_min[i] - articularCoordinates[i];
1798 artNumb = -(int)i - 1;
1803 for (
unsigned int i = 0; i < 6; i++) {
1804 if (articularCoordinates[i] >=
joint_max[i]) {
1805 difft = articularCoordinates[i] -
joint_max[i];
1808 artNumb = (int)(i + 1);
1814 std::cout <<
"\nWarning: Velocity control stopped: axis " << fabs((
float)artNumb) <<
" on joint limit!"
1845 if (!first_time_getdis) {
1848 std::cout <<
"getDisplacement() CAMERA_FRAME not implemented\n";
1853 displacement = q_cur - q_prev_getdis;
1858 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
1863 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
1867 std::cout <<
"getDisplacement() END_EFFECTOR_FRAME not implemented\n";
1872 first_time_getdis =
false;
1876 q_prev_getdis = q_cur;
1942 std::ifstream fd(filename.c_str(), std::ios::in);
1944 if (!fd.is_open()) {
1949 std::string key(
"R:");
1950 std::string id(
"#Viper850 - Position");
1951 bool pos_found =
false;
1956 while (std::getline(fd, line)) {
1959 if (!(line.compare(0,
id.size(),
id) == 0)) {
1960 std::cout <<
"Error: this position file " << filename <<
" is not for Viper850 robot" << std::endl;
1964 if ((line.compare(0, 1,
"#") == 0)) {
1967 if ((line.compare(0, key.size(), key) == 0)) {
1970 if (chain.size() <
njoint + 1)
1972 if (chain.size() <
njoint + 1)
1975 std::istringstream ss(line);
1978 for (
unsigned int i = 0; i <
njoint; i++)
1991 std::cout <<
"Error: unable to find a position for Viper850 robot in " << filename << std::endl;
2023 fd = fopen(filename.c_str(),
"w");
2028 #Viper - Position - Version 1.0\n\
2031 # Joint position in degrees\n\
2161 std::string scene_dir_;
2162 std::vector<std::string> scene_dirs =
vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(
";"));
2163 bool sceneDirExists =
false;
2164 for (
size_t i = 0; i < scene_dirs.size(); i++)
2166 scene_dir_ = scene_dirs[i];
2167 sceneDirExists =
true;
2170 if (!sceneDirExists) {
2173 std::cout <<
"The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
2175 std::cout <<
"Cannot get VISP_SCENES_DIR environment variable" << std::endl;
2179 unsigned int name_length = 30;
2180 if (scene_dir_.size() > FILENAME_MAX)
2183 unsigned int full_length = (
unsigned int)scene_dir_.size() + name_length;
2184 if (full_length > FILENAME_MAX)
2187 char *name_cam =
new char[full_length];
2189 strcpy(name_cam, scene_dir_.c_str());
2190 strcat(name_cam,
"/camera.bnd");
2193 if (arm_dir.size() > FILENAME_MAX)
2195 full_length = (
unsigned int)arm_dir.size() + name_length;
2196 if (full_length > FILENAME_MAX)
2199 char *name_arm =
new char[full_length];
2200 strcpy(name_arm, arm_dir.c_str());
2201 strcat(name_arm,
"/viper850_arm1.bnd");
2203 strcpy(name_arm, arm_dir.c_str());
2204 strcat(name_arm,
"/viper850_arm2.bnd");
2205 set_scene(name_arm,
robotArms + 1, 1.0);
2206 strcpy(name_arm, arm_dir.c_str());
2207 strcat(name_arm,
"/viper850_arm3.bnd");
2208 set_scene(name_arm,
robotArms + 2, 1.0);
2209 strcpy(name_arm, arm_dir.c_str());
2210 strcat(name_arm,
"/viper850_arm4.bnd");
2211 set_scene(name_arm,
robotArms + 3, 1.0);
2212 strcpy(name_arm, arm_dir.c_str());
2213 strcat(name_arm,
"/viper850_arm5.bnd");
2214 set_scene(name_arm,
robotArms + 4, 1.0);
2215 strcpy(name_arm, arm_dir.c_str());
2216 strcat(name_arm,
"/viper850_arm6.bnd");
2217 set_scene(name_arm,
robotArms + 5, 1.0);
2225 add_rfstack(IS_BACK);
2227 add_vwstack(
"start",
"depth", 0.0, 100.0);
2228 add_vwstack(
"start",
"window", -0.1, 0.1, -0.1, 0.1);
2229 add_vwstack(
"start",
"type", PERSPECTIVE);
2242 bool changed =
false;
2246 if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
2266 float w44o[4][4], w44cext[4][4], x, y, z;
2270 add_vwstack(
"start",
"cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
2271 x = w44cext[2][0] + w44cext[3][0];
2272 y = w44cext[2][1] + w44cext[3][1];
2273 z = w44cext[2][2] + w44cext[3][2];
2274 add_vwstack(
"start",
"vrp", x, y, z);
2275 add_vwstack(
"start",
"vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
2276 add_vwstack(
"start",
"vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
2277 add_vwstack(
"start",
"window", -u, u, -v, v);
2285 vp2jlc_matrix(fMit[0], w44o);
2288 vp2jlc_matrix(fMit[1], w44o);
2291 vp2jlc_matrix(fMit[2], w44o);
2294 vp2jlc_matrix(fMit[3], w44o);
2297 vp2jlc_matrix(fMit[6], w44o);
2304 cMe = fMit[6] * cMe;
2305 vp2jlc_matrix(cMe, w44o);
2310 vp2jlc_matrix(
fMo, w44o);
2351 std::cout <<
"Used joint coordinates (rad): " << articularCoordinates.
t() << std::endl;
2382 fMo = fMit[7] * cMo_;
2386 #elif !defined(VISP_BUILD_SHARED_LIBS)
2389 void dummy_vpSimulatorViper850(){};
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.
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.
vpColVector get_velocity()
vpColVector get_artCoord()
void set_velocity(const vpColVector &vel)
void set_displayBusy(const bool &status)
vpHomogeneousMatrix * fMi
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
vpControlFrameType setRobotFrame(vpRobot::vpControlFrameType newFrame)
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as Euler angle minimal representation.
virtual ~vpSimulatorViper850()
static bool savePosFile(const std::string &filename, const vpColVector &q)
void get_fMi(vpHomogeneousMatrix *fMit)
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &displacement)
void get_fJe(vpMatrix &fJe)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
void setCameraParameters(const vpCameraParameters &cam)
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height)
void computeArticularVelocity()
void getExternalImage(vpImage< vpRGBa > &I)
void updateArticularPosition()
void get_cMe(vpHomogeneousMatrix &cMe)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
void get_cVe(vpVelocityTwistMatrix &cVe)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q)
void findHighestPositioningSpeed(vpColVector &q)
bool singularityTest(const vpColVector &q, vpMatrix &J)
void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
static const double defaultPositioningVelocity
vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
void initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo)
static bool readPosFile(const std::string &filename, vpColVector &q)
void move(const char *filename)
void get_eJe(vpMatrix &eJe)
double getPositioningVelocity(void)
bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo)
Implementation of a rotation vector as axis-angle minimal representation.
Class that consider the case of a translation vector.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Modelisation of the ADEPT Viper 850 robot.
static const char *const CONST_PTGREY_FLEA2_CAMERA_NAME
void setToolType(vpViper850::vpToolType tool)
Set the current tool type.
static const vpToolType defaultTool
Default tool attached to the robot end effector.
vpCameraParameters::vpCameraParametersProjType projModel
vpToolType getToolType() const
Get the current tool type.
vpToolType
List of possible tools that can be attached to the robot end-effector.
@ TOOL_SCHUNK_GRIPPER_CAMERA
@ TOOL_PTGREY_FLEA2_CAMERA
@ TOOL_MARLIN_F033C_CAMERA
static const char *const CONST_MARLIN_F033C_CAMERA_NAME
void get_cMe(vpHomogeneousMatrix &cMe) const
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false) const
vpHomogeneousMatrix eMc
End effector to camera transformation.
static const unsigned int njoint
Number of joint.
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
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()