This example shows how to achieve a position-based visual servoing with a RealSense RGB-D sensor attached to a Panda robot from Franka Emika.
#include <iostream>
#include <visp3/core/vpImageConvert.h>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpXmlParserCamera.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpImageIo.h>
#include <visp3/sensor/vpRealSense2.h>
#include <visp3/robot/vpRobotFranka.h>
#include <visp3/detection/vpDetectorAprilTag.h>
#include <visp3/visual_features/vpFeatureThetaU.h>
#include <visp3/visual_features/vpFeatureTranslation.h>
#include <visp3/vs/vpServo.h>
#include <visp3/gui/vpPlot.h>
#if defined(VISP_HAVE_REALSENSE2) && defined(VISP_HAVE_CPP11_COMPATIBILITY) && \
(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(VISP_HAVE_FRANKA)
int main(int argc, char **argv)
{
double opt_tagSize = 0.120;
std::string opt_robot_ip = "192.168.1.1";
std::string opt_eMc_filename = "";
bool display_tag = true;
int opt_quad_decimate = 2;
bool opt_verbose = false;
bool opt_plot = false;
bool opt_adaptive_gain = false;
bool opt_task_sequencing = false;
double convergence_threshold_t = 0.0005, convergence_threshold_tu =
vpMath::rad(0.5);
for (int i = 1; i < argc; i++) {
if (std::string(argv[i]) == "--tag_size" && i + 1 < argc) {
opt_tagSize = std::stod(argv[i + 1]);
}
else if (std::string(argv[i]) == "--ip" && i + 1 < argc) {
opt_robot_ip = std::string(argv[i + 1]);
}
else if (std::string(argv[i]) == "--eMc" && i + 1 < argc) {
opt_eMc_filename = std::string(argv[i + 1]);
}
else if (std::string(argv[i]) == "--verbose") {
opt_verbose = true;
}
else if (std::string(argv[i]) == "--plot") {
opt_plot = true;
}
else if (std::string(argv[i]) == "--adaptive_gain") {
opt_adaptive_gain = true;
}
else if (std::string(argv[i]) == "--task_sequencing") {
opt_task_sequencing = true;
}
else if (std::string(argv[i]) == "--quad_decimate" && i + 1 < argc) {
opt_quad_decimate = std::stod(argv[i + 1]);
}
else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
std::cout << argv[0] << " [--ip <default " << opt_robot_ip << ">] [--tag_size <marker size in meter; default " << opt_tagSize << ">] [--eMc <eMc extrinsic file>] "
<< "[--quad_decimate <decimation; default " << opt_quad_decimate << ">] [--adaptive_gain] [--plot] [--task_sequencing] [--verbose] [--help] [-h]"
<< "\n";
return EXIT_SUCCESS;
}
}
try {
rs2::config config;
unsigned int width = 640, height = 480;
config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 30);
config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 30);
ePc[0] = 0.0337731; ePc[1] = -0.00535012; ePc[2] = -0.0523339;
ePc[3] = -0.247294; ePc[4] = -0.306729; ePc[5] = 1.53055;
if (!opt_eMc_filename.empty()) {
}
else {
std::cout << "Warning, opt_eMc_filename is empty! Use hard coded values." << "\n";
}
std::cout << "eMc:\n" << eMc << "\n";
std::cout << "cam:\n" << cam << "\n";
#if defined(VISP_HAVE_X11)
#elif defined(VISP_HAVE_GDI)
#endif
cdMo[0][0] = 1; cdMo[0][1] = 0; cdMo[0][2] = 0;
cdMo[1][0] = 0; cdMo[1][1] = -1; cdMo[1][2] = 0;
cdMo[2][0] = 0; cdMo[2][1] = 0; cdMo[2][2] = -1;
cdMo[0][3] = 0;
cdMo[1][3] = 0;
cdMo[2][3] = 0.3;
if (opt_adaptive_gain) {
}
else {
}
int iter_plot = 0;
if (opt_plot) {
plotter =
new vpPlot(2, 250 * 2, 500, I.
getWidth() + 80, 10,
"Real time curves plotter");
plotter->
setTitle(0,
"Visual features error");
plotter->
setTitle(1,
"Camera velocities");
plotter->
setLegend(0, 3,
"error_feat_theta_ux");
plotter->
setLegend(0, 4,
"error_feat_theta_uy");
plotter->
setLegend(0, 5,
"error_feat_theta_uz");
}
bool final_quit = false;
bool has_converged = false;
bool send_velocities = false;
bool servo_started = false;
while (!has_converged && !final_quit) {
std::vector<vpHomogeneousMatrix> cMo_vec;
detector.
detect(I, opt_tagSize, cam, cMo_vec);
std::stringstream ss;
ss << "Left click to " << (send_velocities ? "stop the robot" : "servo the robot") << ", right click to quit.";
if (cMo_vec.size() == 1) {
cMo = cMo_vec[0];
static bool first_time = true;
if (first_time) {
std::vector<vpHomogeneousMatrix> v_oMo(2), v_cdMc(2);
v_oMo[1].buildFrom(0, 0, 0, 0, 0, M_PI);
for (size_t i = 0; i < 2; i++) {
v_cdMc[i] = cdMo * v_oMo[i] * cMo.
inverse();
}
if (std::fabs(v_cdMc[0].getThetaUVector().getTheta()) < std::fabs(v_cdMc[1].getThetaUVector().getTheta())) {
oMo = v_oMo[0];
}
else {
std::cout << "Desired frame modified to avoid PI rotation of the camera" << std::endl;
oMo = v_oMo[1];
}
first_time = false;
}
if (opt_task_sequencing) {
if (! servo_started) {
if (send_velocities) {
servo_started = true;
}
}
}
else {
}
if (opt_plot) {
plotter->
plot(1, iter_plot, v_c);
iter_plot++;
}
if (opt_verbose) {
std::cout << "v_c: " << v_c.t() << std::endl;
}
ss.str("");
ss << "error_t: " << error_t;
ss.str("");
ss << "error_tu: " << error_tu;
if (opt_verbose)
std::cout << "error translation: " << error_t << " ; error rotation: " << error_tu << "\n";
if (error_t < convergence_threshold_t && error_tu < convergence_threshold_tu) {
has_converged = true;
std::cout << "Servo task has converged" << "\n";
}
}
else {
v_c = 0;
}
if (!send_velocities) {
v_c = 0;
}
ss.str("");
switch (button) {
send_velocities = !send_velocities;
break;
final_quit = true;
v_c = 0;
break;
default:
break;
}
}
}
std::cout << "Stop the robot " << std::endl;
if (opt_plot && plotter != NULL) {
delete plotter;
plotter = NULL;
}
if (!final_quit) {
while (!final_quit) {
final_quit = true;
}
}
}
}
std::cout <<
"ViSP exception: " << e.
what() << std::endl;
std::cout << "Stop the robot " << std::endl;
return EXIT_FAILURE;
}
catch(const franka::NetworkException &e) {
std::cout << "Franka network exception: " << e.what() << std::endl;
std::cout << "Check if you are connected to the Franka robot"
<< " or if you specified the right IP using --ip command line option set by default to 192.168.1.1. " << std::endl;
return EXIT_FAILURE;
}
catch(const std::exception &e) {
std::cout << "Franka exception: " << e.what() << std::endl;
return EXIT_FAILURE;
}
return 0;
}
#else
int main()
{
#if !defined(VISP_HAVE_REALSENSE2)
std::cout << "Install librealsense-2.x" << std::endl;
#endif
#if !defined(VISP_HAVE_CPP11_COMPATIBILITY)
std::cout << "Build ViSP with C++11 compiler flag (cmake -DUSE_CPP11=ON)." << std::endl;
#endif
#if !defined(VISP_HAVE_FRANKA)
std::cout << "Install libfranka." << std::endl;
#endif
return 0;
}
#endif