61 #include <visp3/core/vpCameraParameters.h>
62 #include <visp3/core/vpConfig.h>
63 #include <visp3/detection/vpDetectorAprilTag.h>
64 #include <visp3/gui/vpDisplayFactory.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) && defined(VISP_HAVE_DISPLAY) && defined(VISP_HAVE_FRANKA)
76 #ifdef ENABLE_VISP_NAMESPACE
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]);
91 traj_vip[i].push_back(vip[i]);
94 for (
size_t i = 0; i < vip.size(); i++) {
95 for (
size_t j = 1; j < traj_vip[i].size(); j++) {
101 int main(
int argc,
char **argv)
103 double opt_tagSize = 0.120;
104 std::string opt_robot_ip =
"192.168.1.1";
105 std::string opt_eMc_filename =
"";
106 bool display_tag =
true;
107 int opt_quad_decimate = 2;
108 bool opt_verbose =
false;
109 bool opt_plot =
false;
110 bool opt_adaptive_gain =
false;
111 bool opt_task_sequencing =
false;
112 double convergence_threshold_t = 0.0005;
113 double convergence_threshold_tu = 0.5;
115 for (
int i = 1; i < argc; i++) {
116 if ((std::string(argv[i]) ==
"--tag-size") && (i + 1 < argc)) {
117 opt_tagSize = std::stod(argv[i + 1]);
120 else if ((std::string(argv[i]) ==
"--ip") && (i + 1 < argc)) {
121 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]);
128 else if (std::string(argv[i]) ==
"--verbose") {
131 else if (std::string(argv[i]) ==
"--plot") {
134 else if (std::string(argv[i]) ==
"--adpative-gain") {
135 opt_adaptive_gain =
true;
137 else if (std::string(argv[i]) ==
"--task-sequencing") {
138 opt_task_sequencing =
true;
140 else if ((std::string(argv[i]) ==
"--quad-decimate") && (i + 1 < argc)) {
141 opt_quad_decimate = std::stoi(argv[i + 1]);
144 else if (std::string(argv[i]) ==
"--no-convergence-threshold") {
145 convergence_threshold_t = 0.;
146 convergence_threshold_tu = 0.;
148 else if ((std::string(argv[i]) ==
"--help") || (std::string(argv[i]) ==
"-h")) {
149 std::cout <<
"SYNOPSYS" << std::endl
151 <<
" [--ip <controller ip>]"
152 <<
" [--tag-size <size>]"
153 <<
" [--eMc <extrinsic transformation file>]"
154 <<
" [--quad-decimate <decimation factor>]"
155 <<
" [--adaptive-gain]"
157 <<
" [--task-sequencing]"
158 <<
" [--no-convergence-threshold]"
160 <<
" [--help] [-h]\n"
162 std::cout <<
"DESCRIPTION" << std::endl
163 <<
" Use a position-based visual-servoing scheme to position the camera in front of an Apriltag." << std::endl
165 <<
" --ip <controller ip>" << std::endl
166 <<
" Franka controller ip address" << std::endl
167 <<
" Default: " << opt_robot_ip << std::endl
169 <<
" --tag-size <size>" << std::endl
170 <<
" Apriltag size in [m]." << std::endl
171 <<
" Default: " << opt_tagSize <<
" [m]" << std::endl
173 <<
" --eMc <extrinsic transformation file>" << std::endl
174 <<
" File containing the homogeneous transformation matrix between" << std::endl
175 <<
" robot end-effector and camera frame." << std::endl
177 <<
" --quad-decimate <decimation factor>" << std::endl
178 <<
" Decimation factor used during Apriltag detection." << std::endl
179 <<
" Default: " << opt_quad_decimate << std::endl
181 <<
" --adaptive-gain" << std::endl
182 <<
" Flag to enable adaptive gain to speed up visual servo near convergence." << std::endl
184 <<
" --plot" << std::endl
185 <<
" Flag to enable curve plotter." << std::endl
187 <<
" --task-sequencing" << std::endl
188 <<
" Flag to enable task sequencing scheme." << std::endl
190 <<
" --no-convergence-threshold" << std::endl
191 <<
" Flag to disable convergence threshold used to stop the visual servo." << std::endl
193 <<
" --verbose" << std::endl
194 <<
" Flag to enable extra verbosity." << std::endl
196 <<
" --help, -h" << std::endl
197 <<
" Print this helper message." << std::endl
206 robot.connect(opt_robot_ip);
210 unsigned int width = 640, height = 480;
211 config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 30);
212 config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
213 config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 30);
220 ePc[1] = -0.00535012;
227 if (!opt_eMc_filename.empty()) {
228 ePc.
loadYAML(opt_eMc_filename, ePc);
231 std::cout <<
"Warning, opt_eMc_filename is empty! Use hard coded values." << std::endl;
234 std::cout <<
"eMc:\n" << eMc << std::endl;
238 std::cout <<
"cam:\n" << cam << std::endl;
242 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
252 detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
253 detector.setDisplayTag(display_tag);
254 detector.setAprilTagQuadDecimate(opt_quad_decimate);
278 if (opt_adaptive_gain) {
286 vpPlot *plotter =
nullptr;
290 plotter =
new vpPlot(2,
static_cast<int>(250 * 2), 500,
static_cast<int>(I.
getWidth()) + 80, 10,
291 "Real time curves plotter");
292 plotter->
setTitle(0,
"Visual features error");
293 plotter->
setTitle(1,
"Camera velocities");
296 plotter->
setLegend(0, 0,
"error_feat_tx");
297 plotter->
setLegend(0, 1,
"error_feat_ty");
298 plotter->
setLegend(0, 2,
"error_feat_tz");
299 plotter->
setLegend(0, 3,
"error_feat_theta_ux");
300 plotter->
setLegend(0, 4,
"error_feat_theta_uy");
301 plotter->
setLegend(0, 5,
"error_feat_theta_uz");
310 bool final_quit =
false;
311 bool has_converged =
false;
312 bool send_velocities =
false;
313 bool servo_started =
false;
314 std::vector<vpImagePoint> *traj_vip =
nullptr;
321 while (!has_converged && !final_quit) {
328 std::vector<vpHomogeneousMatrix> cMo_vec;
329 detector.detect(I, opt_tagSize, cam, cMo_vec);
331 std::stringstream ss;
332 ss <<
"Left click to " << (send_velocities ?
"stop the robot" :
"servo the robot") <<
", right click to quit.";
338 if (cMo_vec.size() == 1) {
341 static bool first_time =
true;
344 std::vector<vpHomogeneousMatrix> v_oMo(2), v_cdMc(2);
345 v_oMo[1].buildFrom(0, 0, 0, 0, 0, M_PI);
346 for (
size_t i = 0; i < 2; i++) {
347 v_cdMc[i] = cdMo * v_oMo[i] * cMo.
inverse();
349 if (std::fabs(v_cdMc[0].getThetaUVector().getTheta()) < std::fabs(v_cdMc[1].getThetaUVector().getTheta())) {
353 std::cout <<
"Desired frame modified to avoid PI rotation of the camera" << std::endl;
359 cdMc = cdMo * oMo * cMo.
inverse();
363 if (opt_task_sequencing) {
364 if (!servo_started) {
365 if (send_velocities) {
366 servo_started =
true;
380 std::vector<vpImagePoint> vip = detector.getPolygon(0);
382 vip.push_back(detector.getCog(0));
385 traj_vip =
new std::vector<vpImagePoint>[vip.size()];
387 display_point_trajectory(I, vip, traj_vip);
391 plotter->
plot(1, iter_plot, v_c);
396 std::cout <<
"v_c: " << v_c.t() << std::endl;
401 double error_tr = sqrt(cd_t_c.
sumSquare());
405 ss <<
"error_t: " << error_tr;
408 ss <<
"error_tu: " << error_tu;
412 std::cout <<
"error translation: " << error_tr <<
" ; error rotation: " << error_tu << std::endl;
414 if (error_tr < convergence_threshold_t && error_tu < convergence_threshold_tu) {
415 has_converged =
true;
416 std::cout <<
"Servo task has converged" << std::endl;
429 if (!send_velocities) {
445 send_velocities = !send_velocities;
458 std::cout <<
"Stop the robot " << std::endl;
461 if (opt_plot && plotter !=
nullptr) {
467 while (!final_quit) {
487 std::cout <<
"ViSP exception: " << e.
what() << std::endl;
488 std::cout <<
"Stop the robot " << std::endl;
490 #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
491 if (display !=
nullptr) {
497 catch (
const franka::NetworkException &e) {
498 std::cout <<
"Franka network exception: " << e.what() << std::endl;
499 std::cout <<
"Check if you are connected to the Franka robot"
500 <<
" or if you specified the right IP using --ip command line option set by default to 192.168.1.1. "
502 #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
503 if (display !=
nullptr) {
509 catch (
const std::exception &e) {
510 std::cout <<
"Franka exception: " << e.what() << std::endl;
511 #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
512 if (display !=
nullptr) {
519 #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
520 if (display !=
nullptr) {
529 #if !defined(VISP_HAVE_REALSENSE2)
530 std::cout <<
"Install librealsense-2.x" << std::endl;
532 #if !defined(VISP_HAVE_FRANKA)
533 std::cout <<
"Install libfranka." << std::endl;
Adaptive gain computation.
static bool loadYAML(const std::string &filename, vpArray2D< Type > &A, char *header=nullptr)
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)
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 & buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpHomogeneousMatrix inverse() const
vpTranslationVector getTranslationVector() const
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
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.
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion, int index=-1) const
void acquire(vpImage< unsigned char > &grey, double *ts=nullptr)
bool open(const rs2::config &cfg=rs2::config())
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) VP_OVERRIDE
@ 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 addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
void setServo(const vpServoType &servo_type)
vpColVector getError() const
vpColVector computeControlLaw()
Implementation of a rotation vector as axis-angle minimal representation.
Class that consider the case of a translation vector.
std::shared_ptr< vpDisplay > createDisplay()
Return a smart pointer vpDisplay specialization if a GUI library is available or nullptr otherwise.
vpDisplay * allocateDisplay()
Return a newly allocated vpDisplay specialization if a GUI library is available or nullptr otherwise.
VISP_EXPORT double measureTimeMs()