Example of eye-in-hand image-based control law. We control here the Afma6 robot at Inria. The velocity is computed in the camera frame. Visual features are the image coordinates of 4 points corresponding to the corners of an AprilTag.
The device used to acquire images is a Realsense D435 device.
Camera intrinsic parameters are retrieved from the Realsense SDK.
#include <iostream>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/detection/vpDetectorAprilTag.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/gui/vpPlot.h>
#include <visp3/io/vpImageIo.h>
#include <visp3/robot/vpRobotAfma6.h>
#include <visp3/sensor/vpRealSense2.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
#if defined(VISP_HAVE_REALSENSE2) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && \
(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(VISP_HAVE_AFMA6)
std::vector<vpImagePoint> *traj_vip)
{
for (size_t i = 0; i < vip.size(); i++) {
if (traj_vip[i].size()) {
traj_vip[i].push_back(vip[i]);
}
} else {
traj_vip[i].push_back(vip[i]);
}
}
for (size_t i = 0; i < vip.size(); i++) {
for (size_t j = 1; j < traj_vip[i].size(); j++) {
}
}
}
int main(int argc, char **argv)
{
double opt_tagSize = 0.120;
bool display_tag = true;
int opt_quad_decimate = 2;
bool opt_verbose = false;
bool opt_plot = false;
bool opt_adaptive_gain = false;
bool opt_task_sequencing = false;
double convergence_threshold = 0.00005;
for (int i = 1; i < argc; i++) {
if (std::string(argv[i]) == "--tag_size" && i + 1 < argc) {
opt_tagSize = std::stod(argv[i + 1]);
} else if (std::string(argv[i]) == "--verbose") {
opt_verbose = true;
} else if (std::string(argv[i]) == "--plot") {
opt_plot = true;
} else if (std::string(argv[i]) == "--adaptive_gain") {
opt_adaptive_gain = true;
} else if (std::string(argv[i]) == "--task_sequencing") {
opt_task_sequencing = true;
} else if (std::string(argv[i]) == "--quad_decimate" && i + 1 < argc) {
opt_quad_decimate = std::stoi(argv[i + 1]);
} else if (std::string(argv[i]) == "--no-convergence-threshold") {
convergence_threshold = 0.;
} else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
std::cout
<< argv[0] << " --tag_size <marker size in meter; default " << opt_tagSize << ">] "
<< "[--quad_decimate <decimation; default " << opt_quad_decimate
<< ">] [--adaptive_gain] [--plot] [--task_sequencing] [--no-convergence-threshold] [--verbose] [--help] [-h]"
<< "\n";
return EXIT_SUCCESS;
}
}
try {
std::cout << "WARNING: This example will move the robot! "
<< "Please make sure to have the user stop button at hand!" << std::endl
<< "Press Enter to continue..." << std::endl;
std::cin.ignore();
q[0] = 0.;
q[1] = 0.;
q[2] = 0.;
std::cout << "Move to joint position: " << q.t() << std::endl;
rs2::config config;
unsigned int width = 640, height = 480;
config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 30);
config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 30);
std::cout << "cam:\n" << cam << "\n";
#if defined(VISP_HAVE_X11)
#elif defined(VISP_HAVE_GDI)
#endif
detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
detector.setDisplayTag(display_tag);
detector.setAprilTagQuadDecimate(opt_quad_decimate);
std::vector<vpFeaturePoint> p(4), pd(4);
std::vector<vpPoint> point(4);
point[0].setWorldCoordinates(-opt_tagSize / 2., -opt_tagSize / 2., 0);
point[1].setWorldCoordinates(opt_tagSize / 2., -opt_tagSize / 2., 0);
point[2].setWorldCoordinates(opt_tagSize / 2., opt_tagSize / 2., 0);
point[3].setWorldCoordinates(-opt_tagSize / 2., opt_tagSize / 2., 0);
for (size_t i = 0; i < p.size(); i++) {
}
if (opt_adaptive_gain) {
} else {
}
int iter_plot = 0;
if (opt_plot) {
plotter =
new vpPlot(2,
static_cast<int>(250 * 2), 500,
static_cast<int>(I.
getWidth()) + 80, 10,
"Real time curves plotter");
plotter->
setTitle(0,
"Visual features error");
plotter->
setTitle(1,
"Camera velocities");
}
bool final_quit = false;
bool has_converged = false;
bool send_velocities = false;
bool servo_started = false;
std::vector<vpImagePoint> *traj_corners = nullptr;
while (!has_converged && !final_quit) {
std::vector<vpHomogeneousMatrix> cMo_vec;
detector.detect(I, opt_tagSize, cam, cMo_vec);
{
std::stringstream ss;
ss << "Left click to " << (send_velocities ? "stop the robot" : "servo the robot") << ", right click to quit.";
}
if (cMo_vec.size() == 1) {
cMo = cMo_vec[0];
static bool first_time = true;
if (first_time) {
std::vector<vpHomogeneousMatrix> v_oMo(2), v_cdMc(2);
v_oMo[1].buildFrom(0, 0, 0, 0, 0, M_PI);
for (size_t i = 0; i < 2; i++) {
v_cdMc[i] = cdMo * v_oMo[i] * cMo.
inverse();
}
if (std::fabs(v_cdMc[0].getThetaUVector().getTheta()) < std::fabs(v_cdMc[1].getThetaUVector().getTheta())) {
oMo = v_oMo[0];
} else {
std::cout << "Desired frame modified to avoid PI rotation of the camera" << std::endl;
oMo = v_oMo[1];
}
for (size_t i = 0; i < point.size(); i++) {
point[i].changeFrame(cdMo * oMo, cP);
point[i].projection(cP, p_);
pd[i].set_x(p_[0]);
pd[i].set_y(p_[1]);
pd[i].set_Z(cP[2]);
}
}
std::vector<vpImagePoint> corners = detector.getPolygon(0);
for (size_t i = 0; i < corners.size(); i++) {
point[i].changeFrame(cMo, cP);
p[i].set_Z(cP[2]);
}
if (opt_task_sequencing) {
if (!servo_started) {
if (send_velocities) {
servo_started = true;
}
}
} else {
}
for (size_t i = 0; i < corners.size(); i++) {
std::stringstream ss;
ss << i;
}
if (first_time) {
traj_corners = new std::vector<vpImagePoint>[corners.size()];
}
display_point_trajectory(I, corners, traj_corners);
if (opt_plot) {
plotter->
plot(1, iter_plot, v_c);
iter_plot++;
}
if (opt_verbose) {
std::cout << "v_c: " << v_c.t() << std::endl;
}
std::stringstream ss;
ss << "error: " << error;
if (opt_verbose)
std::cout << "error: " << error << std::endl;
if (error < convergence_threshold) {
has_converged = true;
std::cout << "Servo task has converged"
<< "\n";
}
if (first_time) {
first_time = false;
}
}
else {
v_c = 0;
}
if (!send_velocities) {
v_c = 0;
}
{
std::stringstream ss;
}
switch (button) {
send_velocities = !send_velocities;
break;
final_quit = true;
v_c = 0;
break;
default:
break;
}
}
}
std::cout << "Stop the robot " << std::endl;
if (opt_plot && plotter != nullptr) {
delete plotter;
plotter = nullptr;
}
if (!final_quit) {
while (!final_quit) {
final_quit = true;
}
}
}
if (traj_corners) {
delete[] traj_corners;
}
std::cout <<
"ViSP exception: " << e.
what() << std::endl;
std::cout << "Stop the robot " << std::endl;
return EXIT_FAILURE;
} catch (const std::exception &e) {
std::cout << "ur_rtde exception: " << e.what() << std::endl;
return EXIT_FAILURE;
}
return EXIT_SUCCESS;
}
#else
int main()
{
#if !defined(VISP_HAVE_REALSENSE2)
std::cout << "Install librealsense-2.x" << std::endl;
#endif
#if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
std::cout << "Build ViSP with c++11 or higher compiler flag (cmake -DUSE_CXX_STANDARD=11)." << std::endl;
#endif
#if !defined(VISP_HAVE_AFMA6)
std::cout << "ViSP is not build with Afma6 robot support..." << std::endl;
#endif
return EXIT_SUCCESS;
}
#endif
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 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 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 emited by ViSP classes.
const char * what() const
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
unsigned int getWidth() const
static double rad(double deg)
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
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.
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 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)
Class that consider the case of a translation vector.
VISP_EXPORT double measureTimeMs()