Visual Servoing Platform  version 3.6.1 under development (2024-03-18)
servoBiclopsPoint2DArtVelocity.cpp

Example of eye-in-hand control law. We control here a real robot, the Biclops robot (pan-tilt head provided by Traclabs). The velocity is computed in articular. The visual feature is the center of gravity of a point.

/*
* ViSP, open source Visual Servoing Platform software.
* Copyright (C) 2005 - 2023 by Inria. All rights reserved.
*
* This software is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
* For using ViSP with software that can not be combined with the GNU
* GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
* See https://visp.inria.fr for more information.
*
* This software was developed at:
* Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
*
* If you have questions regarding the use of this file, please contact
* Inria at visp@inria.fr
*
* This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
* tests the control law
* eye-in-hand control
* velocity computed in articular
*/
#include <iostream>
#include <visp3/core/vpConfig.h>
#if defined(VISP_HAVE_BICLOPS) && defined(VISP_HAVE_APRILTAG) && defined(VISP_HAVE_REALSENSE2)
#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpException.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/detection/vpDetectorAprilTag.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpParseArgv.h>
#include <visp3/robot/vpRobotBiclops.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>
// List of allowed command line options
#define GETOPTARGS "c:d:h"
void usage(const char *name, const char *badparam, std::string &conf)
{
fprintf(stdout, "\n\
Example of eye-in-hand control law. We control here a real robot, the biclops\n\
robot (pan-tilt head provided by Traclabs) equipped with a Realsense camera\n\
mounted on its end-effector. The velocity to apply to the PT head is joint\n\
velocity. The visual feature is a point corresponding to the center of\n\
gravity of an AprilTag. \n\
\n\
SYNOPSIS\n\
%s [-c <Biclops configuration file>] [-h]\n",
name);
fprintf(stdout, "\n\
OPTIONS: Default\n\
-c <Biclops configuration file> %s\n\
Sets the Biclops robot configuration file.\n",
conf.c_str());
if (badparam) {
fprintf(stderr, "ERROR: \n");
fprintf(stderr, "\nBad parameter [%s]\n", badparam);
}
}
bool getOptions(int argc, const char **argv, std::string &conf)
{
const char *optarg_;
int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
case 'c':
conf = optarg_;
break;
case 'h':
usage(argv[0], nullptr, conf);
return false;
break;
default:
usage(argv[0], optarg_, conf);
return false;
break;
}
}
if ((c == 1) || (c == -1)) {
// standalone param or error
usage(argv[0], nullptr, conf);
std::cerr << "ERROR: " << std::endl;
std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
return false;
}
return true;
}
int main(int argc, const char **argv)
{
try {
// Default unix configuration file path
std::string opt_conf = "/usr/share/BiclopsDefault.cfg";
// Read the command line options
if (getOptions(argc, argv, opt_conf) == false) {
return EXIT_FAILURE;
}
// Initialize PTU
vpRobotBiclops robot(opt_conf);
/*
* Biclops DH2 has the following axis orientation
*
* tilt + <---- (end-effector-frame)
* |
* \/ pan +
*
* The end-effector-frame from PT unit rear view is the following
*
* /\ x
* |
* (e) ----> y
*
*
*
* The camera frame attached to the PT unit is the following (rear view)
*
* (c) ----> x
* |
* \/ y
*
* The corresponding cRe (camera to end-effector rotation matrix) is then the following
*
* ( 0 1 0)
* cRe = (-1 0 0)
* ( 0 0 1)
*
* Translation cte (camera to end-effector) can be neglected
*
* (0)
* cte = (0)
* (0)
*/
robot.setDenavitHartenbergModel(vpBiclops::DH2);
cRe[0][0] = 0; cRe[0][1] = 1; cRe[0][2] = 0;
cRe[1][0] = -1; cRe[1][1] = 0; cRe[1][2] = 0;
cRe[2][0] = 0; cRe[2][1] = 0; cRe[2][2] = 1;
vpTranslationVector cte; // By default set to 0
// Robot Jacobian (expressed in the end-effector frame)
vpMatrix eJe;
// Camera to end-effector frame transformation
vpHomogeneousMatrix cMe(cte, cRe);
// Velocity twist transformation to express a velocity from end-effector to camera frame
// Initialize grabber
rs2::config config;
config.disable_stream(RS2_STREAM_DEPTH);
config.disable_stream(RS2_STREAM_INFRARED);
config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 30);
g.open(config);
std::cout << "Read camera parameters from Realsense device" << std::endl;
q = 0;
std::cout << "Move PT to initial position: " << q.t() << std::endl;
robot.setPosition(vpRobot::JOINT_STATE, q);
g.acquire(I);
// We open a window using either X11 or GTK or GDI.
// Its size is automatically defined by the image (I) size
#if defined(VISP_HAVE_X11)
vpDisplayX display(I, 100, 100, "Display X...");
#elif defined(VISP_HAVE_GTK)
vpDisplayGTK display(I, 100, 100, "Display GTK...");
#elif defined(VISP_HAVE_GDI)
vpDisplayGDI display(I, 100, 100, "Display GDI...");
#endif
vpServo task;
// Create current and desired point visual feature
// Sets the desired position of the visual feature
// Here we set Z desired to 1 meter, and (x,y)=(0,0) to center the tag in the image
pd.buildFrom(0, 0, 1);
// Define the task
// - we want an eye-in-hand control law
// - joint velocities are computed
// - interaction matrix is the one at desired position
task.set_cVe(cVe);
// We want to see a point on a point
task.addFeature(p, pd);
// Set the gain
task.setLambda(0.2);
bool quit = false;
bool send_velocities = false;
vpColVector q_dot;
while (!quit) {
g.acquire(I);
{
std::stringstream ss;
ss << "Left click to " << (send_velocities ? "stop the robot" : "servo the robot") << ", right click to quit.";
vpDisplay::displayText(I, 20, 20, ss.str(), vpColor::red);
}
if (detector.detect(I)) {
// We consider the first tag only
vpImagePoint cog = detector.getCog(0); // 0 is the id of the first tag
// Get the jacobian
robot.get_eJe(eJe);
task.set_eJe(eJe);
q_dot = task.computeControlLaw();
vpServoDisplay::display(task, cam, I);
std::cout << "q_dot: " << q_dot.t() << std::endl;
std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
}
else {
q_dot = 0;
}
if (!send_velocities) {
q_dot = 0;
}
if (vpDisplay::getClick(I, button, false)) {
switch (button) {
send_velocities = !send_velocities;
break;
quit = true;
q_dot = 0;
break;
default:
break;
}
}
}
std::cout << "Stop the robot " << std::endl;
return EXIT_SUCCESS;
}
catch (const vpException &e) {
std::cout << "Catch an exception: " << e.getMessage() << std::endl;
return EXIT_FAILURE;
}
}
#else
int main()
{
std::cout << "You do not have an Biclops PT robot connected to your computer..." << std::endl;
return EXIT_SUCCESS;
}
#endif
@ DH2
Second Denavit-Hartenberg representation.
Definition: vpBiclops.h:95
static const unsigned int ndof
Number of dof.
Definition: vpBiclops.h:99
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
vpRowVector t() const
static const vpColor red
Definition: vpColor.h:211
bool detect(const vpImage< unsigned char > &I) vp_override
vpImagePoint getCog(size_t i) const
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:128
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:128
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
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
const char * getMessage() const
Definition: vpException.cpp:64
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void buildFrom(double x, double y, double Z)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion, int index=-1) const
void acquire(vpImage< unsigned char > &grey, double *ts=nullptr)
bool open(const rs2::config &cfg=rs2::config())
Interface for the Biclops, pan, tilt head control.
void get_eJe(vpMatrix &eJe) vp_override
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) vp_override
@ JOINT_STATE
Definition: vpRobot.h:80
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:66
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:65
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:64
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:198
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)
Definition: vpServo.cpp:378
@ EYEINHAND_L_cVe_eJe
Definition: vpServo.h:162
void addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:329
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:1028
void setLambda(double c)
Definition: vpServo.h:976
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:1091
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:132
vpColVector getError() const
Definition: vpServo.h:504
@ PSEUDO_INVERSE
Definition: vpServo.h:229
vpColVector computeControlLaw()
Definition: vpServo.cpp:703
@ DESIRED
Definition: vpServo.h:202
Class that consider the case of a translation vector.
void display(vpImage< unsigned char > &I, const std::string &title)
Display a gray-scale image.