Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoAfma6MegaposePBVS.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See https://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Pose-based visual servoing using MegaPose, on an Afma6 platform.
33 *
34 *****************************************************************************/
35
63#include <iostream>
64
65#include <visp3/core/vpCameraParameters.h>
66#include <visp3/detection/vpDetectorAprilTag.h>
67#include <visp3/gui/vpDisplayGDI.h>
68#include <visp3/gui/vpDisplayX.h>
69#include <visp3/gui/vpPlot.h>
70#include <visp3/io/vpImageIo.h>
71#include <visp3/robot/vpRobotAfma6.h>
72#include <visp3/sensor/vpRealSense2.h>
73#include <visp3/visual_features/vpFeatureThetaU.h>
74#include <visp3/visual_features/vpFeatureTranslation.h>
75#include <visp3/vs/vpServo.h>
76#include <visp3/vs/vpServoDisplay.h>
77#include <visp3/core/vpImageFilter.h>
78#include <visp3/io/vpVideoWriter.h>
79
80
81#if defined(VISP_HAVE_REALSENSE2) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \
82 (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(VISP_HAVE_AFMA6) && defined(VISP_HAVE_MODULE_DNN_TRACKER)
83
84#include <optional>
85#include <visp3/io/vpJsonArgumentParser.h>
86#include <visp3/dnn_tracker/vpMegaPoseTracker.h>
87
88#ifdef VISP_HAVE_NLOHMANN_JSON
89using json = nlohmann::json;
90#endif
91
92std::optional<vpRect> detectObjectForInitMegaposeClick(const vpImage<vpRGBa> &I)
93{
94 const bool startLabelling = vpDisplay::getClick(I, false);
95
96 const vpImagePoint textPosition(10.0, 20.0);
97
98 if (startLabelling) {
99 vpImagePoint topLeft, bottomRight;
100 vpDisplay::displayText(I, textPosition, "Click the upper left corner of the bounding box", vpColor::red);
102 vpDisplay::getClick(I, topLeft, true);
104 vpDisplay::displayCross(I, topLeft, 5, vpColor::red, 2);
105 vpDisplay::displayText(I, textPosition, "Click the bottom right corner of the bounding box", vpColor::red);
107 vpDisplay::getClick(I, bottomRight, true);
108 vpRect bb(topLeft, bottomRight);
109 return bb;
110 }
111 else {
113 vpDisplay::displayText(I, textPosition, "Click when the object is visible and static to start reinitializing megapose.", vpColor::red);
115 return std::nullopt;
116 }
117}
118
119int main(int argc, const char *argv[])
120{
121 bool opt_verbose = true;
122 bool opt_plot = true;
123 double convergence_threshold_t = 0.0005; // Value in [m]
124 double convergence_threshold_tu = 0.5; // Value in [deg]
125
126 unsigned width = 640, height = 480;
127 std::string megaposeAddress = "127.0.0.1";
128 unsigned megaposePort = 5555;
129 int refinerIterations = 1, coarseNumSamples = 1024;
130 std::string objectName = "";
131
132 std::string desiredPosFile = "desired.pos";
133 std::string initialPosFile = "init.pos";
134
135#ifdef VISP_HAVE_NLOHMANN_JSON
136 vpJsonArgumentParser parser("Pose-based visual servoing with Megapose on an Afma6, with a Realsense D435.", "--config", "/");
137 parser
138 .addArgument("initialPose", initialPosFile, true, "Path to the file that contains that the desired pose. Can be acquired with Afma6_office.")
139 .addArgument("desiredPose", desiredPosFile, true, "Path to the file that contains that the desired pose. Can be acquired with Afma6_office.")
140 .addArgument("object", objectName, true, "Name of the object to track with megapose.")
141 .addArgument("megapose/address", megaposeAddress, true, "IP address of the Megapose server.")
142 .addArgument("megapose/port", megaposePort, true, "Port on which the Megapose server listens for connections.")
143 .addArgument("megapose/refinerIterations", refinerIterations, false, "Number of Megapose refiner model iterations."
144 "A higher count may lead to better accuracy, at the cost of more processing time")
145 .addArgument("megapose/initialisationNumSamples", coarseNumSamples, false, "Number of Megapose renderings used for the initial pose estimation.");
146 parser.parse(argc, argv);
147#endif
148
149 vpRobotAfma6 robot;
150
151 try {
152 std::cout << "WARNING: This example will move the robot! "
153 << "Please make sure to have the user stop button at hand!" << std::endl
154 << "Press Enter to continue..." << std::endl;
155 std::cin.ignore();
156 std::vector<vpColVector> velocities;
157 std::vector<vpPoseVector> error;
158 /*
159 * Move to a safe position
160 */
161 vpColVector q(6, 0);
162
163 vpVideoWriter writer;
164
165 // Go to desired pose, save true camera pose wrt world frame
166 robot.setPositioningVelocity(10.0); // In %
167 robot.readPosFile(desiredPosFile, q);
169 robot.setPosition(vpRobot::ARTICULAR_FRAME, q); // Move to the joint position
170 std::cout << "Move to joint position: " << q.t() << std::endl;
171 vpHomogeneousMatrix cdTw = robot.get_fMc(q).inverse();
172
173 // Setup camera
174 vpRealSense2 rs;
175 rs2::config config;
176 config.enable_stream(RS2_STREAM_COLOR, width, height, RS2_FORMAT_RGBA8, 30);
177 rs.open(config);
178
179 // Get camera intrinsics
182 std::cout << "cam:\n" << cam << "\n";
183 // Initialize Megapose
184 std::shared_ptr<vpMegaPose> megapose;
185 try {
186 megapose = std::make_shared<vpMegaPose>(megaposeAddress, megaposePort, cam, height, width);
187 }
188 catch (...) {
189 throw vpException(vpException::ioError, "Could not connect to Megapose server at " + megaposeAddress + " on port " + std::to_string(megaposePort));
190 }
191
192 vpMegaPoseTracker megaposeTracker(megapose, objectName, refinerIterations);
193 megapose->setCoarseNumSamples(coarseNumSamples);
194 const std::vector<std::string> allObjects = megapose->getObjectNames();
195 if (std::find(allObjects.begin(), allObjects.end(), objectName) == allObjects.end()) {
196 throw vpException(vpException::badValue, "Object " + objectName + " is not known by the Megapose server!");
197 }
198 std::future<vpMegaPoseEstimate> trackerFuture;
199
200 vpImage<vpRGBa> I(height, width);
201
202#if defined(VISP_HAVE_X11)
203 vpDisplayX dc(I, 10, 10, "Color image");
204#elif defined(VISP_HAVE_GDI)
205 vpDisplayGDI dc(I, 10, 10, "Color image");
206#endif
207
208 std::optional<vpRect> detection;
209 while (!detection) {
210 rs.acquire(I);
212 detection = detectObjectForInitMegaposeClick(I);
214 }
215
216 vpHomogeneousMatrix cdTo = megaposeTracker.init(I, *detection).get().cTo; //get camera pose relative to object, not world
217
218 // Go to starting pose, save true starting pose in world frame
219 robot.readPosFile(initialPosFile, q);
221 robot.setPosition(vpRobot::ARTICULAR_FRAME, q); // Move to the joint position
222 std::cout << "Move to joint position: " << q.t() << std::endl;
223 vpHomogeneousMatrix cTw = robot.get_fMc(q).inverse();
224 vpHomogeneousMatrix cdTc_true = cdTw * cTw.inverse(); // ground truth error
225
226 detection = std::nullopt;
227 while (!detection) {
228 rs.acquire(I);
230 detection = detectObjectForInitMegaposeClick(I);
232 }
233 auto est = megaposeTracker.init(I, *detection).get();
234 vpHomogeneousMatrix cTo = est.cTo;
235 std::cout << "Estimate score = " << est.score << std::endl;
236 writer.setFileName("video/I%05d.png");
237 //writer.setFramerate(60.0);
238 writer.open(I);
239
240 //vpHomogeneousMatrix oTw = cTo.inverse() * cTw;
241 vpHomogeneousMatrix cdTc = cdTo * cTo.inverse();
244 t.buildFrom(cdTc);
245 tu.buildFrom(cdTc);
246
249
250 vpServo task;
251 task.addFeature(t, td);
252 task.addFeature(tu, tud);
255 task.setLambda(0.2);
256
257 vpPlot *plotter = nullptr;
258 int iter_plot = 0;
259
260 if (opt_plot) {
261 plotter = new vpPlot(2, static_cast<int>(250 * 2), 500, static_cast<int>(I.getWidth()) + 80, 10,
262 "Real time curves plotter");
263 plotter->setTitle(0, "Visual features error");
264 plotter->setTitle(1, "Camera velocities");
265 plotter->initGraph(0, 6);
266 plotter->initGraph(1, 6);
267 plotter->setLegend(0, 0, "error_feat_tx");
268 plotter->setLegend(0, 1, "error_feat_ty");
269 plotter->setLegend(0, 2, "error_feat_tz");
270 plotter->setLegend(0, 3, "error_feat_theta_ux");
271 plotter->setLegend(0, 4, "error_feat_theta_uy");
272 plotter->setLegend(0, 5, "error_feat_theta_uz");
273 plotter->setLegend(1, 0, "vc_x");
274 plotter->setLegend(1, 1, "vc_y");
275 plotter->setLegend(1, 2, "vc_z");
276 plotter->setLegend(1, 3, "wc_x");
277 plotter->setLegend(1, 4, "wc_y");
278 plotter->setLegend(1, 5, "wc_z");
279 }
280
281 bool final_quit = false;
282 bool has_converged = false;
283 bool send_velocities = false;
284
286 vpColVector vLastUpdate(6);
287
288 vpHomogeneousMatrix prev_cTo = cTo;
289
290 vpColVector v(6);
291
292 bool callMegapose = true;
293 vpMegaPoseEstimate megaposeEstimate;
294
295 while (!has_converged && !final_quit) {
296 double t_start = vpTime::measureTimeMs();
297
298 rs.acquire(I);
300 if (!callMegapose && trackerFuture.wait_for(std::chrono::milliseconds(0)) == std::future_status::ready) {
301 megaposeEstimate = trackerFuture.get();
302
303 cTo = megaposeEstimate.cTo;
304 callMegapose = true;
305 if (megaposeEstimate.score < 0.2) { // If confidence is low, exit
306 final_quit = true;
307 std::cout << "Low confidence, exiting" << std::endl;
308 }
309 }
310
311 if (callMegapose) {
312 std::cout << "Calling megapose" << std::endl;
313 trackerFuture = megaposeTracker.track(I);
314 callMegapose = false;
315 }
316
317 std::stringstream ss;
318 ss << "Left click to " << (send_velocities ? "stop the robot" : "servo the robot") << ", right click to quit.";
319 vpDisplay::displayText(I, 20, 20, ss.str(), vpColor::red);
320
321 // Update visual features
322 cdTc = cdTo * cTo.inverse();
323 t.buildFrom(cdTc);
324 tu.buildFrom(cdTc);
325 v = task.computeControlLaw();
326 velocities.push_back(v);
327
328 // Update true pose
329 vpPoseVector p;
330 robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
331 cTw = robot.get_fMc(q).inverse();
332 cdTc_true = cdTw * cTw.inverse();
333 vpPoseVector cdrc(cdTc_true);
334 error.push_back(cdrc);
335
336 // Display desired and current pose features
337 vpDisplay::displayFrame(I, cdTo, cam, 0.05, vpColor::yellow, 2);
338 vpDisplay::displayFrame(I, cTo, cam, 0.05, vpColor::none, 3);
339
340 if (opt_plot) {
341 plotter->plot(0, iter_plot, task.getError());
342 plotter->plot(1, iter_plot, v);
343 iter_plot++;
344 }
345
346 if (opt_verbose) {
347 std::cout << "v: " << v.t() << std::endl;
348 }
349
351 vpThetaUVector cd_tu_c = cdTc.getThetaUVector();
352 double error_tr = sqrt(cd_t_c.sumSquare());
353 double error_tu = vpMath::deg(sqrt(cd_tu_c.sumSquare()));
354 vpTranslationVector cd_t_c_true = cdTc_true.getTranslationVector();
355 vpThetaUVector cd_tu_c_true = cdTc_true.getThetaUVector();
356 double error_tr_true = sqrt(cd_t_c_true.sumSquare());
357 double error_tu_true = vpMath::deg(sqrt(cd_tu_c_true.sumSquare()));
358
359 ss.str("");
360 ss << "Predicted error_t: " << error_tr << ", True error_t:" << error_tr_true;
361 vpDisplay::displayText(I, 20, static_cast<int>(I.getWidth()) - 300, ss.str(), vpColor::red);
362 ss.str("");
363 ss << "Predicted error_tu: " << error_tu << ", True error_tu:" << error_tu_true;
364 vpDisplay::displayText(I, 40, static_cast<int>(I.getWidth()) - 300, ss.str(), vpColor::red);
365
366 if (opt_verbose)
367 std::cout << "error translation: " << error_tr << " ; error rotation: " << error_tu << std::endl;
368
369 if (error_tr < convergence_threshold_t && error_tu < convergence_threshold_tu) {
370 has_converged = true;
371 std::cout << "Servo task has converged" << std::endl;
372 vpDisplay::displayText(I, 100, 20, "Servo task has converged", vpColor::red);
373 }
374
375 // Send to the robot
377
378 ss.str("");
379 ss << "Loop time: " << vpTime::measureTimeMs() - t_start << " ms";
380 vpDisplay::displayText(I, 40, 20, ss.str(), vpColor::red);
382 vpImage<vpRGBa> displayImage;
383 vpDisplay::getImage(I, displayImage);
384 writer.saveFrame(displayImage);
385
387 if (vpDisplay::getClick(I, button, false)) {
388 switch (button) {
390 send_velocities = !send_velocities;
391 break;
392
394 final_quit = true;
395 v = 0;
396 break;
397
398 default:
399 break;
400 }
401 }
402 }
403 std::cout << "Stop the robot " << std::endl;
405
406#ifdef VISP_HAVE_NLOHMANN_JSON
407 // Save results to JSON
408 json j = json {
409 {"velocities", velocities},
410 {"error", error}
411 };
412 std::ofstream jsonFile;
413 jsonFile.open("results.json");
414 jsonFile << j.dump(4);
415 jsonFile.close();
416#endif
417
418 if (opt_plot && plotter != nullptr) {
419 delete plotter;
420 plotter = nullptr;
421 }
422
423 if (!final_quit) {
424 while (!final_quit) {
425 rs.acquire(I);
427
428 vpDisplay::displayText(I, 20, 20, "Click to quit the program.", vpColor::red);
429 vpDisplay::displayText(I, 40, 20, "Visual servo converged.", vpColor::red);
430
431 if (vpDisplay::getClick(I, false)) {
432 final_quit = true;
433 }
434
436 }
437 }
438 }
439 catch (const vpException &e) {
440 std::cout << "ViSP exception: " << e.what() << std::endl;
441 std::cout << "Stop the robot " << std::endl;
443 return EXIT_FAILURE;
444 }
445 catch (const std::exception &e) {
446 std::cout << "ur_rtde exception: " << e.what() << std::endl;
447 return EXIT_FAILURE;
448 }
449
450 return EXIT_SUCCESS;
451}
452#else
453int main()
454{
455#if !defined(VISP_HAVE_REALSENSE2)
456 std::cout << "Install librealsense-2.x" << std::endl;
457#endif
458#if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_17)
459 std::cout << "Build ViSP with c++11 or higher compiler flag (cmake -DUSE_CXX_STANDARD=17)." << std::endl;
460#endif
461#if !defined(VISP_HAVE_AFMA6)
462 std::cout << "ViSP is not built with Afma-6 robot support..." << std::endl;
463#endif
464 return EXIT_SUCCESS;
465}
466#endif
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
Implementation of column vector and the associated operations.
static const vpColor red
Definition vpColor.h:211
static const vpColor none
Definition vpColor.h:223
static const vpColor yellow
Definition vpColor.h:219
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...
Definition vpDisplayX.h:132
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
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 getImage(const vpImage< unsigned char > &Is, vpImage< vpRGBa > &Id)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
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.
Definition vpException.h:59
@ ioError
I/O error.
Definition vpException.h:79
@ badValue
Used to indicate that a value is not in the allowed range.
Definition vpException.h:85
const char * what() const
Class that defines a 3D visual feature from a axis/angle parametrization that represent the rotati...
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)
vp_deprecated void init()
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
Command line argument parsing with support for JSON files. If a JSON file is supplied,...
static double deg(double rad)
Definition vpMath.h:106
vpHomogeneousMatrix cTo
Definition vpMegaPose.h:69
A simplified interface to track a single object with MegaPose. This tracker works asynchronously: A c...
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Definition vpPlot.h:113
void initGraph(unsigned int graphNum, unsigned int curveNbr)
Definition vpPlot.cpp:202
void setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
Definition vpPlot.cpp:545
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
Definition vpPlot.cpp:269
void setTitle(unsigned int graphNum, const std::string &title)
Definition vpPlot.cpp:503
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())
Defines a rectangle in the plane.
Definition vpRect.h:76
Control of Irisa's gantry robot named Afma6.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
@ ARTICULAR_FRAME
Definition vpRobot.h:76
@ CAMERA_FRAME
Definition vpRobot.h:80
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition vpRobot.h:65
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition vpRobot.h:64
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition vpRobot.h:63
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition vpRobot.cpp:198
double sumSquare() const
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition vpServo.cpp:564
@ EYEINHAND_CAMERA
Definition vpServo.h:151
void setLambda(double c)
Definition vpServo.h:403
void setServo(const vpServoType &servo_type)
Definition vpServo.cpp:210
vpColVector getError() const
Definition vpServo.h:276
vpColVector computeControlLaw()
Definition vpServo.cpp:930
@ CURRENT
Definition vpServo.h:179
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition vpServo.cpp:487
Implementation of a rotation vector as axis-angle minimal representation.
Class that consider the case of a translation vector.
Class that enables to write easily a video file or a sequence of images.
void saveFrame(vpImage< vpRGBa > &I)
void setFileName(const std::string &filename)
void open(vpImage< vpRGBa > &I)
VISP_EXPORT double measureTimeMs()