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 mutex_fMi = CreateMutexEx(NULL, NULL, 0, NULL);
79 mutex_fMi = CreateMutex(NULL, FALSE, NULL);
86 DWORD dwThreadIdArray;
87 hThread = CreateThread(NULL,
93 #elif defined(VISP_HAVE_PTHREAD)
100 pthread_attr_init(&
attr);
101 pthread_attr_setdetachstate(&
attr, PTHREAD_CREATE_JOINABLE);
117 positioningVelocity(defaultPositioningVelocity), zeroPos(), reposPos(), toolCustom(false), arm_dir()
126 mutex_fMi = CreateMutexEx(NULL, NULL, 0, NULL);
132 mutex_fMi = CreateMutex(NULL, FALSE, NULL);
139 DWORD dwThreadIdArray;
140 hThread = CreateThread(NULL,
146 #elif defined(VISP_HAVE_PTHREAD)
153 pthread_attr_init(&
attr);
154 pthread_attr_setdetachstate(&
attr, PTHREAD_CREATE_JOINABLE);
170 #if defined(WINRT_8_1)
171 WaitForSingleObjectEx(hThread, INFINITE, FALSE);
173 WaitForSingleObject(hThread, INFINITE);
175 CloseHandle(hThread);
181 #elif defined(VISP_HAVE_PTHREAD)
182 pthread_attr_destroy(&
attr);
183 pthread_join(
thread, NULL);
193 for (
int i = 0; i < 6; i++)
213 std::vector<std::string> arm_dirs =
vpIoTools::splitChain(std::string(VISP_ROBOT_ARMS_DIR), std::string(
";"));
214 bool armDirExists =
false;
215 for (
size_t i = 0; i < arm_dirs.size(); i++)
217 arm_dir = arm_dirs[i];
224 std::cout <<
"The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
226 std::cout <<
"Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
240 zeroPos[1] = -M_PI / 2;
244 reposPos[1] = -M_PI / 2;
246 reposPos[4] = M_PI / 2;
253 first_time_getdis =
true;
331 etc[0] = -0.04558630174;
332 etc[1] = -0.00134326752;
333 etc[2] = 0.001000828017;
341 std::cout <<
"This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
364 const unsigned int &image_height)
373 if (image_width == 640 && image_height == 480) {
378 vpTRACE(
"Cannot get default intrinsic camera parameters for this image "
385 if (image_width == 640 && image_height == 480) {
390 vpTRACE(
"Cannot get default intrinsic camera parameters for this image "
398 std::cout <<
"This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
449 double tcur_1 =
tcur;
461 double ellapsedTime = (
tcur -
tprev) * 1e-3;
479 articularVelocities = 0.0;
484 articularCoordinates[0] = articularCoordinates[0] + ellapsedTime * articularVelocities[0];
485 articularCoordinates[1] = articularCoordinates[1] + ellapsedTime * articularVelocities[1];
486 articularCoordinates[2] = articularCoordinates[2] + ellapsedTime * articularVelocities[2];
487 articularCoordinates[3] = articularCoordinates[3] + ellapsedTime * articularVelocities[3];
488 articularCoordinates[4] = articularCoordinates[4] + ellapsedTime * articularVelocities[4];
489 articularCoordinates[5] = articularCoordinates[5] + ellapsedTime * articularVelocities[5];
495 ellapsedTime = (
joint_min[(
unsigned int)(-jl - 1)] - articularCoordinates[(
unsigned int)(-jl - 1)]) /
496 (articularVelocities[(
unsigned int)(-jl - 1)]);
498 ellapsedTime = (
joint_max[(
unsigned int)(jl - 1)] - articularCoordinates[(
unsigned int)(jl - 1)]) /
499 (articularVelocities[(
unsigned int)(jl - 1)]);
501 for (
unsigned int i = 0; i < 6; i++)
502 articularCoordinates[i] = articularCoordinates[i] + ellapsedTime * articularVelocities[i];
541 for (
int k = 1; k < 7; k++) {
595 double c23 = cos(q2 + q3);
596 double s23 = sin(q2 + q3);
613 fMit[0][0][3] =
a1 * c1;
614 fMit[0][1][3] =
a1 * s1;
617 fMit[1][0][0] = c1 * c2;
618 fMit[1][1][0] = s1 * c2;
620 fMit[1][0][1] = -c1 * s2;
621 fMit[1][1][1] = -s1 * s2;
626 fMit[1][0][3] = c1 * (
a2 * c2 +
a1);
627 fMit[1][1][3] = s1 * (
a2 * c2 +
a1);
628 fMit[1][2][3] =
d1 -
a2 * s2;
630 double quickcomp1 =
a3 * c23 -
a2 * c2 -
a1;
632 fMit[2][0][0] = -c1 * c23;
633 fMit[2][1][0] = -s1 * c23;
638 fMit[2][0][2] = c1 * s23;
639 fMit[2][1][2] = s1 * s23;
641 fMit[2][0][3] = -c1 * quickcomp1;
642 fMit[2][1][3] = -s1 * quickcomp1;
643 fMit[2][2][3] =
a3 * s23 -
a2 * s2 +
d1;
645 double quickcomp2 = c1 * (s23 *
d4 - quickcomp1);
646 double quickcomp3 = s1 * (s23 *
d4 - quickcomp1);
648 fMit[3][0][0] = -c1 * c23 * c4 + s1 * s4;
649 fMit[3][1][0] = -s1 * c23 * c4 - c1 * s4;
650 fMit[3][2][0] = s23 * c4;
651 fMit[3][0][1] = c1 * s23;
652 fMit[3][1][1] = s1 * s23;
654 fMit[3][0][2] = -c1 * c23 * s4 - s1 * c4;
655 fMit[3][1][2] = -s1 * c23 * s4 + c1 * c4;
656 fMit[3][2][2] = s23 * s4;
657 fMit[3][0][3] = quickcomp2;
658 fMit[3][1][3] = quickcomp3;
659 fMit[3][2][3] = c23 *
d4 +
a3 * s23 -
a2 * s2 +
d1;
661 fMit[4][0][0] = c1 * (s23 * s5 - c5 * c23 * c4) + s1 * c5 * s4;
662 fMit[4][1][0] = s1 * (s23 * s5 - c5 * c23 * c4) - c1 * c5 * s4;
663 fMit[4][2][0] = s23 * c4 * c5 + c23 * s5;
664 fMit[4][0][1] = c1 * c23 * s4 + s1 * c4;
665 fMit[4][1][1] = s1 * c23 * s4 - c1 * c4;
666 fMit[4][2][1] = -s23 * s4;
667 fMit[4][0][2] = c1 * (s23 * c5 + s5 * c23 * c4) - s1 * s5 * s4;
668 fMit[4][1][2] = s1 * (s23 * c5 + s5 * c23 * c4) + c1 * s5 * s4;
669 fMit[4][2][2] = -s23 * c4 * s5 + c23 * c5;
670 fMit[4][0][3] = quickcomp2;
671 fMit[4][1][3] = quickcomp3;
672 fMit[4][2][3] = c23 *
d4 +
a3 * s23 -
a2 * s2 +
d1;
674 fMit[5][0][0] = c1 * (c23 * (c4 * c5 * c6 - s4 * s6) - s23 * s5 * c6) - s1 * (s4 * c5 * c6 + c4 * s6);
675 fMit[5][1][0] = -s1 * (c23 * (-c4 * c5 * c6 + s4 * s6) + s23 * s5 * c6) + c1 * (s4 * c5 * c6 + c4 * s6);
676 fMit[5][2][0] = s23 * (s4 * s6 - c4 * c5 * c6) - c23 * s5 * c6;
677 fMit[5][0][1] = -c1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) + s1 * (s4 * c5 * s6 - c4 * c6);
678 fMit[5][1][1] = -s1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) - c1 * (s4 * c5 * s6 - c4 * c6);
679 fMit[5][2][1] = s23 * (c4 * c5 * s6 + s4 * c6) + c23 * s5 * s6;
680 fMit[5][0][2] = c1 * (c23 * c4 * s5 + s23 * c5) - s1 * s4 * s5;
681 fMit[5][1][2] = s1 * (c23 * c4 * s5 + s23 * c5) + c1 * s4 * s5;
682 fMit[5][2][2] = -s23 * c4 * s5 + c23 * c5;
683 fMit[5][0][3] = quickcomp2;
684 fMit[5][1][3] = quickcomp3;
685 fMit[5][2][3] = s23 *
a3 + c23 *
d4 -
a2 * s2 +
d1;
687 fMit[6][0][0] = c1 * (c23 * (c4 * c5 * c6 - s4 * s6) - s23 * s5 * c6) - s1 * (s4 * c5 * c6 + c4 * s6);
688 fMit[6][1][0] = -s1 * (c23 * (-c4 * c5 * c6 + s4 * s6) + s23 * s5 * c6) + c1 * (s4 * c5 * c6 + c4 * s6);
689 fMit[6][2][0] = s23 * (s4 * s6 - c4 * c5 * c6) - c23 * s5 * c6;
690 fMit[6][0][1] = -c1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) + s1 * (s4 * c5 * s6 - c4 * c6);
691 fMit[6][1][1] = -s1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) - c1 * (s4 * c5 * s6 - c4 * c6);
692 fMit[6][2][1] = s23 * (c4 * c5 * s6 + s4 * c6) + c23 * s5 * s6;
693 fMit[6][0][2] = c1 * (c23 * c4 * s5 + s23 * c5) - s1 * s4 * s5;
694 fMit[6][1][2] = s1 * (c23 * c4 * s5 + s23 * c5) + c1 * s4 * s5;
695 fMit[6][2][2] = -s23 * c4 * s5 + c23 * c5;
696 fMit[6][0][3] = c1 * (c23 * (c4 * s5 *
d6 -
a3) + s23 * (c5 *
d6 +
d4) +
a1 +
a2 * c2) - s1 * s4 * s5 *
d6;
697 fMit[6][1][3] = s1 * (c23 * (c4 * s5 *
d6 -
a3) + s23 * (c5 *
d6 +
d4) +
a1 +
a2 * c2) + c1 * s4 * s5 *
d6;
698 fMit[6][2][3] = s23 * (
a3 - c4 * s5 *
d6) + c23 * (c5 *
d6 +
d4) -
a2 * s2 +
d1;
707 #if defined(WINRT_8_1)
708 WaitForSingleObjectEx(
mutex_fMi, INFINITE, FALSE);
710 WaitForSingleObject(
mutex_fMi, INFINITE);
712 for (
int i = 0; i < 8; i++)
715 #elif defined(VISP_HAVE_PTHREAD)
717 for (
int i = 0; i < 8; i++)
740 std::cout <<
"Change the control mode from velocity to position control.\n";
750 std::cout <<
"Change the control mode from stop to velocity control.\n";
839 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
841 "Cannot send a velocity to the robot "
842 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
847 double scale_sat = 1;
859 vpERROR_TRACE(
"The velocity vector must have a size of 6 !!!!");
863 for (
unsigned int i = 0; i < 3; ++i) {
864 vel_abs = fabs(vel[i]);
866 vel_trans_max = vel_abs;
872 vel_abs = fabs(vel[i + 3]);
874 vel_rot_max = vel_abs;
881 double scale_trans_sat = 1;
882 double scale_rot_sat = 1;
889 if ((scale_trans_sat < 1) || (scale_rot_sat < 1)) {
890 if (scale_trans_sat < scale_rot_sat)
891 scale_sat = scale_trans_sat;
893 scale_sat = scale_rot_sat;
901 vpERROR_TRACE(
"The velocity vector must have a size of 6 !!!!");
904 for (
unsigned int i = 0; i < 6; ++i) {
905 vel_abs = fabs(vel[i]);
907 vel_rot_max = vel_abs;
913 double scale_rot_sat = 1;
916 if (scale_rot_sat < 1)
917 scale_sat = scale_rot_sat;
952 articularVelocity = eJe_ * eVc * velocityframe;
962 articularVelocity = fJe_ * velocityframe;
967 articularVelocity = velocityframe;
980 for (
unsigned int i = 0; i < 6; ++i) {
981 double vel_abs = fabs(articularVelocity[i]);
983 vel_rot_max = vel_abs;
986 articularVelocity[i], i + 1);
989 double scale_rot_sat = 1;
990 double scale_sat = 1;
994 if (scale_rot_sat < 1)
995 scale_sat = scale_rot_sat;
1065 vel = cVe * eJe_ * articularVelocity;
1069 vel = articularVelocity;
1075 vel = fJe_ * articularVelocity;
1084 "Case not taken in account.");
1185 double velmax = fabs(q[0]);
1186 for (
unsigned int i = 1; i < 6; i++) {
1187 if (velmax < fabs(q[i]))
1188 velmax = fabs(q[i]);
1273 "Modification of the robot state");
1288 for (
unsigned int i = 0; i < 3; i++) {
1303 qdes = articularCoordinates;
1307 error = qdes - articularCoordinates;
1311 if (errsqr < 1e-4) {
1322 }
while (errsqr > 1e-8 && nbSol > 0);
1330 error = q - articularCoordinates;
1335 if (errsqr < 1e-4) {
1342 }
while (errsqr > 1e-8);
1352 for (
unsigned int i = 0; i < 3; i++) {
1362 qdes = articularCoordinates;
1365 error = qdes - articularCoordinates;
1370 if (errsqr < 1e-4) {
1379 }
while (errsqr > 1e-8 && nbSol > 0);
1384 "Mixt frame not implemented.");
1388 "End-effector frame not implemented.");
1456 double pos3,
double pos4,
double pos5,
double pos6)
1608 for (
unsigned int i = 0; i < 3; i++) {
1616 vpERROR_TRACE(
"Positionning error. Mixt frame not implemented");
1618 "Mixt frame not implemented.");
1621 vpERROR_TRACE(
"Positionning error. Mixt frame not implemented");
1623 "End-effector frame not implemented.");
1681 for (
unsigned int j = 0; j < 3; j++) {
1682 position[j] = posRxyz[j];
1683 position[j + 3] = RtuVect[j];
1716 vpTRACE(
"Joint limit vector has not a size of 6 !");
1746 double c2 = cos(q2);
1747 double c3 = cos(q3);
1748 double s3 = sin(q3);
1749 double c23 = cos(q2 + q3);
1750 double s23 = sin(q2 + q3);
1751 double s5 = sin(q5);
1753 bool cond1 = fabs(s5) < 1e-1;
1754 bool cond2 = fabs(
a3 * s3 + c3 *
d4) < 1e-1;
1755 bool cond3 = fabs(
a2 * c2 -
a3 * c23 + s23 *
d4 +
a1) < 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";
1877 displacement = q_cur - q_prev_getdis;
1882 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
1887 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
1891 std::cout <<
"getDisplacement() END_EFFECTOR_FRAME not implemented\n";
1896 first_time_getdis =
false;
1900 q_prev_getdis = q_cur;
1966 std::ifstream fd(filename.c_str(), std::ios::in);
1968 if (!fd.is_open()) {
1973 std::string key(
"R:");
1974 std::string id(
"#Viper850 - Position");
1975 bool pos_found =
false;
1980 while (std::getline(fd, line)) {
1983 if (!(line.compare(0,
id.size(),
id) == 0)) {
1984 std::cout <<
"Error: this position file " << filename <<
" is not for Viper850 robot" << std::endl;
1988 if ((line.compare(0, 1,
"#") == 0)) {
1991 if ((line.compare(0, key.size(), key) == 0)) {
1994 if (chain.size() <
njoint + 1)
1996 if (chain.size() <
njoint + 1)
1999 std::istringstream ss(line);
2002 for (
unsigned int i = 0; i <
njoint; i++)
2015 std::cout <<
"Error: unable to find a position for Viper850 robot in " << filename << std::endl;
2047 fd = fopen(filename.c_str(),
"w");
2052 #Viper - Position - Version 1.0\n\
2055 # Joint position in degrees\n\
2185 std::string scene_dir_;
2186 std::vector<std::string> scene_dirs =
vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(
";"));
2187 bool sceneDirExists =
false;
2188 for (
size_t i = 0; i < scene_dirs.size(); i++)
2190 scene_dir_ = scene_dirs[i];
2191 sceneDirExists =
true;
2194 if (!sceneDirExists) {
2197 std::cout <<
"The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
2199 std::cout <<
"Cannot get VISP_SCENES_DIR environment variable" << std::endl;
2203 unsigned int name_length = 30;
2204 if (scene_dir_.size() > FILENAME_MAX)
2207 unsigned int full_length = (
unsigned int)scene_dir_.size() + name_length;
2208 if (full_length > FILENAME_MAX)
2211 char *name_cam =
new char[full_length];
2213 strcpy(name_cam, scene_dir_.c_str());
2214 strcat(name_cam,
"/camera.bnd");
2217 if (arm_dir.size() > FILENAME_MAX)
2219 full_length = (
unsigned int)arm_dir.size() + name_length;
2220 if (full_length > FILENAME_MAX)
2223 char *name_arm =
new char[full_length];
2224 strcpy(name_arm, arm_dir.c_str());
2225 strcat(name_arm,
"/viper850_arm1.bnd");
2227 strcpy(name_arm, arm_dir.c_str());
2228 strcat(name_arm,
"/viper850_arm2.bnd");
2229 set_scene(name_arm,
robotArms + 1, 1.0);
2230 strcpy(name_arm, arm_dir.c_str());
2231 strcat(name_arm,
"/viper850_arm3.bnd");
2232 set_scene(name_arm,
robotArms + 2, 1.0);
2233 strcpy(name_arm, arm_dir.c_str());
2234 strcat(name_arm,
"/viper850_arm4.bnd");
2235 set_scene(name_arm,
robotArms + 3, 1.0);
2236 strcpy(name_arm, arm_dir.c_str());
2237 strcat(name_arm,
"/viper850_arm5.bnd");
2238 set_scene(name_arm,
robotArms + 4, 1.0);
2239 strcpy(name_arm, arm_dir.c_str());
2240 strcat(name_arm,
"/viper850_arm6.bnd");
2241 set_scene(name_arm,
robotArms + 5, 1.0);
2249 add_rfstack(IS_BACK);
2251 add_vwstack(
"start",
"depth", 0.0, 100.0);
2252 add_vwstack(
"start",
"window", -0.1, 0.1, -0.1, 0.1);
2253 add_vwstack(
"start",
"type", PERSPECTIVE);
2265 bool changed =
false;
2269 if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
2289 float w44o[4][4], w44cext[4][4], x, y, z;
2293 add_vwstack(
"start",
"cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
2294 x = w44cext[2][0] + w44cext[3][0];
2295 y = w44cext[2][1] + w44cext[3][1];
2296 z = w44cext[2][2] + w44cext[3][2];
2297 add_vwstack(
"start",
"vrp", x, y, z);
2298 add_vwstack(
"start",
"vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
2299 add_vwstack(
"start",
"vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
2300 add_vwstack(
"start",
"window", -u, u, -v, v);
2308 vp2jlc_matrix(fMit[0], w44o);
2311 vp2jlc_matrix(fMit[1], w44o);
2314 vp2jlc_matrix(fMit[2], w44o);
2317 vp2jlc_matrix(fMit[3], w44o);
2320 vp2jlc_matrix(fMit[6], w44o);
2327 cMe = fMit[6] * cMe;
2328 vp2jlc_matrix(cMe, w44o);
2333 vp2jlc_matrix(
fMo, w44o);
2373 std::cout <<
"Used joint coordinates (rad): " << articularCoordinates.
t() << std::endl;
2403 fMo = fMit[7] * cMo_;
2406 #elif !defined(VISP_BUILD_SHARED_LIBS)
2409 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 flush(const vpImage< unsigned char > &I)
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))
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)
pthread_mutex_t mutex_display
void set_displayBusy(const bool &status)
pthread_mutex_t mutex_fMi
vpHomogeneousMatrix * fMi
vpCameraParameters cameraParam
pthread_mutex_t mutex_artCoord
vpHomogeneousMatrix getExternalCameraPosition() const
static void * launcher(void *arg)
vpDisplayRobotType displayType
void set_artCoord(const vpColVector &coord)
unsigned int jointLimitArt
bool constantSamplingTimeMode
pthread_mutex_t mutex_velocity
void setExternalCameraPosition(const vpHomogeneousMatrix &camMf_)
void set_artVel(const vpColVector &vel)
pthread_mutex_t mutex_artVel
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()