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);
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";
840 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
842 "Cannot send a velocity to the robot " 843 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
848 double scale_sat = 1;
860 vpERROR_TRACE(
"The velocity vector must have a size of 6 !!!!");
864 for (
unsigned int i = 0; i < 3; ++i) {
865 vel_abs = fabs(vel[i]);
867 vel_trans_max = vel_abs;
873 vel_abs = fabs(vel[i + 3]);
875 vel_rot_max = vel_abs;
882 double scale_trans_sat = 1;
883 double scale_rot_sat = 1;
890 if ((scale_trans_sat < 1) || (scale_rot_sat < 1)) {
891 if (scale_trans_sat < scale_rot_sat)
892 scale_sat = scale_trans_sat;
894 scale_sat = scale_rot_sat;
902 vpERROR_TRACE(
"The velocity vector must have a size of 6 !!!!");
905 for (
unsigned int i = 0; i < 6; ++i) {
906 vel_abs = fabs(vel[i]);
908 vel_rot_max = vel_abs;
914 double scale_rot_sat = 1;
917 if (scale_rot_sat < 1)
918 scale_sat = scale_rot_sat;
953 articularVelocity = eJe_ * eVc * velocityframe;
963 articularVelocity = fJe_ * velocityframe;
968 articularVelocity = velocityframe;
981 for (
unsigned int i = 0; i < 6; ++i) {
982 double vel_abs = fabs(articularVelocity[i]);
984 vel_rot_max = vel_abs;
987 articularVelocity[i], i + 1);
990 double scale_rot_sat = 1;
991 double scale_sat = 1;
995 if (scale_rot_sat < 1)
996 scale_sat = scale_rot_sat;
1067 vel = cVe * eJe_ * articularVelocity;
1071 vel = articularVelocity;
1077 vel = fJe_ * articularVelocity;
1086 "Case not taken in account.");
1188 double velmax = fabs(q[0]);
1189 for (
unsigned int i = 1; i < 6; i++) {
1190 if (velmax < fabs(q[i]))
1191 velmax = fabs(q[i]);
1277 "Modification of the robot state");
1292 for (
unsigned int i = 0; i < 3; i++) {
1307 qdes = articularCoordinates;
1311 error = qdes - articularCoordinates;
1315 if (errsqr < 1e-4) {
1326 }
while (errsqr > 1e-8 && nbSol > 0);
1334 error = q - articularCoordinates;
1339 if (errsqr < 1e-4) {
1346 }
while (errsqr > 1e-8);
1356 for (
unsigned int i = 0; i < 3; i++) {
1366 qdes = articularCoordinates;
1369 error = qdes - articularCoordinates;
1374 if (errsqr < 1e-4) {
1383 }
while (errsqr > 1e-8 && nbSol > 0);
1388 "Mixt frame not implemented.");
1392 "End-effector frame not implemented.");
1461 const double pos3,
const double pos4,
const double pos5,
const double pos6)
1615 for (
unsigned int i = 0; i < 3; i++) {
1623 vpERROR_TRACE(
"Positionning error. Mixt frame not implemented");
1625 "Mixt frame not implemented.");
1628 vpERROR_TRACE(
"Positionning error. Mixt frame not implemented");
1630 "End-effector frame not implemented.");
1688 for (
unsigned int j = 0; j < 3; j++) {
1689 position[j] = posRxyz[j];
1690 position[j + 3] = RtuVect[j];
1723 vpTRACE(
"Joint limit vector has not a size of 6 !");
1753 double c2 = cos(q2);
1754 double c3 = cos(q3);
1755 double s3 = sin(q3);
1756 double c23 = cos(q2 + q3);
1757 double s23 = sin(q2 + q3);
1758 double s5 = sin(q5);
1760 bool cond1 = fabs(s5) < 1e-1;
1761 bool cond2 = fabs(
a3 * s3 + c3 *
d4) < 1e-1;
1762 bool cond3 = fabs(
a2 * c2 -
a3 * c23 + s23 * d4 +
a1) < 1e-1;
1824 for (
unsigned int i = 0; i < 6; i++) {
1825 if (articularCoordinates[i] <=
joint_min[i]) {
1826 difft =
joint_min[i] - articularCoordinates[i];
1829 artNumb = -(int)i - 1;
1834 for (
unsigned int i = 0; i < 6; i++) {
1835 if (articularCoordinates[i] >=
joint_max[i]) {
1836 difft = articularCoordinates[i] -
joint_max[i];
1839 artNumb = (int)(i + 1);
1845 std::cout <<
"\nWarning: Velocity control stopped: axis " << fabs((
float)artNumb) <<
" on joint limit!" 1876 if (!first_time_getdis) {
1879 std::cout <<
"getDisplacement() CAMERA_FRAME not implemented\n";
1884 displacement = q_cur - q_prev_getdis;
1889 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
1894 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
1898 std::cout <<
"getDisplacement() END_EFFECTOR_FRAME not implemented\n";
1903 first_time_getdis =
false;
1907 q_prev_getdis = q_cur;
1974 std::ifstream fd(filename.c_str(), std::ios::in);
1976 if (!fd.is_open()) {
1981 std::string key(
"R:");
1982 std::string id(
"#Viper850 - Position");
1983 bool pos_found =
false;
1988 while (std::getline(fd, line)) {
1991 if (!(line.compare(0,
id.size(), id) == 0)) {
1992 std::cout <<
"Error: this position file " << filename <<
" is not for Viper850 robot" << std::endl;
1996 if ((line.compare(0, 1,
"#") == 0)) {
1999 if ((line.compare(0, key.size(), key) == 0)) {
2002 if (chain.size() <
njoint + 1)
2004 if (chain.size() <
njoint + 1)
2007 std::istringstream ss(line);
2010 for (
unsigned int i = 0; i <
njoint; i++)
2023 std::cout <<
"Error: unable to find a position for Viper850 robot in " << filename << std::endl;
2055 fd = fopen(filename.c_str(),
"w");
2060 #Viper - Position - Version 1.0\n\ 2063 # Joint position in degrees\n\ 2193 std::string scene_dir_;
2194 std::vector<std::string> scene_dirs =
vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(
";"));
2195 bool sceneDirExists =
false;
2196 for (
size_t i = 0; i < scene_dirs.size(); i++)
2198 scene_dir_ = scene_dirs[i];
2199 sceneDirExists =
true;
2202 if (!sceneDirExists) {
2205 std::cout <<
"The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
2207 std::cout <<
"Cannot get VISP_SCENES_DIR environment variable" << std::endl;
2211 unsigned int name_length = 30;
2212 if (scene_dir_.size() > FILENAME_MAX)
2215 unsigned int full_length = (
unsigned int)scene_dir_.size() + name_length;
2216 if (full_length > FILENAME_MAX)
2219 char *name_cam =
new char[full_length];
2221 strcpy(name_cam, scene_dir_.c_str());
2222 strcat(name_cam,
"/camera.bnd");
2225 if (arm_dir.size() > FILENAME_MAX)
2227 full_length = (
unsigned int)arm_dir.size() + name_length;
2228 if (full_length > FILENAME_MAX)
2231 char *name_arm =
new char[full_length];
2232 strcpy(name_arm, arm_dir.c_str());
2233 strcat(name_arm,
"/viper850_arm1.bnd");
2235 strcpy(name_arm, arm_dir.c_str());
2236 strcat(name_arm,
"/viper850_arm2.bnd");
2237 set_scene(name_arm,
robotArms + 1, 1.0);
2238 strcpy(name_arm, arm_dir.c_str());
2239 strcat(name_arm,
"/viper850_arm3.bnd");
2240 set_scene(name_arm,
robotArms + 2, 1.0);
2241 strcpy(name_arm, arm_dir.c_str());
2242 strcat(name_arm,
"/viper850_arm4.bnd");
2243 set_scene(name_arm,
robotArms + 3, 1.0);
2244 strcpy(name_arm, arm_dir.c_str());
2245 strcat(name_arm,
"/viper850_arm5.bnd");
2246 set_scene(name_arm,
robotArms + 4, 1.0);
2247 strcpy(name_arm, arm_dir.c_str());
2248 strcat(name_arm,
"/viper850_arm6.bnd");
2249 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);
2273 bool changed =
false;
2277 if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
2297 float w44o[4][4], w44cext[4][4], x, y, z;
2301 add_vwstack(
"start",
"cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
2302 x = w44cext[2][0] + w44cext[3][0];
2303 y = w44cext[2][1] + w44cext[3][1];
2304 z = w44cext[2][2] + w44cext[3][2];
2305 add_vwstack(
"start",
"vrp", x, y, z);
2306 add_vwstack(
"start",
"vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
2307 add_vwstack(
"start",
"vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
2308 add_vwstack(
"start",
"window", -u, u, -v, v);
2316 vp2jlc_matrix(fMit[0], w44o);
2319 vp2jlc_matrix(fMit[1], w44o);
2322 vp2jlc_matrix(fMit[2], w44o);
2325 vp2jlc_matrix(fMit[3], w44o);
2328 vp2jlc_matrix(fMit[6], w44o);
2335 cMe = fMit[6] * cMe;
2336 vp2jlc_matrix(cMe, w44o);
2341 vp2jlc_matrix(
fMo, w44o);
2381 std::cout <<
"Used joint coordinates (rad): " << articularCoordinates.
t() << std::endl;
2411 fMo = fMit[7] * cMo_;
2414 #elif !defined(VISP_BUILD_SHARED_LIBS) 2417 void dummy_vpSimulatorViper850(){};
static void displayCamera(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness)
Implementation of a matrix and operations on matrices.
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
VISP_EXPORT int wait(double t0, double t)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
vpHomogeneousMatrix eMc
End effector to camera transformation.
Error that can be emited by the vpRobot class and its derivates.
vpColVector get_artCoord()
virtual ~vpSimulatorViper850()
void get_fMi(vpHomogeneousMatrix *fMit)
void computeArticularVelocity()
static const vpToolType defaultTool
Default tool attached to the robot end effector.
unsigned int getWidth() const
void get_cMe(vpHomogeneousMatrix &cMe)
double getSamplingTime() const
Implementation of an homogeneous matrix and operations on such kind of matrices.
unsigned int jointLimitArt
static void * launcher(void *arg)
bool singularityTest(const vpColVector &q, vpMatrix &J)
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
VISP_EXPORT double measureTimeSecond()
void getExternalImage(vpImage< vpRGBa > &I)
bool constantSamplingTimeMode
bool singularityManagement
double getMaxTranslationVelocity(void) const
static const vpColor none
Initialize the position controller.
error that can be emited by ViSP classes.
void track(const vpHomogeneousMatrix &cMo)
void get_fJe(vpMatrix &fJe)
vpColVector get_velocity()
double get_y() const
Get the point y coordinate in the image plane.
static const char *const CONST_MARLIN_F033C_CAMERA_NAME
static const vpColor green
static void flush(const vpImage< unsigned char > &I)
double getMaxRotationVelocity(void) const
static bool savePosFile(const std::string &filename, const vpColVector &q)
VISP_EXPORT double measureTimeMs()
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Class that defines what is a point.
static Type maximum(const Type &a, const Type &b)
Implementation of a rotation matrix and operations on such kind of matrices.
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
pthread_mutex_t mutex_artCoord
pthread_mutex_t mutex_display
void move(const char *filename)
vpDisplayRobotType displayType
Initialize the velocity controller.
vpCameraParameters cameraParam
void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
void setToolType(vpViper850::vpToolType tool)
Set the current tool type.
Initialize the acceleration controller.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void set_displayBusy(const bool &status)
vpCameraParametersProjType
void initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo)
static void display(const vpImage< unsigned char > &I)
void setExternalCameraPosition(const vpHomogeneousMatrix &camMf_)
vpToolType
List of possible tools that can be attached to the robot end-effector.
Generic class defining intrinsic camera parameters.
vpHomogeneousMatrix getExternalCameraPosition() const
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &displacement)
Modelisation of the ADEPT Viper 850 robot.
void extract(vpRotationMatrix &R) const
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
double get_x() const
Get the point x coordinate in the image plane.
vpToolType getToolType() const
Get the current tool type.
static Type minimum(const Type &a, const Type &b)
static bool readPosFile(const std::string &filename, vpColVector &q)
unsigned int getRows() const
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false) const
void set_velocity(const vpColVector &vel)
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static double rad(double deg)
void setExternalCameraParameters(const vpCameraParameters &cam)
vpControlFrameType getRobotFrame(void) const
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
This class aims to be a basis used to create all the simulators of robots.
vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
vpHomogeneousMatrix * fMi
pthread_mutex_t mutex_fMi
void get_cMe(vpHomogeneousMatrix &cMe) const
void updateArticularPosition()
void display_scene(Matrix mat, Bound_scene &sc, const vpImage< vpRGBa > &I, const vpColor &color)
vpHomogeneousMatrix camMf
vpHomogeneousMatrix navigation(const vpImage< vpRGBa > &I, bool &changed)
static double deg(double rad)
Implementation of column vector and the associated operations.
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))
void setCameraParameters(const vpCameraParameters &cam)
Implementation of a pose vector and operations on poses.
vpHomogeneousMatrix inverse() const
virtual vpRobotStateType getRobotState(void) const
unsigned int getHeight() const
Implementation of a rotation vector as Euler angle minimal representation.
vpControlFrameType setRobotFrame(vpRobot::vpControlFrameType newFrame)
vpMatrix pseudoInverse(double svThreshold=1e-6) const
void findHighestPositioningSpeed(vpColVector &q)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
void set_artCoord(const vpColVector &coord)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)
void get_cVe(vpVelocityTwistMatrix &cVe)
vpCameraParameters::vpCameraParametersProjType projModel
VISP_EXPORT double getMinTimeForUsleepCall()
void set_artVel(const vpColVector &vel)
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q)
pthread_mutex_t mutex_velocity
static const double defaultPositioningVelocity
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
double getPositioningVelocity(void)
pthread_mutex_t mutex_artVel
static const unsigned int njoint
Number of joint.
vpHomogeneousMatrix camMf2
bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo)
static const char *const CONST_PTGREY_FLEA2_CAMERA_NAME
void get_eJe(vpMatrix &eJe)
void resize(const unsigned int i, const bool flagNullify=true)