53#include <visp3/core/vpCameraParameters.h>
54#include <visp3/detection/vpDetectorAprilTag.h>
55#include <visp3/gui/vpDisplayGDI.h>
56#include <visp3/gui/vpDisplayX.h>
57#include <visp3/gui/vpPlot.h>
58#include <visp3/io/vpImageIo.h>
59#include <visp3/robot/vpRobotAfma6.h>
60#include <visp3/sensor/vpRealSense2.h>
61#include <visp3/visual_features/vpFeatureThetaU.h>
62#include <visp3/visual_features/vpFeatureTranslation.h>
63#include <visp3/vs/vpServo.h>
64#include <visp3/vs/vpServoDisplay.h>
66#if defined(VISP_HAVE_REALSENSE2) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && \
67 (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(VISP_HAVE_AFMA6)
70 std::vector<vpImagePoint> *traj_vip)
72 for (
size_t i = 0; i < vip.size(); i++) {
73 if (traj_vip[i].size()) {
76 traj_vip[i].push_back(vip[i]);
79 traj_vip[i].push_back(vip[i]);
82 for (
size_t i = 0; i < vip.size(); i++) {
83 for (
size_t j = 1; j < traj_vip[i].size(); j++) {
89int main(
int argc,
char **argv)
91 double opt_tagSize = 0.120;
92 bool display_tag =
true;
93 int opt_quad_decimate = 2;
94 bool opt_verbose =
false;
95 bool opt_plot =
false;
96 bool opt_adaptive_gain =
false;
97 bool opt_task_sequencing =
false;
98 double convergence_threshold_t = 0.0005;
99 double convergence_threshold_tu = 0.5;
101 for (
int i = 1; i < argc; i++) {
102 if (std::string(argv[i]) ==
"--tag_size" && i + 1 < argc) {
103 opt_tagSize = std::stod(argv[i + 1]);
104 }
else if (std::string(argv[i]) ==
"--verbose") {
106 }
else if (std::string(argv[i]) ==
"--plot") {
108 }
else if (std::string(argv[i]) ==
"--adaptive_gain") {
109 opt_adaptive_gain =
true;
110 }
else if (std::string(argv[i]) ==
"--task_sequencing") {
111 opt_task_sequencing =
true;
112 }
else if (std::string(argv[i]) ==
"--quad_decimate" && i + 1 < argc) {
113 opt_quad_decimate = std::stoi(argv[i + 1]);
114 }
else if (std::string(argv[i]) ==
"--no-convergence-threshold") {
115 convergence_threshold_t = 0.;
116 convergence_threshold_tu = 0.;
117 }
else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
119 << argv[0] <<
" [--tag_size <marker size in meter; default " << opt_tagSize <<
">] "
120 <<
"[--quad_decimate <decimation; default " << opt_quad_decimate
121 <<
">] [--adaptive_gain] [--plot] [--task_sequencing] [--no-convergence-threshold] [--verbose] [--help] [-h]"
130 std::cout <<
"WARNING: This example will move the robot! "
131 <<
"Please make sure to have the user stop button at hand!" << std::endl
132 <<
"Press Enter to continue..." << std::endl;
145 std::cout <<
"Move to joint position: " << q.t() << std::endl;
151 unsigned int width = 640, height = 480;
152 config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 30);
153 config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
154 config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 30);
160 std::cout <<
"cam:\n" << cam <<
"\n";
164#if defined(VISP_HAVE_X11)
166#elif defined(VISP_HAVE_GDI)
174 detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
175 detector.setDisplayTag(display_tag);
176 detector.setAprilTagQuadDecimate(opt_quad_decimate);
200 if (opt_adaptive_gain) {
207 vpPlot *plotter =
nullptr;
211 plotter =
new vpPlot(2,
static_cast<int>(250 * 2), 500,
static_cast<int>(I.
getWidth()) + 80, 10,
212 "Real time curves plotter");
213 plotter->
setTitle(0,
"Visual features error");
214 plotter->
setTitle(1,
"Camera velocities");
217 plotter->
setLegend(0, 0,
"error_feat_tx");
218 plotter->
setLegend(0, 1,
"error_feat_ty");
219 plotter->
setLegend(0, 2,
"error_feat_tz");
220 plotter->
setLegend(0, 3,
"error_feat_theta_ux");
221 plotter->
setLegend(0, 4,
"error_feat_theta_uy");
222 plotter->
setLegend(0, 5,
"error_feat_theta_uz");
231 bool final_quit =
false;
232 bool has_converged =
false;
233 bool send_velocities =
false;
234 bool servo_started =
false;
235 std::vector<vpImagePoint> *traj_vip =
nullptr;
241 while (!has_converged && !final_quit) {
248 std::vector<vpHomogeneousMatrix> cMo_vec;
249 detector.detect(I, opt_tagSize, cam, cMo_vec);
251 std::stringstream ss;
252 ss <<
"Left click to " << (send_velocities ?
"stop the robot" :
"servo the robot") <<
", right click to quit.";
258 if (cMo_vec.size() == 1) {
261 static bool first_time =
true;
264 std::vector<vpHomogeneousMatrix> v_oMo(2), v_cdMc(2);
265 v_oMo[1].buildFrom(0, 0, 0, 0, 0, M_PI);
266 for (
size_t i = 0; i < 2; i++) {
267 v_cdMc[i] = cdMo * v_oMo[i] * cMo.
inverse();
269 if (std::fabs(v_cdMc[0].getThetaUVector().getTheta()) < std::fabs(v_cdMc[1].getThetaUVector().getTheta())) {
272 std::cout <<
"Desired frame modified to avoid PI rotation of the camera" << std::endl;
278 cdMc = cdMo * oMo * cMo.
inverse();
282 if (opt_task_sequencing) {
283 if (!servo_started) {
284 if (send_velocities) {
285 servo_started =
true;
298 std::vector<vpImagePoint> vip = detector.getPolygon(0);
300 vip.push_back(detector.getCog(0));
303 traj_vip =
new std::vector<vpImagePoint>[vip.size()];
305 display_point_trajectory(I, vip, traj_vip);
309 plotter->
plot(1, iter_plot, v_c);
314 std::cout <<
"v_c: " << v_c.t() << std::endl;
319 double error_tr = sqrt(cd_t_c.
sumSquare());
323 ss <<
"error_t: " << error_tr;
326 ss <<
"error_tu: " << error_tu;
330 std::cout <<
"error translation: " << error_tr <<
" ; error rotation: " << error_tu << std::endl;
332 if (error_tr < convergence_threshold_t && error_tu < convergence_threshold_tu) {
333 has_converged =
true;
334 std::cout <<
"Servo task has converged" << std::endl;
347 if (!send_velocities) {
363 send_velocities = !send_velocities;
376 std::cout <<
"Stop the robot " << std::endl;
379 if (opt_plot && plotter !=
nullptr) {
385 while (!final_quit) {
404 std::cout <<
"ViSP exception: " << e.
what() << std::endl;
405 std::cout <<
"Stop the robot " << std::endl;
408 }
catch (
const std::exception &e) {
409 std::cout <<
"ur_rtde exception: " << e.what() << std::endl;
418#if !defined(VISP_HAVE_REALSENSE2)
419 std::cout <<
"Install librealsense-2.x" << std::endl;
421#if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
422 std::cout <<
"Build ViSP with c++11 or higher compiler flag (cmake -DUSE_CXX_STANDARD=11)." << std::endl;
424#if !defined(VISP_HAVE_AFMA6)
425 std::cout <<
"ViSP is not build with Afma-6 robot support..." << std::endl;
Adaptive gain computation.
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 rad(double deg)
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)
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())
Control of Irisa's gantry robot named Afma6.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
@ STATE_POSITION_CONTROL
Initialize the position controller.
@ 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()