61#include <visp3/core/vpCameraParameters.h>
62#include <visp3/detection/vpDetectorAprilTag.h>
63#include <visp3/gui/vpDisplayGDI.h>
64#include <visp3/gui/vpDisplayX.h>
65#include <visp3/gui/vpPlot.h>
66#include <visp3/io/vpImageIo.h>
67#include <visp3/robot/vpRobotFranka.h>
68#include <visp3/sensor/vpRealSense2.h>
69#include <visp3/visual_features/vpFeatureThetaU.h>
70#include <visp3/visual_features/vpFeatureTranslation.h>
71#include <visp3/vs/vpServo.h>
72#include <visp3/vs/vpServoDisplay.h>
74#if defined(VISP_HAVE_REALSENSE2) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && \
75 (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(VISP_HAVE_FRANKA)
78 std::vector<vpImagePoint> *traj_vip)
80 for (
size_t i = 0; i < vip.size(); i++) {
81 if (traj_vip[i].size()) {
84 traj_vip[i].push_back(vip[i]);
87 traj_vip[i].push_back(vip[i]);
90 for (
size_t i = 0; i < vip.size(); i++) {
91 for (
size_t j = 1; j < traj_vip[i].size(); j++) {
97int main(
int argc,
char **argv)
99 double opt_tagSize = 0.120;
100 std::string opt_robot_ip =
"192.168.1.1";
101 std::string opt_eMc_filename =
"";
102 bool display_tag =
true;
103 int opt_quad_decimate = 2;
104 bool opt_verbose =
false;
105 bool opt_plot =
false;
106 bool opt_adaptive_gain =
false;
107 bool opt_task_sequencing =
false;
108 double convergence_threshold_t = 0.0005;
109 double convergence_threshold_tu = 0.5;
111 for (
int i = 1; i < argc; i++) {
112 if (std::string(argv[i]) ==
"--tag_size" && i + 1 < argc) {
113 opt_tagSize = std::stod(argv[i + 1]);
114 }
else if (std::string(argv[i]) ==
"--ip" && i + 1 < argc) {
115 opt_robot_ip = std::string(argv[i + 1]);
116 }
else if (std::string(argv[i]) ==
"--eMc" && i + 1 < argc) {
117 opt_eMc_filename = std::string(argv[i + 1]);
118 }
else if (std::string(argv[i]) ==
"--verbose") {
120 }
else if (std::string(argv[i]) ==
"--plot") {
122 }
else if (std::string(argv[i]) ==
"--adaptive_gain") {
123 opt_adaptive_gain =
true;
124 }
else if (std::string(argv[i]) ==
"--task_sequencing") {
125 opt_task_sequencing =
true;
126 }
else if (std::string(argv[i]) ==
"--quad_decimate" && i + 1 < argc) {
127 opt_quad_decimate = std::stoi(argv[i + 1]);
128 }
else if (std::string(argv[i]) ==
"--no-convergence-threshold") {
129 convergence_threshold_t = 0.;
130 convergence_threshold_tu = 0.;
131 }
else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
133 << argv[0] <<
" [--ip <default " << opt_robot_ip <<
">] [--tag_size <marker size in meter; default "
134 << opt_tagSize <<
">] [--eMc <eMc extrinsic file>] "
135 <<
"[--quad_decimate <decimation; default " << opt_quad_decimate
136 <<
">] [--adaptive_gain] [--plot] [--task_sequencing] [--no-convergence-threshold] [--verbose] [--help] [-h]"
145 robot.connect(opt_robot_ip);
149 unsigned int width = 640, height = 480;
150 config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 30);
151 config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
152 config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 30);
159 ePc[1] = -0.00535012;
166 if (!opt_eMc_filename.empty()) {
167 ePc.
loadYAML(opt_eMc_filename, ePc);
169 std::cout <<
"Warning, opt_eMc_filename is empty! Use hard coded values."
173 std::cout <<
"eMc:\n" << eMc <<
"\n";
178 std::cout <<
"cam:\n" << cam <<
"\n";
182#if defined(VISP_HAVE_X11)
184#elif defined(VISP_HAVE_GDI)
192 detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
193 detector.setDisplayTag(display_tag);
194 detector.setAprilTagQuadDecimate(opt_quad_decimate);
218 if (opt_adaptive_gain) {
225 vpPlot *plotter =
nullptr;
229 plotter =
new vpPlot(2,
static_cast<int>(250 * 2), 500,
static_cast<int>(I.
getWidth()) + 80, 10,
230 "Real time curves plotter");
231 plotter->
setTitle(0,
"Visual features error");
232 plotter->
setTitle(1,
"Camera velocities");
235 plotter->
setLegend(0, 0,
"error_feat_tx");
236 plotter->
setLegend(0, 1,
"error_feat_ty");
237 plotter->
setLegend(0, 2,
"error_feat_tz");
238 plotter->
setLegend(0, 3,
"error_feat_theta_ux");
239 plotter->
setLegend(0, 4,
"error_feat_theta_uy");
240 plotter->
setLegend(0, 5,
"error_feat_theta_uz");
249 bool final_quit =
false;
250 bool has_converged =
false;
251 bool send_velocities =
false;
252 bool servo_started =
false;
253 std::vector<vpImagePoint> *traj_vip =
nullptr;
260 while (!has_converged && !final_quit) {
267 std::vector<vpHomogeneousMatrix> cMo_vec;
268 detector.detect(I, opt_tagSize, cam, cMo_vec);
270 std::stringstream ss;
271 ss <<
"Left click to " << (send_velocities ?
"stop the robot" :
"servo the robot") <<
", right click to quit.";
277 if (cMo_vec.size() == 1) {
280 static bool first_time =
true;
283 std::vector<vpHomogeneousMatrix> v_oMo(2), v_cdMc(2);
284 v_oMo[1].buildFrom(0, 0, 0, 0, 0, M_PI);
285 for (
size_t i = 0; i < 2; i++) {
286 v_cdMc[i] = cdMo * v_oMo[i] * cMo.
inverse();
288 if (std::fabs(v_cdMc[0].getThetaUVector().getTheta()) < std::fabs(v_cdMc[1].getThetaUVector().getTheta())) {
291 std::cout <<
"Desired frame modified to avoid PI rotation of the camera" << std::endl;
297 cdMc = cdMo * oMo * cMo.
inverse();
301 if (opt_task_sequencing) {
302 if (!servo_started) {
303 if (send_velocities) {
304 servo_started =
true;
317 std::vector<vpImagePoint> vip = detector.getPolygon(0);
319 vip.push_back(detector.getCog(0));
322 traj_vip =
new std::vector<vpImagePoint>[vip.size()];
324 display_point_trajectory(I, vip, traj_vip);
328 plotter->
plot(1, iter_plot, v_c);
333 std::cout <<
"v_c: " << v_c.t() << std::endl;
338 double error_tr = sqrt(cd_t_c.
sumSquare());
342 ss <<
"error_t: " << error_tr;
345 ss <<
"error_tu: " << error_tu;
349 std::cout <<
"error translation: " << error_tr <<
" ; error rotation: " << error_tu << std::endl;
351 if (error_tr < convergence_threshold_t && error_tu < convergence_threshold_tu) {
352 has_converged =
true;
353 std::cout <<
"Servo task has converged" << std::endl;
366 if (!send_velocities) {
382 send_velocities = !send_velocities;
395 std::cout <<
"Stop the robot " << std::endl;
398 if (opt_plot && plotter !=
nullptr) {
404 while (!final_quit) {
423 std::cout <<
"ViSP exception: " << e.
what() << std::endl;
424 std::cout <<
"Stop the robot " << std::endl;
427 }
catch (
const franka::NetworkException &e) {
428 std::cout <<
"Franka network exception: " << e.what() << std::endl;
429 std::cout <<
"Check if you are connected to the Franka robot"
430 <<
" or if you specified the right IP using --ip command line option set by default to 192.168.1.1. "
433 }
catch (
const std::exception &e) {
434 std::cout <<
"Franka exception: " << e.what() << std::endl;
443#if !defined(VISP_HAVE_REALSENSE2)
444 std::cout <<
"Install librealsense-2.x" << std::endl;
446#if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
447 std::cout <<
"Build ViSP with c++11 or higher compiler flag (cmake -DUSE_CXX_STANDARD=11)." << std::endl;
449#if !defined(VISP_HAVE_FRANKA)
450 std::cout <<
"Install libfranka." << std::endl;
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 none
static const vpColor yellow
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 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 displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emitted by ViSP classes.
const char * what() const
Class that defines a 3D visual feature from a axis/angle parametrization that represent the rotatio...
Class that defines the translation visual feature .
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpThetaUVector getThetaUVector() const
vpHomogeneousMatrix inverse() const
vpTranslationVector getTranslationVector() const
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
Definition of the vpImage class member functions.
unsigned int getWidth() const
static double deg(double rad)
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_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.
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)
Implementation of a rotation vector as axis-angle minimal representation.
Class that consider the case of a translation vector.
VISP_EXPORT double measureTimeMs()