67 #include <visp3/core/vpCameraParameters.h>
68 #include <visp3/gui/vpDisplayGDI.h>
69 #include <visp3/gui/vpDisplayX.h>
70 #include <visp3/io/vpImageIo.h>
71 #include <visp3/sensor/vpRealSense2.h>
72 #include <visp3/robot/vpRobotFranka.h>
73 #include <visp3/detection/vpDetectorAprilTag.h>
74 #include <visp3/visual_features/vpFeatureBuilder.h>
75 #include <visp3/visual_features/vpFeaturePoint.h>
76 #include <visp3/vs/vpServo.h>
77 #include <visp3/vs/vpServoDisplay.h>
78 #include <visp3/gui/vpPlot.h>
80 #if defined(VISP_HAVE_REALSENSE2) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && \
81 (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(VISP_HAVE_FRANKA)
84 std::vector<vpImagePoint> *traj_vip)
86 for (
size_t i = 0; i < vip.size(); i++) {
87 if (traj_vip[i].size()) {
90 traj_vip[i].push_back(vip[i]);
94 traj_vip[i].push_back(vip[i]);
97 for (
size_t i = 0; i < vip.size(); i++) {
98 for (
size_t j = 1; j < traj_vip[i].size(); j++) {
104 int main(
int argc,
char **argv)
106 double opt_tagSize = 0.120;
107 std::string opt_robot_ip =
"192.168.1.1";
108 std::string opt_eMc_filename =
"";
109 bool display_tag =
true;
110 int opt_quad_decimate = 2;
111 bool opt_verbose =
false;
112 bool opt_plot =
false;
113 bool opt_adaptive_gain =
false;
114 bool opt_task_sequencing =
false;
115 double convergence_threshold = 0.00005;
117 for (
int i = 1; i < argc; i++) {
118 if (std::string(argv[i]) ==
"--tag_size" && i + 1 < argc) {
119 opt_tagSize = std::stod(argv[i + 1]);
121 else if (std::string(argv[i]) ==
"--ip" && i + 1 < argc) {
122 opt_robot_ip = std::string(argv[i + 1]);
124 else if (std::string(argv[i]) ==
"--eMc" && i + 1 < argc) {
125 opt_eMc_filename = std::string(argv[i + 1]);
127 else if (std::string(argv[i]) ==
"--verbose") {
130 else if (std::string(argv[i]) ==
"--plot") {
133 else if (std::string(argv[i]) ==
"--adaptive_gain") {
134 opt_adaptive_gain =
true;
136 else if (std::string(argv[i]) ==
"--task_sequencing") {
137 opt_task_sequencing =
true;
139 else if (std::string(argv[i]) ==
"--quad_decimate" && i + 1 < argc) {
140 opt_quad_decimate = std::stoi(argv[i + 1]);
142 else if (std::string(argv[i]) ==
"--no-convergence-threshold") {
143 convergence_threshold = 0.;
145 else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
146 std::cout << argv[0] <<
" [--ip <default " << opt_robot_ip <<
">] [--tag_size <marker size in meter; default " << opt_tagSize <<
">] [--eMc <eMc extrinsic file>] "
147 <<
"[--quad_decimate <decimation; default " << opt_quad_decimate <<
">] [--adaptive_gain] [--plot] [--task_sequencing] [--no-convergence-threshold] [--verbose] [--help] [-h]"
156 robot.connect(opt_robot_ip);
160 unsigned int width = 640, height = 480;
161 config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 30);
162 config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
163 config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 30);
169 ePc[0] = 0.0337731; ePc[1] = -0.00535012; ePc[2] = -0.0523339;
170 ePc[3] = -0.247294; ePc[4] = -0.306729; ePc[5] = 1.53055;
173 if (!opt_eMc_filename.empty()) {
174 ePc.
loadYAML(opt_eMc_filename, ePc);
177 std::cout <<
"Warning, opt_eMc_filename is empty! Use hard coded values." <<
"\n";
180 std::cout <<
"eMc:\n" << eMc <<
"\n";
184 std::cout <<
"cam:\n" << cam <<
"\n";
188 #if defined(VISP_HAVE_X11)
190 #elif defined(VISP_HAVE_GDI)
210 std::vector<vpFeaturePoint> p(4), pd(4);
213 std::vector<vpPoint> point(4);
214 point[0].setWorldCoordinates(-opt_tagSize/2., -opt_tagSize/2., 0);
215 point[1].setWorldCoordinates( opt_tagSize/2., -opt_tagSize/2., 0);
216 point[2].setWorldCoordinates( opt_tagSize/2., opt_tagSize/2., 0);
217 point[3].setWorldCoordinates(-opt_tagSize/2., opt_tagSize/2., 0);
221 for (
size_t i = 0; i < p.size(); i++) {
227 if (opt_adaptive_gain) {
235 vpPlot *plotter =
nullptr;
239 plotter =
new vpPlot(2,
static_cast<int>(250 * 2), 500,
static_cast<int>(I.
getWidth()) + 80, 10,
"Real time curves plotter");
240 plotter->
setTitle(0,
"Visual features error");
241 plotter->
setTitle(1,
"Camera velocities");
244 plotter->
setLegend(0, 0,
"error_feat_p1_x");
245 plotter->
setLegend(0, 1,
"error_feat_p1_y");
246 plotter->
setLegend(0, 2,
"error_feat_p2_x");
247 plotter->
setLegend(0, 3,
"error_feat_p2_y");
248 plotter->
setLegend(0, 4,
"error_feat_p3_x");
249 plotter->
setLegend(0, 5,
"error_feat_p3_y");
250 plotter->
setLegend(0, 6,
"error_feat_p4_x");
251 plotter->
setLegend(0, 7,
"error_feat_p4_y");
260 bool final_quit =
false;
261 bool has_converged =
false;
262 bool send_velocities =
false;
263 bool servo_started =
false;
264 std::vector<vpImagePoint> *traj_corners =
nullptr;
271 while (!has_converged && !final_quit) {
278 std::vector<vpHomogeneousMatrix> cMo_vec;
279 detector.
detect(I, opt_tagSize, cam, cMo_vec);
282 std::stringstream ss;
283 ss <<
"Left click to " << (send_velocities ?
"stop the robot" :
"servo the robot") <<
", right click to quit.";
290 if (cMo_vec.size() == 1) {
293 static bool first_time =
true;
296 std::vector<vpHomogeneousMatrix> v_oMo(2), v_cdMc(2);
297 v_oMo[1].buildFrom(0, 0, 0, 0, 0, M_PI);
298 for (
size_t i = 0; i < 2; i++) {
299 v_cdMc[i] = cdMo * v_oMo[i] * cMo.
inverse();
301 if (std::fabs(v_cdMc[0].getThetaUVector().getTheta()) < std::fabs(v_cdMc[1].getThetaUVector().getTheta())) {
305 std::cout <<
"Desired frame modified to avoid PI rotation of the camera" << std::endl;
310 for (
size_t i = 0; i < point.size(); i++) {
312 point[i].changeFrame(cdMo * oMo, cP);
313 point[i].projection(cP, p_);
322 std::vector<vpImagePoint> corners = detector.
getPolygon(0);
325 for (
size_t i = 0; i < corners.size(); i++) {
330 point[i].changeFrame(cMo, cP);
335 if (opt_task_sequencing) {
336 if (! servo_started) {
337 if (send_velocities) {
338 servo_started =
true;
350 for (
size_t i = 0; i < corners.size(); i++) {
351 std::stringstream ss;
361 traj_corners =
new std::vector<vpImagePoint> [corners.size()];
364 display_point_trajectory(I, corners, traj_corners);
368 plotter->
plot(1, iter_plot, v_c);
373 std::cout <<
"v_c: " << v_c.t() << std::endl;
377 std::stringstream ss;
378 ss <<
"error: " << error;
382 std::cout <<
"error: " << error << std::endl;
384 if (error < convergence_threshold) {
385 has_converged =
true;
386 std::cout <<
"Servo task has converged" <<
"\n";
397 if (!send_velocities) {
405 std::stringstream ss;
415 send_velocities = !send_velocities;
428 std::cout <<
"Stop the robot " << std::endl;
431 if (opt_plot && plotter !=
nullptr) {
439 while (!final_quit) {
454 delete [] traj_corners;
458 std::cout <<
"ViSP exception: " << e.
what() << std::endl;
459 std::cout <<
"Stop the robot " << std::endl;
463 catch(
const franka::NetworkException &e) {
464 std::cout <<
"Franka network exception: " << e.what() << std::endl;
465 std::cout <<
"Check if you are connected to the Franka robot"
466 <<
" or if you specified the right IP using --ip command line option set by default to 192.168.1.1. " << std::endl;
469 catch(
const std::exception &e) {
470 std::cout <<
"Franka exception: " << e.what() << std::endl;
479 #if !defined(VISP_HAVE_REALSENSE2)
480 std::cout <<
"Install librealsense-2.x" << std::endl;
482 #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
483 std::cout <<
"Build ViSP with c++11 or higher compiler flag (cmake -DUSE_CXX_STANDARD=11)." << std::endl;
485 #if !defined(VISP_HAVE_FRANKA)
486 std::cout <<
"Install libfranka." << std::endl;