44 #include <visp3/core/vpImageConvert.h>
45 #include <visp3/core/vpCameraParameters.h>
46 #include <visp3/core/vpXmlParserCamera.h>
47 #include <visp3/gui/vpDisplayGDI.h>
48 #include <visp3/gui/vpDisplayX.h>
49 #include <visp3/io/vpImageIo.h>
50 #include <visp3/sensor/vpRealSense2.h>
51 #include <visp3/robot/vpRobotFranka.h>
52 #include <visp3/detection/vpDetectorAprilTag.h>
53 #include <visp3/visual_features/vpFeatureThetaU.h>
54 #include <visp3/visual_features/vpFeatureTranslation.h>
55 #include <visp3/vs/vpServo.h>
56 #include <visp3/gui/vpPlot.h>
58 #if defined(VISP_HAVE_REALSENSE2) && defined(VISP_HAVE_CPP11_COMPATIBILITY) && \
59 (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(VISP_HAVE_FRANKA)
61 int main(
int argc,
char **argv)
63 double opt_tagSize = 0.120;
64 std::string opt_robot_ip =
"192.168.1.1";
65 std::string opt_eMc_filename =
"";
66 bool display_tag =
true;
67 int opt_quad_decimate = 2;
68 bool opt_verbose =
false;
69 bool opt_plot =
false;
70 bool opt_adaptive_gain =
false;
71 bool opt_task_sequencing =
false;
72 double convergence_threshold_t = 0.0005, convergence_threshold_tu =
vpMath::rad(0.5);
74 for (
int i = 1; i < argc; i++) {
75 if (std::string(argv[i]) ==
"--tag_size" && i + 1 < argc) {
76 opt_tagSize = std::stod(argv[i + 1]);
78 else if (std::string(argv[i]) ==
"--ip" && i + 1 < argc) {
79 opt_robot_ip = std::string(argv[i + 1]);
81 else if (std::string(argv[i]) ==
"--eMc" && i + 1 < argc) {
82 opt_eMc_filename = std::string(argv[i + 1]);
84 else if (std::string(argv[i]) ==
"--verbose") {
87 else if (std::string(argv[i]) ==
"--plot") {
90 else if (std::string(argv[i]) ==
"--adaptive_gain") {
91 opt_adaptive_gain =
true;
93 else if (std::string(argv[i]) ==
"--task_sequencing") {
94 opt_task_sequencing =
true;
96 else if (std::string(argv[i]) ==
"--quad_decimate" && i + 1 < argc) {
97 opt_quad_decimate = std::stod(argv[i + 1]);
99 else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
100 std::cout << argv[0] <<
" [--ip <default " << opt_robot_ip <<
">] [--tag_size <marker size in meter; default " << opt_tagSize <<
">] [--eMc <eMc extrinsic file>] "
101 <<
"[--quad_decimate <decimation; default " << opt_quad_decimate <<
">] [--adaptive_gain] [--plot] [--task_sequencing] [--verbose] [--help] [-h]"
110 robot.connect(opt_robot_ip);
114 unsigned int width = 640, height = 480;
115 config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 30);
116 config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
117 config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 30);
122 ePc[0] = 0.0337731; ePc[1] = -0.00535012; ePc[2] = -0.0523339;
123 ePc[3] = -0.247294; ePc[4] = -0.306729; ePc[5] = 1.53055;
125 if (!opt_eMc_filename.empty()) {
126 ePc.
loadYAML(opt_eMc_filename, ePc);
129 std::cout <<
"Warning, opt_eMc_filename is empty! Use hard coded values." <<
"\n";
132 std::cout <<
"eMc:\n" << eMc <<
"\n";
136 std::cout <<
"cam:\n" << cam <<
"\n";
140 #if defined(VISP_HAVE_X11)
142 #elif defined(VISP_HAVE_GDI)
150 detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
151 detector.setDisplayTag(display_tag);
152 detector.setAprilTagQuadDecimate(opt_quad_decimate);
158 cdMo[0][0] = 1; cdMo[0][1] = 0; cdMo[0][2] = 0;
159 cdMo[1][0] = 0; cdMo[1][1] = -1; cdMo[1][2] = 0;
160 cdMo[2][0] = 0; cdMo[2][1] = 0; cdMo[2][2] = -1;
180 if (opt_adaptive_gain) {
192 plotter =
new vpPlot(2, 250 * 2, 500, I.
getWidth() + 80, 10,
"Real time curves plotter");
193 plotter->
setTitle(0,
"Visual features error");
194 plotter->
setTitle(1,
"Camera velocities");
197 plotter->
setLegend(0, 0,
"error_feat_tx");
198 plotter->
setLegend(0, 1,
"error_feat_ty");
199 plotter->
setLegend(0, 2,
"error_feat_tz");
200 plotter->
setLegend(0, 3,
"error_feat_theta_ux");
201 plotter->
setLegend(0, 4,
"error_feat_theta_uy");
202 plotter->
setLegend(0, 5,
"error_feat_theta_uz");
211 bool final_quit =
false;
212 bool has_converged =
false;
213 bool send_velocities =
false;
214 bool servo_started =
false;
221 while (!has_converged && !final_quit) {
228 std::vector<vpHomogeneousMatrix> cMo_vec;
229 detector.detect(I, opt_tagSize, cam, cMo_vec);
231 std::stringstream ss;
232 ss <<
"Left click to " << (send_velocities ?
"stop the robot" :
"servo the robot") <<
", right click to quit.";
238 if (cMo_vec.size() == 1) {
241 static bool first_time =
true;
244 std::vector<vpHomogeneousMatrix> v_oMo(2), v_cdMc(2);
245 v_oMo[1].buildFrom(0, 0, 0, 0, 0, M_PI);
246 for (
size_t i = 0; i < 2; i++) {
247 v_cdMc[i] = cdMo * v_oMo[i] * cMo.
inverse();
249 if (std::fabs(v_cdMc[0].getThetaUVector().getTheta()) < std::fabs(v_cdMc[1].getThetaUVector().getTheta())) {
253 std::cout <<
"Desired frame modified to avoid PI rotation of the camera" << std::endl;
264 cdMc = cdMo * oMo * cMo.
inverse();
268 if (opt_task_sequencing) {
269 if (! servo_started) {
270 if (send_velocities) {
271 servo_started =
true;
283 plotter->
plot(1, iter_plot, v_c);
288 std::cout <<
"v_c: " << v_c.t() << std::endl;
293 double error_t = sqrt(cd_t_c.
sumSquare());
297 ss <<
"error_t: " << error_t;
300 ss <<
"error_tu: " << error_tu;
304 std::cout <<
"error translation: " << error_t <<
" ; error rotation: " << error_tu <<
"\n";
306 if (error_t < convergence_threshold_t && error_tu < convergence_threshold_tu) {
307 has_converged =
true;
308 std::cout <<
"Servo task has converged" <<
"\n";
316 if (!send_velocities) {
332 send_velocities = !send_velocities;
345 std::cout <<
"Stop the robot " << std::endl;
348 if (opt_plot && plotter != NULL) {
356 while (!final_quit) {
372 std::cout <<
"ViSP exception: " << e.
what() << std::endl;
373 std::cout <<
"Stop the robot " << std::endl;
377 catch(
const franka::NetworkException &e) {
378 std::cout <<
"Franka network exception: " << e.what() << std::endl;
379 std::cout <<
"Check if you are connected to the Franka robot"
380 <<
" or if you specified the right IP using --ip command line option set by default to 192.168.1.1. " << std::endl;
383 catch(
const std::exception &e) {
384 std::cout <<
"Franka exception: " << e.what() << std::endl;
393 #if !defined(VISP_HAVE_REALSENSE2)
394 std::cout <<
"Install librealsense-2.x" << std::endl;
396 #if !defined(VISP_HAVE_CPP11_COMPATIBILITY)
397 std::cout <<
"Build ViSP with C++11 compiler flag (cmake -DUSE_CPP11=ON)." << std::endl;
399 #if !defined(VISP_HAVE_FRANKA)
400 std::cout <<
"Install libfranka." << std::endl;