36 #include <visp3/robot/vpSimulatorViper850.h>
38 #if defined(VISP_HAVE_MODULE_GUI) && ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD))
43 #include <visp3/core/vpImagePoint.h>
44 #include <visp3/core/vpIoTools.h>
45 #include <visp3/core/vpMeterPixelConversion.h>
46 #include <visp3/core/vpPoint.h>
47 #include <visp3/core/vpTime.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()
70 DWORD dwThreadIdArray;
71 hThread = CreateThread(
nullptr,
77 #elif defined(VISP_HAVE_PTHREAD)
78 pthread_attr_init(&
attr);
79 pthread_attr_setdetachstate(&
attr, PTHREAD_CREATE_JOINABLE);
95 positioningVelocity(defaultPositioningVelocity), zeroPos(), reposPos(), toolCustom(false), arm_dir()
103 DWORD dwThreadIdArray;
104 hThread = CreateThread(
nullptr,
110 #elif defined(VISP_HAVE_PTHREAD)
111 pthread_attr_init(&
attr);
112 pthread_attr_setdetachstate(&
attr, PTHREAD_CREATE_JOINABLE);
130 #if defined(WINRT_8_1)
131 WaitForSingleObjectEx(hThread, INFINITE, FALSE);
133 WaitForSingleObject(hThread, INFINITE);
135 CloseHandle(hThread);
136 #elif defined(VISP_HAVE_PTHREAD)
137 pthread_attr_destroy(&
attr);
138 pthread_join(
thread,
nullptr);
143 for (
int i = 0; i < 6; i++)
163 std::vector<std::string> arm_dirs =
vpIoTools::splitChain(std::string(VISP_ROBOT_ARMS_DIR), std::string(
";"));
164 bool armDirExists =
false;
165 for (
size_t i = 0; i < arm_dirs.size(); i++)
167 arm_dir = arm_dirs[i];
174 std::cout <<
"The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
176 std::cout <<
"Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
190 zeroPos[1] = -M_PI / 2;
194 reposPos[1] = -M_PI / 2;
196 reposPos[4] = M_PI / 2;
203 first_time_getdis =
true;
281 etc[0] = -0.04558630174;
282 etc[1] = -0.00134326752;
283 etc[2] = 0.001000828017;
291 std::cout <<
"This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
317 const unsigned int &image_height)
326 if (image_width == 640 && image_height == 480) {
331 vpTRACE(
"Cannot get default intrinsic camera parameters for this image "
338 if (image_width == 640 && image_height == 480) {
343 vpTRACE(
"Cannot get default intrinsic camera parameters for this image "
351 std::cout <<
"This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
402 double tcur_1 =
tcur;
406 bool setVelocityCalled_ =
false;
422 double ellapsedTime = (
tcur -
tprev) * 1e-3;
440 articularVelocities = 0.0;
445 articularCoordinates[0] = articularCoordinates[0] + ellapsedTime * articularVelocities[0];
446 articularCoordinates[1] = articularCoordinates[1] + ellapsedTime * articularVelocities[1];
447 articularCoordinates[2] = articularCoordinates[2] + ellapsedTime * articularVelocities[2];
448 articularCoordinates[3] = articularCoordinates[3] + ellapsedTime * articularVelocities[3];
449 articularCoordinates[4] = articularCoordinates[4] + ellapsedTime * articularVelocities[4];
450 articularCoordinates[5] = articularCoordinates[5] + ellapsedTime * articularVelocities[5];
456 ellapsedTime = (
joint_min[(
unsigned int)(-jl - 1)] - articularCoordinates[(
unsigned int)(-jl - 1)]) /
457 (articularVelocities[(
unsigned int)(-jl - 1)]);
459 ellapsedTime = (
joint_max[(
unsigned int)(jl - 1)] - articularCoordinates[(
unsigned int)(jl - 1)]) /
460 (articularVelocities[(
unsigned int)(jl - 1)]);
462 for (
unsigned int i = 0; i < 6; i++)
463 articularCoordinates[i] = articularCoordinates[i] + ellapsedTime * articularVelocities[i];
503 for (
int k = 1; k < 7; k++) {
560 double c23 = cos(q2 + q3);
561 double s23 = sin(q2 + q3);
578 fMit[0][0][3] =
a1 * c1;
579 fMit[0][1][3] =
a1 * s1;
582 fMit[1][0][0] = c1 * c2;
583 fMit[1][1][0] = s1 * c2;
585 fMit[1][0][1] = -c1 * s2;
586 fMit[1][1][1] = -s1 * s2;
591 fMit[1][0][3] = c1 * (
a2 * c2 +
a1);
592 fMit[1][1][3] = s1 * (
a2 * c2 +
a1);
593 fMit[1][2][3] =
d1 -
a2 * s2;
595 double quickcomp1 =
a3 * c23 -
a2 * c2 -
a1;
597 fMit[2][0][0] = -c1 * c23;
598 fMit[2][1][0] = -s1 * c23;
603 fMit[2][0][2] = c1 * s23;
604 fMit[2][1][2] = s1 * s23;
606 fMit[2][0][3] = -c1 * quickcomp1;
607 fMit[2][1][3] = -s1 * quickcomp1;
608 fMit[2][2][3] =
a3 * s23 -
a2 * s2 +
d1;
610 double quickcomp2 = c1 * (s23 *
d4 - quickcomp1);
611 double quickcomp3 = s1 * (s23 *
d4 - quickcomp1);
613 fMit[3][0][0] = -c1 * c23 * c4 + s1 * s4;
614 fMit[3][1][0] = -s1 * c23 * c4 - c1 * s4;
615 fMit[3][2][0] = s23 * c4;
616 fMit[3][0][1] = c1 * s23;
617 fMit[3][1][1] = s1 * s23;
619 fMit[3][0][2] = -c1 * c23 * s4 - s1 * c4;
620 fMit[3][1][2] = -s1 * c23 * s4 + c1 * c4;
621 fMit[3][2][2] = s23 * s4;
622 fMit[3][0][3] = quickcomp2;
623 fMit[3][1][3] = quickcomp3;
624 fMit[3][2][3] = c23 *
d4 +
a3 * s23 -
a2 * s2 +
d1;
626 fMit[4][0][0] = c1 * (s23 * s5 - c5 * c23 * c4) + s1 * c5 * s4;
627 fMit[4][1][0] = s1 * (s23 * s5 - c5 * c23 * c4) - c1 * c5 * s4;
628 fMit[4][2][0] = s23 * c4 * c5 + c23 * s5;
629 fMit[4][0][1] = c1 * c23 * s4 + s1 * c4;
630 fMit[4][1][1] = s1 * c23 * s4 - c1 * c4;
631 fMit[4][2][1] = -s23 * s4;
632 fMit[4][0][2] = c1 * (s23 * c5 + s5 * c23 * c4) - s1 * s5 * s4;
633 fMit[4][1][2] = s1 * (s23 * c5 + s5 * c23 * c4) + c1 * s5 * s4;
634 fMit[4][2][2] = -s23 * c4 * s5 + c23 * c5;
635 fMit[4][0][3] = quickcomp2;
636 fMit[4][1][3] = quickcomp3;
637 fMit[4][2][3] = c23 *
d4 +
a3 * s23 -
a2 * s2 +
d1;
639 fMit[5][0][0] = c1 * (c23 * (c4 * c5 * c6 - s4 * s6) - s23 * s5 * c6) - s1 * (s4 * c5 * c6 + c4 * s6);
640 fMit[5][1][0] = -s1 * (c23 * (-c4 * c5 * c6 + s4 * s6) + s23 * s5 * c6) + c1 * (s4 * c5 * c6 + c4 * s6);
641 fMit[5][2][0] = s23 * (s4 * s6 - c4 * c5 * c6) - c23 * s5 * c6;
642 fMit[5][0][1] = -c1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) + s1 * (s4 * c5 * s6 - c4 * c6);
643 fMit[5][1][1] = -s1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) - c1 * (s4 * c5 * s6 - c4 * c6);
644 fMit[5][2][1] = s23 * (c4 * c5 * s6 + s4 * c6) + c23 * s5 * s6;
645 fMit[5][0][2] = c1 * (c23 * c4 * s5 + s23 * c5) - s1 * s4 * s5;
646 fMit[5][1][2] = s1 * (c23 * c4 * s5 + s23 * c5) + c1 * s4 * s5;
647 fMit[5][2][2] = -s23 * c4 * s5 + c23 * c5;
648 fMit[5][0][3] = quickcomp2;
649 fMit[5][1][3] = quickcomp3;
650 fMit[5][2][3] = s23 *
a3 + c23 *
d4 -
a2 * s2 +
d1;
652 fMit[6][0][0] = c1 * (c23 * (c4 * c5 * c6 - s4 * s6) - s23 * s5 * c6) - s1 * (s4 * c5 * c6 + c4 * s6);
653 fMit[6][1][0] = -s1 * (c23 * (-c4 * c5 * c6 + s4 * s6) + s23 * s5 * c6) + c1 * (s4 * c5 * c6 + c4 * s6);
654 fMit[6][2][0] = s23 * (s4 * s6 - c4 * c5 * c6) - c23 * s5 * c6;
655 fMit[6][0][1] = -c1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) + s1 * (s4 * c5 * s6 - c4 * c6);
656 fMit[6][1][1] = -s1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) - c1 * (s4 * c5 * s6 - c4 * c6);
657 fMit[6][2][1] = s23 * (c4 * c5 * s6 + s4 * c6) + c23 * s5 * s6;
658 fMit[6][0][2] = c1 * (c23 * c4 * s5 + s23 * c5) - s1 * s4 * s5;
659 fMit[6][1][2] = s1 * (c23 * c4 * s5 + s23 * c5) + c1 * s4 * s5;
660 fMit[6][2][2] = -s23 * c4 * s5 + c23 * c5;
661 fMit[6][0][3] = c1 * (c23 * (c4 * s5 *
d6 -
a3) + s23 * (c5 *
d6 +
d4) +
a1 +
a2 * c2) - s1 * s4 * s5 *
d6;
662 fMit[6][1][3] = s1 * (c23 * (c4 * s5 *
d6 -
a3) + s23 * (c5 *
d6 +
d4) +
a1 +
a2 * c2) + c1 * s4 * s5 *
d6;
663 fMit[6][2][3] = s23 * (
a3 - c4 * s5 *
d6) + c23 * (c5 *
d6 +
d4) -
a2 * s2 +
d1;
674 for (
int i = 0; i < 8; i++) {
697 std::cout <<
"Change the control mode from velocity to position control.\n";
707 std::cout <<
"Change the control mode from stop to velocity control.\n";
796 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
798 "Cannot send a velocity to the robot "
799 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
804 double scale_sat = 1;
816 vpERROR_TRACE(
"The velocity vector must have a size of 6 !!!!");
820 for (
unsigned int i = 0; i < 3; ++i) {
821 vel_abs = fabs(vel[i]);
823 vel_trans_max = vel_abs;
829 vel_abs = fabs(vel[i + 3]);
831 vel_rot_max = vel_abs;
838 double scale_trans_sat = 1;
839 double scale_rot_sat = 1;
846 if ((scale_trans_sat < 1) || (scale_rot_sat < 1)) {
847 if (scale_trans_sat < scale_rot_sat)
848 scale_sat = scale_trans_sat;
850 scale_sat = scale_rot_sat;
858 vpERROR_TRACE(
"The velocity vector must have a size of 6 !!!!");
861 for (
unsigned int i = 0; i < 6; ++i) {
862 vel_abs = fabs(vel[i]);
864 vel_rot_max = vel_abs;
870 double scale_rot_sat = 1;
873 if (scale_rot_sat < 1)
874 scale_sat = scale_rot_sat;
919 articularVelocity = eJe_ * eVc * velocityframe;
929 articularVelocity = fJe_ * velocityframe;
934 articularVelocity = velocityframe;
947 for (
unsigned int i = 0; i < 6; ++i) {
948 double vel_abs = fabs(articularVelocity[i]);
950 vel_rot_max = vel_abs;
953 articularVelocity[i], i + 1);
956 double scale_rot_sat = 1;
957 double scale_sat = 1;
961 if (scale_rot_sat < 1)
962 scale_sat = scale_rot_sat;
1032 vel = cVe * eJe_ * articularVelocity;
1036 vel = articularVelocity;
1042 vel = fJe_ * articularVelocity;
1051 "Case not taken in account.");
1152 double velmax = fabs(q[0]);
1153 for (
unsigned int i = 1; i < 6; i++) {
1154 if (velmax < fabs(q[i]))
1155 velmax = fabs(q[i]);
1240 "Modification of the robot state");
1255 for (
unsigned int i = 0; i < 3; i++) {
1270 qdes = articularCoordinates;
1276 error = qdes - articularCoordinates;
1280 if (errsqr < 1e-4) {
1291 }
while (errsqr > 1e-8 && nbSol > 0);
1299 error = q - articularCoordinates;
1306 if (errsqr < 1e-4) {
1313 }
while (errsqr > 1e-8);
1323 for (
unsigned int i = 0; i < 3; i++) {
1333 qdes = articularCoordinates;
1336 error = qdes - articularCoordinates;
1343 if (errsqr < 1e-4) {
1352 }
while (errsqr > 1e-8 && nbSol > 0);
1357 "Mixt frame not implemented.");
1361 "End-effector frame not implemented.");
1429 double pos4,
double pos5,
double pos6)
1581 for (
unsigned int i = 0; i < 3; i++) {
1589 vpERROR_TRACE(
"Positioning error. Mixt frame not implemented");
1591 "Mixt frame not implemented.");
1594 vpERROR_TRACE(
"Positioning error. Mixt frame not implemented");
1596 "End-effector frame not implemented.");
1654 for (
unsigned int j = 0; j < 3; j++) {
1655 position[j] = posRxyz[j];
1656 position[j + 3] = RtuVect[j];
1689 vpTRACE(
"Joint limit vector has not a size of 6 !");
1719 double c2 = cos(q2);
1720 double c3 = cos(q3);
1721 double s3 = sin(q3);
1722 double c23 = cos(q2 + q3);
1723 double s23 = sin(q2 + q3);
1724 double s5 = sin(q5);
1726 bool cond1 = fabs(s5) < 1e-1;
1727 bool cond2 = fabs(
a3 * s3 + c3 *
d4) < 1e-1;
1728 bool cond3 = fabs(
a2 * c2 -
a3 * c23 + s23 *
d4 +
a1) < 1e-1;
1790 for (
unsigned int i = 0; i < 6; i++) {
1791 if (articularCoordinates[i] <=
joint_min[i]) {
1792 difft =
joint_min[i] - articularCoordinates[i];
1795 artNumb = -(int)i - 1;
1800 for (
unsigned int i = 0; i < 6; i++) {
1801 if (articularCoordinates[i] >=
joint_max[i]) {
1802 difft = articularCoordinates[i] -
joint_max[i];
1805 artNumb = (int)(i + 1);
1811 std::cout <<
"\nWarning: Velocity control stopped: axis " << fabs((
float)artNumb) <<
" on joint limit!"
1842 if (!first_time_getdis) {
1845 std::cout <<
"getDisplacement() CAMERA_FRAME not implemented\n";
1850 displacement = q_cur - q_prev_getdis;
1855 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
1860 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
1864 std::cout <<
"getDisplacement() END_EFFECTOR_FRAME not implemented\n";
1869 first_time_getdis =
false;
1873 q_prev_getdis = q_cur;
1939 std::ifstream fd(filename.c_str(), std::ios::in);
1941 if (!fd.is_open()) {
1946 std::string key(
"R:");
1947 std::string id(
"#Viper850 - Position");
1948 bool pos_found =
false;
1953 while (std::getline(fd, line)) {
1956 if (!(line.compare(0,
id.size(),
id) == 0)) {
1957 std::cout <<
"Error: this position file " << filename <<
" is not for Viper850 robot" << std::endl;
1961 if ((line.compare(0, 1,
"#") == 0)) {
1964 if ((line.compare(0, key.size(), key) == 0)) {
1967 if (chain.size() <
njoint + 1)
1969 if (chain.size() <
njoint + 1)
1972 std::istringstream ss(line);
1975 for (
unsigned int i = 0; i <
njoint; i++)
1988 std::cout <<
"Error: unable to find a position for Viper850 robot in " << filename << std::endl;
2020 fd = fopen(filename.c_str(),
"w");
2025 #Viper - Position - Version 1.0\n\
2028 # Joint position in degrees\n\
2158 std::string scene_dir_;
2159 std::vector<std::string> scene_dirs =
vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(
";"));
2160 bool sceneDirExists =
false;
2161 for (
size_t i = 0; i < scene_dirs.size(); i++)
2163 scene_dir_ = scene_dirs[i];
2164 sceneDirExists =
true;
2167 if (!sceneDirExists) {
2170 std::cout <<
"The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
2172 std::cout <<
"Cannot get VISP_SCENES_DIR environment variable" << std::endl;
2176 unsigned int name_length = 30;
2177 if (scene_dir_.size() > FILENAME_MAX)
2180 unsigned int full_length = (
unsigned int)scene_dir_.size() + name_length;
2181 if (full_length > FILENAME_MAX)
2184 char *name_cam =
new char[full_length];
2186 strcpy(name_cam, scene_dir_.c_str());
2187 strcat(name_cam,
"/camera.bnd");
2190 if (arm_dir.size() > FILENAME_MAX)
2192 full_length = (
unsigned int)arm_dir.size() + name_length;
2193 if (full_length > FILENAME_MAX)
2196 char *name_arm =
new char[full_length];
2197 strcpy(name_arm, arm_dir.c_str());
2198 strcat(name_arm,
"/viper850_arm1.bnd");
2200 strcpy(name_arm, arm_dir.c_str());
2201 strcat(name_arm,
"/viper850_arm2.bnd");
2202 set_scene(name_arm,
robotArms + 1, 1.0);
2203 strcpy(name_arm, arm_dir.c_str());
2204 strcat(name_arm,
"/viper850_arm3.bnd");
2205 set_scene(name_arm,
robotArms + 2, 1.0);
2206 strcpy(name_arm, arm_dir.c_str());
2207 strcat(name_arm,
"/viper850_arm4.bnd");
2208 set_scene(name_arm,
robotArms + 3, 1.0);
2209 strcpy(name_arm, arm_dir.c_str());
2210 strcat(name_arm,
"/viper850_arm5.bnd");
2211 set_scene(name_arm,
robotArms + 4, 1.0);
2212 strcpy(name_arm, arm_dir.c_str());
2213 strcat(name_arm,
"/viper850_arm6.bnd");
2214 set_scene(name_arm,
robotArms + 5, 1.0);
2222 add_rfstack(IS_BACK);
2224 add_vwstack(
"start",
"depth", 0.0, 100.0);
2225 add_vwstack(
"start",
"window", -0.1, 0.1, -0.1, 0.1);
2226 add_vwstack(
"start",
"type", PERSPECTIVE);
2239 bool changed =
false;
2243 if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
2263 float w44o[4][4], w44cext[4][4], x, y, z;
2267 add_vwstack(
"start",
"cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
2268 x = w44cext[2][0] + w44cext[3][0];
2269 y = w44cext[2][1] + w44cext[3][1];
2270 z = w44cext[2][2] + w44cext[3][2];
2271 add_vwstack(
"start",
"vrp", x, y, z);
2272 add_vwstack(
"start",
"vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
2273 add_vwstack(
"start",
"vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
2274 add_vwstack(
"start",
"window", -u, u, -v, v);
2282 vp2jlc_matrix(fMit[0], w44o);
2285 vp2jlc_matrix(fMit[1], w44o);
2288 vp2jlc_matrix(fMit[2], w44o);
2291 vp2jlc_matrix(fMit[3], w44o);
2294 vp2jlc_matrix(fMit[6], w44o);
2301 cMe = fMit[6] * cMe;
2302 vp2jlc_matrix(cMe, w44o);
2307 vp2jlc_matrix(
fMo, w44o);
2348 std::cout <<
"Used joint coordinates (rad): " << articularCoordinates.
t() << std::endl;
2379 fMo = fMit[7] * cMo_;
2383 #elif !defined(VISP_BUILD_SHARED_LIBS)
2386 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 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.
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.
void computeArticularVelocity() override
static bool savePosFile(const std::string &filename, const vpColVector &q)
void get_fMi(vpHomogeneousMatrix *fMit) override
void setCameraParameters(const vpCameraParameters &cam)
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height)
void getExternalImage(vpImage< vpRGBa > &I)
void updateArticularPosition() override
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &displacement) override
void get_cMe(vpHomogeneousMatrix &cMe)
vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState) override
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity) override
virtual ~vpSimulatorViper850() override
void get_cVe(vpVelocityTwistMatrix &cVe)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q)
void findHighestPositioningSpeed(vpColVector &q)
void get_eJe(vpMatrix &eJe) override
bool singularityTest(const vpColVector &q, vpMatrix &J)
void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
static const double defaultPositioningVelocity
void initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q) override
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q) override
static bool readPosFile(const std::string &filename, vpColVector &q)
void move(const char *filename)
double getPositioningVelocity(void)
int isInJointLimit() override
bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo)
void get_fJe(vpMatrix &fJe) override
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)
Modelization 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()