64 #include <visp3/core/vpCameraParameters.h>
65 #include <visp3/detection/vpDetectorAprilTag.h>
66 #include <visp3/gui/vpDisplayGDI.h>
67 #include <visp3/gui/vpDisplayX.h>
68 #include <visp3/gui/vpPlot.h>
69 #include <visp3/io/vpImageIo.h>
70 #include <visp3/robot/vpRobotUniversalRobots.h>
71 #include <visp3/sensor/vpRealSense2.h>
72 #include <visp3/visual_features/vpFeatureBuilder.h>
73 #include <visp3/visual_features/vpFeaturePoint.h>
74 #include <visp3/vs/vpServo.h>
75 #include <visp3/vs/vpServoDisplay.h>
77 #if defined(VISP_HAVE_REALSENSE2) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && \
78 (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(VISP_HAVE_UR_RTDE)
81 std::vector<vpImagePoint> *traj_vip)
83 for (
size_t i = 0; i < vip.size(); i++) {
84 if (traj_vip[i].size()) {
87 traj_vip[i].push_back(vip[i]);
90 traj_vip[i].push_back(vip[i]);
93 for (
size_t i = 0; i < vip.size(); i++) {
94 for (
size_t j = 1; j < traj_vip[i].size(); j++) {
100 int main(
int argc,
char **argv)
102 double opt_tagSize = 0.120;
103 std::string opt_robot_ip =
"192.168.0.100";
104 std::string opt_eMc_filename =
"";
105 bool display_tag =
true;
106 int opt_quad_decimate = 2;
107 bool opt_verbose =
false;
108 bool opt_plot =
false;
109 bool opt_adaptive_gain =
false;
110 bool opt_task_sequencing =
false;
111 double convergence_threshold = 0.00005;
113 for (
int i = 1; i < argc; i++) {
114 if (std::string(argv[i]) ==
"--tag_size" && i + 1 < argc) {
115 opt_tagSize = std::stod(argv[i + 1]);
116 }
else if (std::string(argv[i]) ==
"--ip" && i + 1 < argc) {
117 opt_robot_ip = std::string(argv[i + 1]);
118 }
else if (std::string(argv[i]) ==
"--eMc" && i + 1 < argc) {
119 opt_eMc_filename = std::string(argv[i + 1]);
120 }
else if (std::string(argv[i]) ==
"--verbose") {
122 }
else if (std::string(argv[i]) ==
"--plot") {
124 }
else if (std::string(argv[i]) ==
"--adaptive_gain") {
125 opt_adaptive_gain =
true;
126 }
else if (std::string(argv[i]) ==
"--task_sequencing") {
127 opt_task_sequencing =
true;
128 }
else if (std::string(argv[i]) ==
"--quad_decimate" && i + 1 < argc) {
129 opt_quad_decimate = std::stoi(argv[i + 1]);
130 }
else if (std::string(argv[i]) ==
"--no-convergence-threshold") {
131 convergence_threshold = 0.;
132 }
else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
134 << argv[0] <<
" [--ip <default " << opt_robot_ip <<
">] [--tag_size <marker size in meter; default "
135 << opt_tagSize <<
">] [--eMc <eMc extrinsic file>] "
136 <<
"[--quad_decimate <decimation; default " << opt_quad_decimate
137 <<
">] [--adaptive_gain] [--plot] [--task_sequencing] [--no-convergence-threshold] [--verbose] [--help] [-h]"
146 robot.connect(opt_robot_ip);
148 std::cout <<
"WARNING: This example will move the robot! "
149 <<
"Please make sure to have the user stop button at hand!" << std::endl
150 <<
"Press Enter to continue..." << std::endl;
163 std::cout <<
"Move to joint position: " << q.t() << std::endl;
169 unsigned int width = 640, height = 480;
170 config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 30);
171 config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
172 config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 30);
181 ePc[3] = -0.00506562;
182 ePc[4] = -0.00293325;
186 if (!opt_eMc_filename.empty()) {
187 ePc.
loadYAML(opt_eMc_filename, ePc);
189 std::cout <<
"Warning, opt_eMc_filename is empty! Use hard coded values."
193 std::cout <<
"eMc:\n" << eMc <<
"\n";
198 std::cout <<
"cam:\n" << cam <<
"\n";
202 #if defined(VISP_HAVE_X11)
204 #elif defined(VISP_HAVE_GDI)
212 detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
213 detector.setDisplayTag(display_tag);
214 detector.setAprilTagQuadDecimate(opt_quad_decimate);
224 std::vector<vpFeaturePoint> p(4), pd(4);
227 std::vector<vpPoint> point(4);
228 point[0].setWorldCoordinates(-opt_tagSize / 2., -opt_tagSize / 2., 0);
229 point[1].setWorldCoordinates(opt_tagSize / 2., -opt_tagSize / 2., 0);
230 point[2].setWorldCoordinates(opt_tagSize / 2., opt_tagSize / 2., 0);
231 point[3].setWorldCoordinates(-opt_tagSize / 2., opt_tagSize / 2., 0);
235 for (
size_t i = 0; i < p.size(); i++) {
241 if (opt_adaptive_gain) {
248 vpPlot *plotter =
nullptr;
252 plotter =
new vpPlot(2,
static_cast<int>(250 * 2), 500,
static_cast<int>(I.
getWidth()) + 80, 10,
253 "Real time curves plotter");
254 plotter->
setTitle(0,
"Visual features error");
255 plotter->
setTitle(1,
"Camera velocities");
258 plotter->
setLegend(0, 0,
"error_feat_p1_x");
259 plotter->
setLegend(0, 1,
"error_feat_p1_y");
260 plotter->
setLegend(0, 2,
"error_feat_p2_x");
261 plotter->
setLegend(0, 3,
"error_feat_p2_y");
262 plotter->
setLegend(0, 4,
"error_feat_p3_x");
263 plotter->
setLegend(0, 5,
"error_feat_p3_y");
264 plotter->
setLegend(0, 6,
"error_feat_p4_x");
265 plotter->
setLegend(0, 7,
"error_feat_p4_y");
274 bool final_quit =
false;
275 bool has_converged =
false;
276 bool send_velocities =
false;
277 bool servo_started =
false;
278 std::vector<vpImagePoint> *traj_corners =
nullptr;
285 while (!has_converged && !final_quit) {
292 std::vector<vpHomogeneousMatrix> cMo_vec;
293 detector.detect(I, opt_tagSize, cam, cMo_vec);
296 std::stringstream ss;
297 ss <<
"Left click to " << (send_velocities ?
"stop the robot" :
"servo the robot") <<
", right click to quit.";
304 if (cMo_vec.size() == 1) {
307 static bool first_time =
true;
310 std::vector<vpHomogeneousMatrix> v_oMo(2), v_cdMc(2);
311 v_oMo[1].buildFrom(0, 0, 0, 0, 0, M_PI);
312 for (
size_t i = 0; i < 2; i++) {
313 v_cdMc[i] = cdMo * v_oMo[i] * cMo.
inverse();
315 if (std::fabs(v_cdMc[0].getThetaUVector().getTheta()) < std::fabs(v_cdMc[1].getThetaUVector().getTheta())) {
318 std::cout <<
"Desired frame modified to avoid PI rotation of the camera" << std::endl;
323 for (
size_t i = 0; i < point.size(); i++) {
325 point[i].changeFrame(cdMo * oMo, cP);
326 point[i].projection(cP, p_);
335 std::vector<vpImagePoint> corners = detector.getPolygon(0);
338 for (
size_t i = 0; i < corners.size(); i++) {
343 point[i].changeFrame(cMo, cP);
348 if (opt_task_sequencing) {
349 if (!servo_started) {
350 if (send_velocities) {
351 servo_started =
true;
362 for (
size_t i = 0; i < corners.size(); i++) {
363 std::stringstream ss;
373 traj_corners =
new std::vector<vpImagePoint>[corners.size()];
376 display_point_trajectory(I, corners, traj_corners);
380 plotter->
plot(1, iter_plot, v_c);
385 std::cout <<
"v_c: " << v_c.t() << std::endl;
389 std::stringstream ss;
390 ss <<
"error: " << error;
394 std::cout <<
"error: " << error << std::endl;
396 if (error < convergence_threshold) {
397 has_converged =
true;
398 std::cout <<
"Servo task has converged"
410 if (!send_velocities) {
418 std::stringstream ss;
428 send_velocities = !send_velocities;
441 std::cout <<
"Stop the robot " << std::endl;
444 if (opt_plot && plotter !=
nullptr) {
450 while (!final_quit) {
465 delete[] traj_corners;
468 std::cout <<
"ViSP exception: " << e.
what() << std::endl;
469 std::cout <<
"Stop the robot " << std::endl;
472 }
catch (
const std::exception &e) {
473 std::cout <<
"ur_rtde exception: " << e.what() << std::endl;
482 #if !defined(VISP_HAVE_REALSENSE2)
483 std::cout <<
"Install librealsense-2.x" << std::endl;
485 #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
486 std::cout <<
"Build ViSP with c++11 or higher compiler flag (cmake -DUSE_CXX_STANDARD=11)." << std::endl;
488 #if !defined(VISP_HAVE_UR_RTDE)
489 std::cout <<
"ViSP is not build with libur_rtde 3rd party used to control a robot from Universal Robots..."
Adaptive gain computation.
static bool loadYAML(const std::string &filename, vpArray2D< Type > &A, char *header=NULL)
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithDistortion
Perspective projection with distortion model.
Implementation of column vector and the associated operations.
static const vpColor green
@ TAG_36h11
AprilTag 36h11 pattern (recommended)
Display for windows using GDI (available on any windows 32 platform).
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
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 displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emited by ViSP classes.
const char * what() const
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
unsigned int getWidth() const
static double rad(double deg)
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
void initGraph(unsigned int graphNum, unsigned int curveNbr)
void setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
void setTitle(unsigned int graphNum, const std::string &title)
Implementation of a pose vector and operations on poses.
void acquire(vpImage< unsigned char > &grey, double *ts=NULL)
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion, int index=-1) const
bool open(const rs2::config &cfg=rs2::config())
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
@ STATE_POSITION_CONTROL
Initialize the position controller.
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Implementation of a rotation matrix and operations on such kind of matrices.
static void display(const vpServo &s, const vpCameraParameters &cam, const vpImage< unsigned char > &I, vpColor currentColor=vpColor::green, vpColor desiredColor=vpColor::red, unsigned int thickness=1)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
void setServo(const vpServoType &servo_type)
vpColVector getError() const
vpColVector computeControlLaw()
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Class that consider the case of a translation vector.
VISP_EXPORT double measureTimeMs()