64 #include <visp3/core/vpCameraParameters.h> 65 #include <visp3/detection/vpDetectorAprilTag.h> 66 #include <visp3/gui/vpDisplayGDI.h> 67 #include <visp3/gui/vpDisplayX.h> 68 #include <visp3/gui/vpPlot.h> 69 #include <visp3/io/vpImageIo.h> 70 #include <visp3/robot/vpRobotUniversalRobots.h> 71 #include <visp3/sensor/vpRealSense2.h> 72 #include <visp3/visual_features/vpFeatureBuilder.h> 73 #include <visp3/visual_features/vpFeaturePoint.h> 74 #include <visp3/vs/vpServo.h> 75 #include <visp3/vs/vpServoDisplay.h> 77 #if defined(VISP_HAVE_REALSENSE2) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && \ 78 (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(VISP_HAVE_UR_RTDE) 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]);
90 traj_vip[i].push_back(vip[i]);
93 for (
size_t i = 0; i < vip.size(); i++) {
94 for (
size_t j = 1; j < traj_vip[i].size(); j++) {
100 int main(
int argc,
char **argv)
102 double opt_tagSize = 0.120;
103 std::string opt_robot_ip =
"192.168.0.100";
104 std::string opt_eMc_filename =
"";
105 bool display_tag =
true;
106 int opt_quad_decimate = 2;
107 bool opt_verbose =
false;
108 bool opt_plot =
false;
109 bool opt_adaptive_gain =
false;
110 bool opt_task_sequencing =
false;
111 double convergence_threshold = 0.00005;
113 for (
int i = 1; i < argc; i++) {
114 if (std::string(argv[i]) ==
"--tag_size" && i + 1 < argc) {
115 opt_tagSize = std::stod(argv[i + 1]);
116 }
else if (std::string(argv[i]) ==
"--ip" && i + 1 < argc) {
117 opt_robot_ip = std::string(argv[i + 1]);
118 }
else if (std::string(argv[i]) ==
"--eMc" && i + 1 < argc) {
119 opt_eMc_filename = std::string(argv[i + 1]);
120 }
else if (std::string(argv[i]) ==
"--verbose") {
122 }
else if (std::string(argv[i]) ==
"--plot") {
124 }
else if (std::string(argv[i]) ==
"--adaptive_gain") {
125 opt_adaptive_gain =
true;
126 }
else if (std::string(argv[i]) ==
"--task_sequencing") {
127 opt_task_sequencing =
true;
128 }
else if (std::string(argv[i]) ==
"--quad_decimate" && i + 1 < argc) {
129 opt_quad_decimate = std::stoi(argv[i + 1]);
130 }
else if (std::string(argv[i]) ==
"--no-convergence-threshold") {
131 convergence_threshold = 0.;
132 }
else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
134 << argv[0] <<
" [--ip <default " << opt_robot_ip <<
">] [--tag_size <marker size in meter; default " 135 << opt_tagSize <<
">] [--eMc <eMc extrinsic file>] " 136 <<
"[--quad_decimate <decimation; default " << opt_quad_decimate
137 <<
">] [--adaptive_gain] [--plot] [--task_sequencing] [--no-convergence-threshold] [--verbose] [--help] [-h]" 148 std::cout <<
"WARNING: This example will move the robot! " 149 <<
"Please make sure to have the user stop button at hand!" << std::endl
150 <<
"Press Enter to continue..." << std::endl;
163 std::cout <<
"Move to joint position: " << q.t() << std::endl;
169 unsigned int width = 640, height = 480;
170 config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 30);
171 config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
172 config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 30);
181 ePc[3] = -0.00506562;
182 ePc[4] = -0.00293325;
186 if (!opt_eMc_filename.empty()) {
187 ePc.
loadYAML(opt_eMc_filename, ePc);
189 std::cout <<
"Warning, opt_eMc_filename is empty! Use hard coded values." 193 std::cout <<
"eMc:\n" << eMc <<
"\n";
198 std::cout <<
"cam:\n" << cam <<
"\n";
202 #if defined(VISP_HAVE_X11) 204 #elif defined(VISP_HAVE_GDI) 224 std::vector<vpFeaturePoint> p(4), pd(4);
227 std::vector<vpPoint> point(4);
228 point[0].setWorldCoordinates(-opt_tagSize / 2., -opt_tagSize / 2., 0);
229 point[1].setWorldCoordinates(opt_tagSize / 2., -opt_tagSize / 2., 0);
230 point[2].setWorldCoordinates(opt_tagSize / 2., opt_tagSize / 2., 0);
231 point[3].setWorldCoordinates(-opt_tagSize / 2., opt_tagSize / 2., 0);
235 for (
size_t i = 0; i < p.size(); i++) {
241 if (opt_adaptive_gain) {
248 vpPlot *plotter =
nullptr;
252 plotter =
new vpPlot(2, static_cast<int>(250 * 2), 500, static_cast<int>(I.
getWidth()) + 80, 10,
253 "Real time curves plotter");
254 plotter->
setTitle(0,
"Visual features error");
255 plotter->
setTitle(1,
"Camera velocities");
258 plotter->
setLegend(0, 0,
"error_feat_p1_x");
259 plotter->
setLegend(0, 1,
"error_feat_p1_y");
260 plotter->
setLegend(0, 2,
"error_feat_p2_x");
261 plotter->
setLegend(0, 3,
"error_feat_p2_y");
262 plotter->
setLegend(0, 4,
"error_feat_p3_x");
263 plotter->
setLegend(0, 5,
"error_feat_p3_y");
264 plotter->
setLegend(0, 6,
"error_feat_p4_x");
265 plotter->
setLegend(0, 7,
"error_feat_p4_y");
274 bool final_quit =
false;
275 bool has_converged =
false;
276 bool send_velocities =
false;
277 bool servo_started =
false;
278 std::vector<vpImagePoint> *traj_corners =
nullptr;
285 while (!has_converged && !final_quit) {
292 std::vector<vpHomogeneousMatrix> cMo_vec;
293 detector.
detect(I, opt_tagSize, cam, cMo_vec);
296 std::stringstream ss;
297 ss <<
"Left click to " << (send_velocities ?
"stop the robot" :
"servo the robot") <<
", right click to quit.";
304 if (cMo_vec.size() == 1) {
307 static bool first_time =
true;
310 std::vector<vpHomogeneousMatrix> v_oMo(2), v_cdMc(2);
311 v_oMo[1].buildFrom(0, 0, 0, 0, 0, M_PI);
312 for (
size_t i = 0; i < 2; i++) {
313 v_cdMc[i] = cdMo * v_oMo[i] * cMo.
inverse();
315 if (std::fabs(v_cdMc[0].getThetaUVector().getTheta()) < std::fabs(v_cdMc[1].getThetaUVector().getTheta())) {
318 std::cout <<
"Desired frame modified to avoid PI rotation of the camera" << std::endl;
323 for (
size_t i = 0; i < point.size(); i++) {
325 point[i].changeFrame(cdMo * oMo, cP);
326 point[i].projection(cP, p_);
335 std::vector<vpImagePoint> corners = detector.
getPolygon(0);
338 for (
size_t i = 0; i < corners.size(); i++) {
343 point[i].changeFrame(cMo, cP);
348 if (opt_task_sequencing) {
349 if (!servo_started) {
350 if (send_velocities) {
351 servo_started =
true;
362 for (
size_t i = 0; i < corners.size(); i++) {
363 std::stringstream ss;
373 traj_corners =
new std::vector<vpImagePoint>[corners.size()];
376 display_point_trajectory(I, corners, traj_corners);
380 plotter->
plot(1, iter_plot, v_c);
385 std::cout <<
"v_c: " << v_c.t() << std::endl;
389 std::stringstream ss;
390 ss <<
"error: " << error;
394 std::cout <<
"error: " << error << std::endl;
396 if (error < convergence_threshold) {
397 has_converged =
true;
398 std::cout <<
"Servo task has converged" 410 if (!send_velocities) {
418 std::stringstream ss;
428 send_velocities = !send_velocities;
441 std::cout <<
"Stop the robot " << std::endl;
444 if (opt_plot && plotter !=
nullptr) {
450 while (!final_quit) {
465 delete[] traj_corners;
468 std::cout <<
"ViSP exception: " << e.
what() << std::endl;
469 std::cout <<
"Stop the robot " << std::endl;
472 }
catch (
const std::exception &e) {
473 std::cout <<
"ur_rtde exception: " << e.what() << std::endl;
482 #if !defined(VISP_HAVE_REALSENSE2) 483 std::cout <<
"Install librealsense-2.x" << std::endl;
485 #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11) 486 std::cout <<
"Build ViSP with c++11 or higher compiler flag (cmake -DUSE_CXX_STANDARD=11)." << std::endl;
488 #if !defined(VISP_HAVE_UR_RTDE) 489 std::cout <<
"ViSP is not build with libur_rtde 3rd party used to control a robot from Universal Robots..."
Adaptive gain computation.
void connect(const std::string &ur_address)
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
void setAprilTagPoseEstimationMethod(const vpPoseEstimationMethod &poseEstimationMethod)
static bool loadYAML(const std::string &filename, vpArray2D< Type > &A, char *header=NULL)
Implementation of an homogeneous matrix and operations on such kind of matrices.
AprilTag 36h11 pattern (recommended)
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Display for windows using GDI (available on any windows 32 platform).
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Initialize the position controller.
error that can be emited by ViSP classes.
vpHomogeneousMatrix inverse() const
static const vpColor green
void setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
static void flush(const vpImage< unsigned char > &I)
bool open(const rs2::config &cfg=rs2::config())
VISP_EXPORT double measureTimeMs()
Implementation of a rotation matrix and operations on such kind of matrices.
void setAprilTagQuadDecimate(float quadDecimate)
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
Initialize the velocity controller.
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
vpColVector computeControlLaw()
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
static void display(const vpImage< unsigned char > &I)
Generic class defining intrinsic camera parameters.
void setTitle(unsigned int graphNum, const std::string &title)
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
const char * what() const
static double rad(double deg)
Stops robot motion especially in velocity and acceleration control.
void initGraph(unsigned int graphNum, unsigned int curveNbr)
Implementation of column vector and the associated operations.
void setDisplayTag(bool display, const vpColor &color=vpColor::none, unsigned int thickness=2)
void set_eMc(const vpHomogeneousMatrix &eMc)
Implementation of a pose vector and operations on poses.
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion, int index=-1) const
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
vpColVector getError() const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
unsigned int getWidth() const
void setServo(const vpServoType &servo_type)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
void acquire(vpImage< unsigned char > &grey, double *ts=NULL)
Class that consider the case of a translation vector.
static void display(const vpServo &s, const vpCameraParameters &cam, const vpImage< unsigned char > &I, vpColor currentColor=vpColor::green, vpColor desiredColor=vpColor::red, unsigned int thickness=1)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
std::vector< std::vector< vpImagePoint > > & getPolygon()
bool detect(const vpImage< unsigned char > &I)