58#include <visp3/core/vpCameraParameters.h>
59#include <visp3/detection/vpDetectorAprilTag.h>
60#include <visp3/gui/vpDisplayGDI.h>
61#include <visp3/gui/vpDisplayX.h>
62#include <visp3/gui/vpPlot.h>
63#include <visp3/io/vpImageIo.h>
64#include <visp3/robot/vpRobotUniversalRobots.h>
65#include <visp3/sensor/vpRealSense2.h>
66#include <visp3/visual_features/vpFeatureThetaU.h>
67#include <visp3/visual_features/vpFeatureTranslation.h>
68#include <visp3/vs/vpServo.h>
69#include <visp3/vs/vpServoDisplay.h>
71#if defined(VISP_HAVE_REALSENSE2) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && \
72 (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(VISP_HAVE_UR_RTDE)
75 std::vector<vpImagePoint> *traj_vip)
77 for (
size_t i = 0; i < vip.size(); i++) {
78 if (traj_vip[i].size()) {
81 traj_vip[i].push_back(vip[i]);
84 traj_vip[i].push_back(vip[i]);
87 for (
size_t i = 0; i < vip.size(); i++) {
88 for (
size_t j = 1; j < traj_vip[i].size(); j++) {
94int main(
int argc,
char **argv)
96 double opt_tagSize = 0.120;
97 std::string opt_robot_ip =
"192.168.0.100";
98 std::string opt_eMc_filename =
"";
99 bool display_tag =
true;
100 int opt_quad_decimate = 2;
101 bool opt_verbose =
false;
102 bool opt_plot =
false;
103 bool opt_adaptive_gain =
false;
104 bool opt_task_sequencing =
false;
105 double convergence_threshold_t = 0.0005;
106 double convergence_threshold_tu = 0.5;
108 for (
int i = 1; i < argc; i++) {
109 if (std::string(argv[i]) ==
"--tag_size" && i + 1 < argc) {
110 opt_tagSize = std::stod(argv[i + 1]);
111 }
else if (std::string(argv[i]) ==
"--ip" && i + 1 < argc) {
112 opt_robot_ip = std::string(argv[i + 1]);
113 }
else if (std::string(argv[i]) ==
"--eMc" && i + 1 < argc) {
114 opt_eMc_filename = std::string(argv[i + 1]);
115 }
else if (std::string(argv[i]) ==
"--verbose") {
117 }
else if (std::string(argv[i]) ==
"--plot") {
119 }
else if (std::string(argv[i]) ==
"--adaptive_gain") {
120 opt_adaptive_gain =
true;
121 }
else if (std::string(argv[i]) ==
"--task_sequencing") {
122 opt_task_sequencing =
true;
123 }
else if (std::string(argv[i]) ==
"--quad_decimate" && i + 1 < argc) {
124 opt_quad_decimate = std::stoi(argv[i + 1]);
125 }
else if (std::string(argv[i]) ==
"--no-convergence-threshold") {
126 convergence_threshold_t = 0.;
127 convergence_threshold_tu = 0.;
128 }
else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
130 << argv[0] <<
" [--ip <default " << opt_robot_ip <<
">] [--tag_size <marker size in meter; default "
131 << opt_tagSize <<
">] [--eMc <eMc extrinsic file>] "
132 <<
"[--quad_decimate <decimation; default " << opt_quad_decimate
133 <<
">] [--adaptive_gain] [--plot] [--task_sequencing] [--no-convergence-threshold] [--verbose] [--help] [-h]"
142 robot.connect(opt_robot_ip);
144 std::cout <<
"WARNING: This example will move the robot! "
145 <<
"Please make sure to have the user stop button at hand!" << std::endl
146 <<
"Press Enter to continue..." << std::endl;
159 std::cout <<
"Move to joint position: " << q.t() << std::endl;
165 unsigned int width = 640, height = 480;
166 config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 30);
167 config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
168 config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 30);
177 ePc[3] = -0.00506562;
178 ePc[4] = -0.00293325;
182 if (!opt_eMc_filename.empty()) {
183 ePc.
loadYAML(opt_eMc_filename, ePc);
185 std::cout <<
"Warning, opt_eMc_filename is empty! Use hard coded values."
189 std::cout <<
"eMc:\n" << eMc <<
"\n";
194 std::cout <<
"cam:\n" << cam <<
"\n";
198#if defined(VISP_HAVE_X11)
200#elif defined(VISP_HAVE_GDI)
208 detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
209 detector.setDisplayTag(display_tag);
210 detector.setAprilTagQuadDecimate(opt_quad_decimate);
234 if (opt_adaptive_gain) {
241 vpPlot *plotter =
nullptr;
245 plotter =
new vpPlot(2,
static_cast<int>(250 * 2), 500,
static_cast<int>(I.
getWidth()) + 80, 10,
246 "Real time curves plotter");
247 plotter->
setTitle(0,
"Visual features error");
248 plotter->
setTitle(1,
"Camera velocities");
251 plotter->
setLegend(0, 0,
"error_feat_tx");
252 plotter->
setLegend(0, 1,
"error_feat_ty");
253 plotter->
setLegend(0, 2,
"error_feat_tz");
254 plotter->
setLegend(0, 3,
"error_feat_theta_ux");
255 plotter->
setLegend(0, 4,
"error_feat_theta_uy");
256 plotter->
setLegend(0, 5,
"error_feat_theta_uz");
265 bool final_quit =
false;
266 bool has_converged =
false;
267 bool send_velocities =
false;
268 bool servo_started =
false;
269 std::vector<vpImagePoint> *traj_vip =
nullptr;
276 while (!has_converged && !final_quit) {
283 std::vector<vpHomogeneousMatrix> cMo_vec;
284 detector.detect(I, opt_tagSize, cam, cMo_vec);
286 std::stringstream ss;
287 ss <<
"Left click to " << (send_velocities ?
"stop the robot" :
"servo the robot") <<
", right click to quit.";
293 if (cMo_vec.size() == 1) {
296 static bool first_time =
true;
299 std::vector<vpHomogeneousMatrix> v_oMo(2), v_cdMc(2);
300 v_oMo[1].buildFrom(0, 0, 0, 0, 0, M_PI);
301 for (
size_t i = 0; i < 2; i++) {
302 v_cdMc[i] = cdMo * v_oMo[i] * cMo.
inverse();
304 if (std::fabs(v_cdMc[0].getThetaUVector().getTheta()) < std::fabs(v_cdMc[1].getThetaUVector().getTheta())) {
307 std::cout <<
"Desired frame modified to avoid PI rotation of the camera" << std::endl;
313 cdMc = cdMo * oMo * cMo.
inverse();
317 if (opt_task_sequencing) {
318 if (!servo_started) {
319 if (send_velocities) {
320 servo_started =
true;
333 std::vector<vpImagePoint> vip = detector.getPolygon(0);
335 vip.push_back(detector.getCog(0));
338 traj_vip =
new std::vector<vpImagePoint>[vip.size()];
340 display_point_trajectory(I, vip, traj_vip);
344 plotter->
plot(1, iter_plot, v_c);
349 std::cout <<
"v_c: " << v_c.t() << std::endl;
354 double error_tr = sqrt(cd_t_c.
sumSquare());
358 ss <<
"error_t: " << error_tr;
361 ss <<
"error_tu: " << error_tu;
365 std::cout <<
"error translation: " << error_tr <<
" ; error rotation: " << error_tu << std::endl;
367 if (error_tr < convergence_threshold_t && error_tu < convergence_threshold_tu) {
368 has_converged =
true;
369 std::cout <<
"Servo task has converged" << std::endl;
382 if (!send_velocities) {
398 send_velocities = !send_velocities;
411 std::cout <<
"Stop the robot " << std::endl;
414 if (opt_plot && plotter !=
nullptr) {
420 while (!final_quit) {
439 std::cout <<
"ViSP exception: " << e.
what() << std::endl;
440 std::cout <<
"Stop the robot " << std::endl;
443 }
catch (
const std::exception &e) {
444 std::cout <<
"ur_rtde exception: " << e.what() << std::endl;
453#if !defined(VISP_HAVE_REALSENSE2)
454 std::cout <<
"Install librealsense-2.x" << std::endl;
456#if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
457 std::cout <<
"Build ViSP with c++11 or higher compiler flag (cmake -DUSE_CXX_STANDARD=11)." << std::endl;
459#if !defined(VISP_HAVE_UR_RTDE)
460 std::cout <<
"ViSP is not build with libur_rtde 3rd party used to control a robot from Universal Robots..."
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 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)
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_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()