36 #include <visp3/robot/vpSimulatorViper850.h>
38 #if defined(VISP_HAVE_MODULE_GUI) && defined(VISP_HAVE_THREADS)
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()
81 positioningVelocity(defaultPositioningVelocity), zeroPos(), reposPos(), toolCustom(false), arm_dir()
106 for (
int i = 0; i < 6; i++)
127 std::vector<std::string> arm_dirs =
vpIoTools::splitChain(std::string(VISP_ROBOT_ARMS_DIR), std::string(
";"));
128 bool armDirExists =
false;
129 for (
size_t i = 0; i < arm_dirs.size(); i++)
131 arm_dir = arm_dirs[i];
138 std::cout <<
"The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
141 std::cout <<
"Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
155 zeroPos[1] = -M_PI / 2;
159 reposPos[1] = -M_PI / 2;
161 reposPos[4] = M_PI / 2;
168 first_time_getdis =
true;
246 etc[0] = -0.04558630174;
247 etc[1] = -0.00134326752;
248 etc[2] = 0.001000828017;
256 std::cout <<
"This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
282 const unsigned int &image_height)
291 if (image_width == 640 && image_height == 480) {
297 vpTRACE(
"Cannot get default intrinsic camera parameters for this image "
304 if (image_width == 640 && image_height == 480) {
310 vpTRACE(
"Cannot get default intrinsic camera parameters for this image "
318 std::cout <<
"This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
369 double tcur_1 =
tcur;
373 bool setVelocityCalled_ =
false;
389 double ellapsedTime = (
tcur -
tprev) * 1e-3;
407 articularVelocities = 0.0;
413 articularCoordinates[0] = articularCoordinates[0] + ellapsedTime * articularVelocities[0];
414 articularCoordinates[1] = articularCoordinates[1] + ellapsedTime * articularVelocities[1];
415 articularCoordinates[2] = articularCoordinates[2] + ellapsedTime * articularVelocities[2];
416 articularCoordinates[3] = articularCoordinates[3] + ellapsedTime * articularVelocities[3];
417 articularCoordinates[4] = articularCoordinates[4] + ellapsedTime * articularVelocities[4];
418 articularCoordinates[5] = articularCoordinates[5] + ellapsedTime * articularVelocities[5];
424 ellapsedTime = (
joint_min[(
unsigned int)(-jl - 1)] - articularCoordinates[(
unsigned int)(-jl - 1)]) /
425 (articularVelocities[(
unsigned int)(-jl - 1)]);
427 ellapsedTime = (
joint_max[(
unsigned int)(jl - 1)] - articularCoordinates[(
unsigned int)(jl - 1)]) /
428 (articularVelocities[(
unsigned int)(jl - 1)]);
430 for (
unsigned int i = 0; i < 6; i++)
431 articularCoordinates[i] = articularCoordinates[i] + ellapsedTime * articularVelocities[i];
471 for (
int k = 1; k < 7; k++) {
529 double c23 = cos(q2 + q3);
530 double s23 = sin(q2 + q3);
547 fMit[0][0][3] =
a1 * c1;
548 fMit[0][1][3] =
a1 * s1;
551 fMit[1][0][0] = c1 * c2;
552 fMit[1][1][0] = s1 * c2;
554 fMit[1][0][1] = -c1 * s2;
555 fMit[1][1][1] = -s1 * s2;
560 fMit[1][0][3] = c1 * (
a2 * c2 +
a1);
561 fMit[1][1][3] = s1 * (
a2 * c2 +
a1);
562 fMit[1][2][3] =
d1 -
a2 * s2;
564 double quickcomp1 =
a3 * c23 -
a2 * c2 -
a1;
566 fMit[2][0][0] = -c1 * c23;
567 fMit[2][1][0] = -s1 * c23;
572 fMit[2][0][2] = c1 * s23;
573 fMit[2][1][2] = s1 * s23;
575 fMit[2][0][3] = -c1 * quickcomp1;
576 fMit[2][1][3] = -s1 * quickcomp1;
577 fMit[2][2][3] =
a3 * s23 -
a2 * s2 +
d1;
579 double quickcomp2 = c1 * (s23 *
d4 - quickcomp1);
580 double quickcomp3 = s1 * (s23 *
d4 - quickcomp1);
582 fMit[3][0][0] = -c1 * c23 * c4 + s1 * s4;
583 fMit[3][1][0] = -s1 * c23 * c4 - c1 * s4;
584 fMit[3][2][0] = s23 * c4;
585 fMit[3][0][1] = c1 * s23;
586 fMit[3][1][1] = s1 * s23;
588 fMit[3][0][2] = -c1 * c23 * s4 - s1 * c4;
589 fMit[3][1][2] = -s1 * c23 * s4 + c1 * c4;
590 fMit[3][2][2] = s23 * s4;
591 fMit[3][0][3] = quickcomp2;
592 fMit[3][1][3] = quickcomp3;
593 fMit[3][2][3] = c23 *
d4 +
a3 * s23 -
a2 * s2 +
d1;
595 fMit[4][0][0] = c1 * (s23 * s5 - c5 * c23 * c4) + s1 * c5 * s4;
596 fMit[4][1][0] = s1 * (s23 * s5 - c5 * c23 * c4) - c1 * c5 * s4;
597 fMit[4][2][0] = s23 * c4 * c5 + c23 * s5;
598 fMit[4][0][1] = c1 * c23 * s4 + s1 * c4;
599 fMit[4][1][1] = s1 * c23 * s4 - c1 * c4;
600 fMit[4][2][1] = -s23 * s4;
601 fMit[4][0][2] = c1 * (s23 * c5 + s5 * c23 * c4) - s1 * s5 * s4;
602 fMit[4][1][2] = s1 * (s23 * c5 + s5 * c23 * c4) + c1 * s5 * s4;
603 fMit[4][2][2] = -s23 * c4 * s5 + c23 * c5;
604 fMit[4][0][3] = quickcomp2;
605 fMit[4][1][3] = quickcomp3;
606 fMit[4][2][3] = c23 *
d4 +
a3 * s23 -
a2 * s2 +
d1;
608 fMit[5][0][0] = c1 * (c23 * (c4 * c5 * c6 - s4 * s6) - s23 * s5 * c6) - s1 * (s4 * c5 * c6 + c4 * s6);
609 fMit[5][1][0] = -s1 * (c23 * (-c4 * c5 * c6 + s4 * s6) + s23 * s5 * c6) + c1 * (s4 * c5 * c6 + c4 * s6);
610 fMit[5][2][0] = s23 * (s4 * s6 - c4 * c5 * c6) - c23 * s5 * c6;
611 fMit[5][0][1] = -c1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) + s1 * (s4 * c5 * s6 - c4 * c6);
612 fMit[5][1][1] = -s1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) - c1 * (s4 * c5 * s6 - c4 * c6);
613 fMit[5][2][1] = s23 * (c4 * c5 * s6 + s4 * c6) + c23 * s5 * s6;
614 fMit[5][0][2] = c1 * (c23 * c4 * s5 + s23 * c5) - s1 * s4 * s5;
615 fMit[5][1][2] = s1 * (c23 * c4 * s5 + s23 * c5) + c1 * s4 * s5;
616 fMit[5][2][2] = -s23 * c4 * s5 + c23 * c5;
617 fMit[5][0][3] = quickcomp2;
618 fMit[5][1][3] = quickcomp3;
619 fMit[5][2][3] = s23 *
a3 + c23 *
d4 -
a2 * s2 +
d1;
621 fMit[6][0][0] = c1 * (c23 * (c4 * c5 * c6 - s4 * s6) - s23 * s5 * c6) - s1 * (s4 * c5 * c6 + c4 * s6);
622 fMit[6][1][0] = -s1 * (c23 * (-c4 * c5 * c6 + s4 * s6) + s23 * s5 * c6) + c1 * (s4 * c5 * c6 + c4 * s6);
623 fMit[6][2][0] = s23 * (s4 * s6 - c4 * c5 * c6) - c23 * s5 * c6;
624 fMit[6][0][1] = -c1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) + s1 * (s4 * c5 * s6 - c4 * c6);
625 fMit[6][1][1] = -s1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) - c1 * (s4 * c5 * s6 - c4 * c6);
626 fMit[6][2][1] = s23 * (c4 * c5 * s6 + s4 * c6) + c23 * s5 * s6;
627 fMit[6][0][2] = c1 * (c23 * c4 * s5 + s23 * c5) - s1 * s4 * s5;
628 fMit[6][1][2] = s1 * (c23 * c4 * s5 + s23 * c5) + c1 * s4 * s5;
629 fMit[6][2][2] = -s23 * c4 * s5 + c23 * c5;
630 fMit[6][0][3] = c1 * (c23 * (c4 * s5 *
d6 -
a3) + s23 * (c5 *
d6 +
d4) +
a1 +
a2 * c2) - s1 * s4 * s5 *
d6;
631 fMit[6][1][3] = s1 * (c23 * (c4 * s5 *
d6 -
a3) + s23 * (c5 *
d6 +
d4) +
a1 +
a2 * c2) + c1 * s4 * s5 *
d6;
632 fMit[6][2][3] = s23 * (
a3 - c4 * s5 *
d6) + c23 * (c5 *
d6 +
d4) -
a2 * s2 +
d1;
643 for (
int i = 0; i < 8; i++) {
666 std::cout <<
"Change the control mode from velocity to position control.\n";
677 std::cout <<
"Change the control mode from stop to velocity control.\n";
766 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
768 "Cannot send a velocity to the robot "
769 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
774 double scale_sat = 1;
786 vpERROR_TRACE(
"The velocity vector must have a size of 6 !!!!");
790 for (
unsigned int i = 0; i < 3; ++i) {
791 vel_abs = fabs(vel[i]);
793 vel_trans_max = vel_abs;
799 vel_abs = fabs(vel[i + 3]);
801 vel_rot_max = vel_abs;
808 double scale_trans_sat = 1;
809 double scale_rot_sat = 1;
816 if ((scale_trans_sat < 1) || (scale_rot_sat < 1)) {
817 if (scale_trans_sat < scale_rot_sat)
818 scale_sat = scale_trans_sat;
820 scale_sat = scale_rot_sat;
828 vpERROR_TRACE(
"The velocity vector must have a size of 6 !!!!");
831 for (
unsigned int i = 0; i < 6; ++i) {
832 vel_abs = fabs(vel[i]);
834 vel_rot_max = vel_abs;
840 double scale_rot_sat = 1;
843 if (scale_rot_sat < 1)
844 scale_sat = scale_rot_sat;
889 articularVelocity = eJe_ * eVc * velocityframe;
899 articularVelocity = fJe_ * velocityframe;
904 articularVelocity = velocityframe;
917 for (
unsigned int i = 0; i < 6; ++i) {
918 double vel_abs = fabs(articularVelocity[i]);
920 vel_rot_max = vel_abs;
923 articularVelocity[i], i + 1);
926 double scale_rot_sat = 1;
927 double scale_sat = 1;
931 if (scale_rot_sat < 1)
932 scale_sat = scale_rot_sat;
1002 vel = cVe * eJe_ * articularVelocity;
1006 vel = articularVelocity;
1012 vel = fJe_ * articularVelocity;
1021 "Case not taken in account.");
1122 double velmax = fabs(q[0]);
1123 for (
unsigned int i = 1; i < 6; i++) {
1124 if (velmax < fabs(q[i]))
1125 velmax = fabs(q[i]);
1210 "Modification of the robot state");
1225 for (
unsigned int i = 0; i < 3; i++) {
1240 qdes = articularCoordinates;
1246 error = qdes - articularCoordinates;
1250 if (errsqr < 1e-4) {
1262 }
while (errsqr > 1e-8 && nbSol > 0);
1270 error = q - articularCoordinates;
1277 if (errsqr < 1e-4) {
1284 }
while (errsqr > 1e-8);
1294 for (
unsigned int i = 0; i < 3; i++) {
1304 qdes = articularCoordinates;
1307 error = qdes - articularCoordinates;
1314 if (errsqr < 1e-4) {
1324 }
while (errsqr > 1e-8 && nbSol > 0);
1329 "Mixt frame not implemented.");
1333 "End-effector frame not implemented.");
1401 double pos4,
double pos5,
double pos6)
1554 for (
unsigned int i = 0; i < 3; i++) {
1562 vpERROR_TRACE(
"Positioning error. Mixt frame not implemented");
1564 "Mixt frame not implemented.");
1567 vpERROR_TRACE(
"Positioning error. Mixt frame not implemented");
1569 "End-effector frame not implemented.");
1627 for (
unsigned int j = 0; j < 3; j++) {
1628 position[j] = posRxyz[j];
1629 position[j + 3] = RtuVect[j];
1662 vpTRACE(
"Joint limit vector has not a size of 6 !");
1692 double c2 = cos(q2);
1693 double c3 = cos(q3);
1694 double s3 = sin(q3);
1695 double c23 = cos(q2 + q3);
1696 double s23 = sin(q2 + q3);
1697 double s5 = sin(q5);
1699 bool cond1 = fabs(s5) < 1e-1;
1700 bool cond2 = fabs(
a3 * s3 + c3 *
d4) < 1e-1;
1701 bool cond3 = fabs(
a2 * c2 -
a3 * c23 + s23 *
d4 +
a1) < 1e-1;
1763 for (
unsigned int i = 0; i < 6; i++) {
1764 if (articularCoordinates[i] <=
joint_min[i]) {
1765 difft =
joint_min[i] - articularCoordinates[i];
1768 artNumb = -(int)i - 1;
1773 for (
unsigned int i = 0; i < 6; i++) {
1774 if (articularCoordinates[i] >=
joint_max[i]) {
1775 difft = articularCoordinates[i] -
joint_max[i];
1778 artNumb = (int)(i + 1);
1784 std::cout <<
"\nWarning: Velocity control stopped: axis " << fabs((
float)artNumb) <<
" on joint limit!"
1815 if (!first_time_getdis) {
1818 std::cout <<
"getDisplacement() CAMERA_FRAME not implemented\n";
1823 displacement = q_cur - q_prev_getdis;
1828 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
1833 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
1837 std::cout <<
"getDisplacement() END_EFFECTOR_FRAME not implemented\n";
1843 first_time_getdis =
false;
1847 q_prev_getdis = q_cur;
1913 std::ifstream fd(filename.c_str(), std::ios::in);
1915 if (!fd.is_open()) {
1920 std::string key(
"R:");
1921 std::string id(
"#Viper850 - Position");
1922 bool pos_found =
false;
1927 while (std::getline(fd, line)) {
1930 if (!(line.compare(0,
id.size(),
id) == 0)) {
1931 std::cout <<
"Error: this position file " << filename <<
" is not for Viper850 robot" << std::endl;
1935 if ((line.compare(0, 1,
"#") == 0)) {
1938 if ((line.compare(0, key.size(), key) == 0)) {
1941 if (chain.size() <
njoint + 1)
1943 if (chain.size() <
njoint + 1)
1946 std::istringstream ss(line);
1949 for (
unsigned int i = 0; i <
njoint; i++)
1962 std::cout <<
"Error: unable to find a position for Viper850 robot in " << filename << std::endl;
1994 fd = fopen(filename.c_str(),
"w");
1999 #Viper - Position - Version 1.0\n\
2002 # Joint position in degrees\n\
2135 std::string scene_dir_;
2136 std::vector<std::string> scene_dirs =
vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(
";"));
2137 bool sceneDirExists =
false;
2138 for (
size_t i = 0; i < scene_dirs.size(); i++)
2140 scene_dir_ = scene_dirs[i];
2141 sceneDirExists =
true;
2144 if (!sceneDirExists) {
2147 std::cout <<
"The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
2150 std::cout <<
"Cannot get VISP_SCENES_DIR environment variable" << std::endl;
2154 unsigned int name_length = 30;
2155 if (scene_dir_.size() > FILENAME_MAX)
2158 unsigned int full_length = (
unsigned int)scene_dir_.size() + name_length;
2159 if (full_length > FILENAME_MAX)
2162 char *name_cam =
new char[full_length];
2164 strcpy(name_cam, scene_dir_.c_str());
2165 strcat(name_cam,
"/camera.bnd");
2168 if (arm_dir.size() > FILENAME_MAX)
2170 full_length = (
unsigned int)arm_dir.size() + name_length;
2171 if (full_length > FILENAME_MAX)
2174 char *name_arm =
new char[full_length];
2175 strcpy(name_arm, arm_dir.c_str());
2176 strcat(name_arm,
"/viper850_arm1.bnd");
2178 strcpy(name_arm, arm_dir.c_str());
2179 strcat(name_arm,
"/viper850_arm2.bnd");
2180 set_scene(name_arm,
robotArms + 1, 1.0);
2181 strcpy(name_arm, arm_dir.c_str());
2182 strcat(name_arm,
"/viper850_arm3.bnd");
2183 set_scene(name_arm,
robotArms + 2, 1.0);
2184 strcpy(name_arm, arm_dir.c_str());
2185 strcat(name_arm,
"/viper850_arm4.bnd");
2186 set_scene(name_arm,
robotArms + 3, 1.0);
2187 strcpy(name_arm, arm_dir.c_str());
2188 strcat(name_arm,
"/viper850_arm5.bnd");
2189 set_scene(name_arm,
robotArms + 4, 1.0);
2190 strcpy(name_arm, arm_dir.c_str());
2191 strcat(name_arm,
"/viper850_arm6.bnd");
2192 set_scene(name_arm,
robotArms + 5, 1.0);
2200 add_rfstack(IS_BACK);
2202 add_vwstack(
"start",
"depth", 0.0, 100.0);
2203 add_vwstack(
"start",
"window", -0.1, 0.1, -0.1, 0.1);
2204 add_vwstack(
"start",
"type", PERSPECTIVE);
2217 bool changed =
false;
2221 if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
2242 float w44o[4][4], w44cext[4][4], x, y, z;
2246 add_vwstack(
"start",
"cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
2247 x = w44cext[2][0] + w44cext[3][0];
2248 y = w44cext[2][1] + w44cext[3][1];
2249 z = w44cext[2][2] + w44cext[3][2];
2250 add_vwstack(
"start",
"vrp", x, y, z);
2251 add_vwstack(
"start",
"vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
2252 add_vwstack(
"start",
"vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
2253 add_vwstack(
"start",
"window", -u, u, -v, v);
2261 vp2jlc_matrix(fMit[0], w44o);
2264 vp2jlc_matrix(fMit[1], w44o);
2267 vp2jlc_matrix(fMit[2], w44o);
2270 vp2jlc_matrix(fMit[3], w44o);
2273 vp2jlc_matrix(fMit[6], w44o);
2280 cMe = fMit[6] * cMe;
2281 vp2jlc_matrix(cMe, w44o);
2286 vp2jlc_matrix(
fMo, w44o);
2327 std::cout <<
"Used joint coordinates (rad): " << articularCoordinates.
t() << std::endl;
2358 fMo = fMit[7] * cMo_;
2362 #elif !defined(VISP_BUILD_SHARED_LIBS)
2365 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)
static void launcher(vpRobotWireFrameSimulator &simulator)
void set_displayBusy(const bool &status)
vpHomogeneousMatrix * fMi
vpCameraParameters cameraParam
vpHomogeneousMatrix getExternalCameraPosition() const
std::mutex m_mutex_robotStop
vpDisplayRobotType displayType
void set_artCoord(const vpColVector &coord)
unsigned int jointLimitArt
bool constantSamplingTimeMode
void setExternalCameraPosition(const vpHomogeneousMatrix &camMf_)
std::mutex m_mutex_setVelocityCalled
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 getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &displacement) vp_override
static bool savePosFile(const std::string &filename, const vpColVector &q)
void updateArticularPosition() vp_override
int isInJointLimit() vp_override
virtual ~vpSimulatorViper850() vp_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 get_cMe(vpHomogeneousMatrix &cMe)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity) vp_override
void get_cVe(vpVelocityTwistMatrix &cVe)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q)
void findHighestPositioningSpeed(vpColVector &q)
void computeArticularVelocity() vp_override
void get_eJe(vpMatrix &eJe) vp_override
void get_fJe(vpMatrix &fJe) vp_override
vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState) vp_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 get_fMi(vpHomogeneousMatrix *fMit) vp_override
static bool readPosFile(const std::string &filename, vpColVector &q)
void move(const char *filename)
double getPositioningVelocity(void)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q) vp_override
bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q) vp_override
void initArms() vp_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()