Visual Servoing Platform  version 3.3.0
servoMomentPoints.cpp

Example of moment-based visual servoing with Images

/****************************************************************************
*
* ViSP, open source Visual Servoing Platform software.
* Copyright (C) 2005 - 2019 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 http://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:
* Example of visual servoing with moments using discrete points as object
* container
*
* Authors:
* Filip Novotny
*
*****************************************************************************/
#include <iostream>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpMomentCommon.h>
#include <visp3/core/vpMomentDatabase.h>
#include <visp3/core/vpMomentObject.h>
#include <visp3/core/vpPlane.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/robot/vpSimulatorAfma6.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeatureMomentCommon.h>
#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/vs/vpServo.h>
#if !defined(_WIN32) && !defined(VISP_HAVE_PTHREAD)
// Robot simulator used in this example is not available
int main()
{
std::cout << "Can't run this example since vpSimulatorAfma6 capability is "
"not available."
<< std::endl;
std::cout << "You should install pthread third-party library." << std::endl;
return EXIT_SUCCESS;
}
// No display available
#elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && \
!defined(VISP_HAVE_GTK)
int main()
{
std::cout << "Can't run this example since no display capability is available." << std::endl;
std::cout << "You should install one of the following third-party library: "
"X11, OpenCV, GDI, GTK."
<< std::endl;
return EXIT_SUCCESS;
}
#else
// setup robot parameters
void paramRobot();
// update moment objects and interface
void refreshScene(vpMomentObject &obj);
// initialize scene in the interface
void initScene();
// initialize the moment features
void initFeatures();
void execute(unsigned int nbIter); // launch the simulation
void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type);
double error();
void planeToABC(vpPlane &pl, double &A, double &B, double &C);
void paramRobot();
void removeJointLimits(vpSimulatorAfma6 &robot);
int main()
{
try { // intial pose
vpHomogeneousMatrix cMo(0.05, 0.1, 1.5, vpMath::rad(30), vpMath::rad(20), -vpMath::rad(15));
// Desired pose
// init and run the simulation
init(cMo, cdMo);
execute(1500);
return EXIT_SUCCESS;
} catch (const vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return EXIT_FAILURE;
}
}
// init the right display
#if defined VISP_HAVE_X11
vpDisplayX displayInt;
#elif defined VISP_HAVE_OPENCV
vpDisplayOpenCV displayInt;
#elif defined VISP_HAVE_GDI
vpDisplayGDI displayInt;
#elif defined VISP_HAVE_D3D9
vpDisplayD3D displayInt;
#elif defined VISP_HAVE_GTK
vpDisplayGTK displayInt;
#endif
// start and destination positioning matrices
vpSimulatorAfma6 robot(false); // robot used in this simulation
vpImage<vpRGBa> Iint(480, 640,
255); // internal image used for interface display
vpServo::vpServoIteractionMatrixType interaction_type; // current or desired
vpServo task; // servoing task
vpCameraParameters cam; // robot camera parameters
double _error; // current error
vpImageSimulator imsim; // image simulator used to simulate the
// perspective-projection camera
// moment sets and their corresponding features
vpMomentCommon *moments;
vpMomentCommon *momentsDes;
vpFeatureMomentCommon *featureMoments;
vpFeatureMomentCommon *featureMomentsDes;
// source and destination objects for moment manipulation
void initScene()
{
std::vector<vpPoint> src_pts;
std::vector<vpPoint> dst_pts;
double x[8] = {1, 3, 4, -1, -3, -2, -1, 1};
double y[8] = {0, 1, 4, 4, -2, -2, 1, 0};
int nbpoints = 8;
for (int i = 0; i < nbpoints; i++) {
vpPoint p(x[i] / 20, y[i] / 20, 0.0);
p.track(cMo);
src_pts.push_back(p);
}
src.fromVector(src_pts);
for (int i = 0; i < nbpoints; i++) {
vpPoint p(x[i] / 20, y[i] / 20, 0.0);
p.track(cdMo);
dst_pts.push_back(p);
}
dst.fromVector(dst_pts);
}
void initFeatures()
{
// A,B,C parameters of source and destination plane
double A;
double B;
double C;
double Ad;
double Bd;
double Cd;
// init main object: using moments up to order 6
// Initializing values from regular plane (with ax+by+cz=d convention)
vpPlane pl;
pl.setABCD(0, 0, 1.0, 0);
pl.changeFrame(cMo);
planeToABC(pl, A, B, C);
pl.setABCD(0, 0, 1.0, 0);
pl.changeFrame(cdMo);
planeToABC(pl, Ad, Bd, Cd);
// extracting initial position (actually we only care about Zdst)
cdMo.extract(vec);
// don't need to be specific, vpMomentCommon automatically loads
// Xg,Yg,An,Ci,Cj,Alpha moments
vpMomentCommon::getAlpha(dst), vec[2]);
vpMomentCommon::getAlpha(dst), vec[2]);
// same thing with common features
featureMoments = new vpFeatureMomentCommon(*moments);
featureMomentsDes = new vpFeatureMomentCommon(*momentsDes);
moments->updateAll(src);
momentsDes->updateAll(dst);
featureMoments->updateAll(A, B, C);
featureMomentsDes->updateAll(Ad, Bd, Cd);
// setup the interaction type
task.setInteractionMatrixType(interaction_type);
task.addFeature(featureMoments->getFeatureGravityNormalized(), featureMomentsDes->getFeatureGravityNormalized());
task.addFeature(featureMoments->getFeatureAn(), featureMomentsDes->getFeatureAn());
task.addFeature(featureMoments->getFeatureCInvariant(), featureMomentsDes->getFeatureCInvariant(),
(1 << 3) | (1 << 5));
task.addFeature(featureMoments->getFeatureAlpha(), featureMomentsDes->getFeatureAlpha());
task.setLambda(1.);
}
void refreshScene(vpMomentObject &obj)
{
// double x[8] = { 0.05,0.15, 0.2,-0.05 ,-0.15,-0.1,-0.05,0.05};
// double y[8] = { 0,0.05, 0.2, 0.2, -0.1,-0.1, 0.05,0};
double x[8] = {1, 3, 4, -1, -3, -2, -1, 1};
double y[8] = {0, 1, 4, 4, -2, -2, 1, 0};
int nbpoints = 8;
std::vector<vpPoint> cur_pts;
for (int i = 0; i < nbpoints; i++) {
vpPoint p(x[i] / 20, y[i] / 20, 0.0);
p.track(cMo);
cur_pts.push_back(p);
}
obj.fromVector(cur_pts);
}
void init(vpHomogeneousMatrix &_cMo, vpHomogeneousMatrix &_cdMo)
{
cMo = _cMo; // init source matrix
cdMo = _cdMo; // init destination matrix
interaction_type = vpServo::CURRENT; // use interaction matrix for current position
displayInt.init(Iint, 700, 0, "Visual servoing with moments");
paramRobot(); // set up robot parameters
initScene(); // initialize graphical scene (for interface)
initFeatures(); // initialize moment features
}
void execute(unsigned int nbIter)
{
// init main object: using moments up to order 6
// setting object type (disrete, continuous[form polygon])
vpTRACE("Display task information ");
task.print();
robot.getInternalView(Iint);
unsigned int iter = 0;
while (iter++ < nbIter) {
// get the cMo
cMo = robot.get_cMo();
// setup the plane in A,B,C style
vpPlane pl;
double A, B, C;
pl.setABCD(0, 0, 1.0, 0);
pl.changeFrame(cMo);
planeToABC(pl, A, B, C);
// track points, draw points and add refresh our object
refreshScene(obj);
// this is the most important thing to do: update our moments
moments->updateAll(obj);
// and update our features. Do it in that order. Features need to use the
// information computed by moments
featureMoments->updateAll(A, B, C);
robot.getInternalView(Iint);
if (iter == 1)
v = task.computeControlLaw();
// pilot robot using position control. The displacement is t*v with t=10ms
// step robot.setPosition(vpRobot::CAMERA_FRAME,0.01*v);
_error = (task.getError()).sumSquare();
}
task.kill();
vpTRACE("\n\nClick in the internal view window to end...");
delete moments;
delete momentsDes;
delete featureMoments;
delete featureMomentsDes;
}
void removeJointLimits(vpSimulatorAfma6 &robot_)
{
vpColVector limMin(6);
vpColVector limMax(6);
limMin[0] = vpMath::rad(-3600);
limMin[1] = vpMath::rad(-3600);
limMin[2] = vpMath::rad(-3600);
limMin[3] = vpMath::rad(-3600);
limMin[4] = vpMath::rad(-3600);
limMin[5] = vpMath::rad(-3600);
limMax[0] = vpMath::rad(3600);
limMax[1] = vpMath::rad(3600);
limMax[2] = vpMath::rad(3600);
limMax[3] = vpMath::rad(3600);
limMax[4] = vpMath::rad(3600);
limMax[5] = vpMath::rad(3600);
robot_.setJointLimit(limMin, limMax);
robot_.setMaxRotationVelocity(99999);
robot_.setMaxTranslationVelocity(999999);
}
void planeToABC(vpPlane &pl, double &A, double &B, double &C)
{
if (fabs(pl.getD()) < std::numeric_limits<double>::epsilon()) {
std::cout << "Invalid position:" << std::endl;
std::cout << cMo << std::endl;
std::cout << "Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
throw vpException(vpException::divideByZeroError, "invalid position!");
}
A = -pl.getA() / pl.getD();
B = -pl.getB() / pl.getD();
C = -pl.getC() / pl.getD();
}
void paramRobot()
{
/*Initialise the robot and especially the camera*/
robot.setCurrentViewColor(vpColor(150, 150, 150));
robot.setDesiredViewColor(vpColor(200, 200, 200));
removeJointLimits(robot);
robot.setConstantSamplingTimeMode(true);
/*Initialise the position of the object relative to the pose of the robot's
* camera*/
robot.initialiseObjectRelativeToCamera(cMo);
/*Set the desired position (for the displaypart)*/
robot.setDesiredCameraPosition(cdMo);
robot.getCameraParameters(cam, Iint);
}
void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type) { interaction_type = type; }
double error() { return _error; }
#endif
vpRobot::STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:65
vpDisplayX
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:149
vpException::divideByZeroError
Division by zero.
Definition: vpException.h:93
vpWireFrameSimulator::D_STANDARD
Definition: vpWireFrameSimulator.h:211
vpFeatureMomentCommon
This class allows to access common vpFeatureMoments in a pre-filled database.
Definition: vpFeatureMomentCommon.h:225
vpServo::kill
void kill()
Definition: vpServo.cpp:191
vpMomentObject::setType
void setType(vpObjectType input_type)
Definition: vpMomentObject.h:293
vpServo::CURRENT
Definition: vpServo.h:185
vpMath::rad
static double rad(double deg)
Definition: vpMath.h:107
vpCameraParameters
Generic class defining intrinsic camera parameters.
Definition: vpCameraParameters.h:232
vpCameraParameters::perspectiveProjWithoutDistortion
Definition: vpCameraParameters.h:239
vpMomentCommon::updateAll
void updateAll(vpMomentObject &object)
Definition: vpMomentCommon.cpp:125
vpFeatureMomentCommon::getFeatureAlpha
vpFeatureMomentAlpha & getFeatureAlpha()
Definition: vpFeatureMomentCommon.h:243
vpMomentCommon
This class initializes and allows access to commonly used moments.
Definition: vpMomentCommon.h:103
vpServo::setLambda
void setLambda(double c)
Definition: vpServo.h:405
vpRobot::setMaxTranslationVelocity
void setMaxTranslationVelocity(double maxVt)
Definition: vpRobot.cpp:238
vpServo::EYEINHAND_CAMERA
Definition: vpServo.h:158
vpDisplayGDI
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:127
vpWireFrameSimulator::POINT_CLOUD
Definition: vpWireFrameSimulator.h:195
vpTranslationVector
Class that consider the case of a translation vector.
Definition: vpTranslationVector.h:118
vpImageSimulator
Class which enables to project an image in the 3D space and get the view of a virtual camera.
Definition: vpImageSimulator.h:141
vpSimulatorAfma6::setJointLimit
void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
Definition: vpSimulatorAfma6.cpp:1763
vpDisplayD3D
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed....
Definition: vpDisplayD3D.h:105
vpColVector
Implementation of column vector and the associated operations.
Definition: vpColVector.h:129
vpHomogeneousMatrix::extract
void extract(vpRotationMatrix &R) const
Definition: vpHomogeneousMatrix.cpp:550
vpDisplayOpenCV
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Definition: vpDisplayOpenCV.h:140
vpServo::setServo
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:222
vpPlane::setABCD
void setABCD(double a, double b, double c, double d)
Definition: vpPlane.h:89
vpRobot::setRobotState
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:200
vpFeatureMomentCommon::getFeatureGravityNormalized
vpFeatureMomentGravityCenterNormalized & getFeatureGravityNormalized()
Definition: vpFeatureMomentCommon.h:265
vpServo::print
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:312
vpSimulatorAfma6
Simulator of Irisa's gantry robot named Afma6.
Definition: vpSimulatorAfma6.h:176
vpDisplay::display
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay_uchar.cpp:739
vpDisplayGTK
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:136
vpMomentObject::fromVector
void fromVector(std::vector< vpPoint > &points)
Definition: vpMomentObject.cpp:229
vpRobot::setMaxRotationVelocity
void setMaxRotationVelocity(double maxVr)
Definition: vpRobot.cpp:259
vpMomentObject::DISCRETE
Definition: vpMomentObject.h:230
vpPlane::getB
double getB() const
Definition: vpPlane.h:103
vpServo::getError
vpColVector getError() const
Definition: vpServo.h:281
vpFeatureMomentCommon::updateAll
void updateAll(double A, double B, double C)
Definition: vpFeatureMomentCommon.cpp:73
vpPlane::getC
double getC() const
Definition: vpPlane.h:105
vpRobot::CAMERA_FRAME
Definition: vpRobot.h:81
vpMomentCommon::getMu3
static std::vector< double > getMu3(vpMomentObject &object)
Definition: vpMomentCommon.cpp:198
vpServo::addFeature
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:496
vpDisplayX::init
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const std::string &title="")
Definition: vpDisplayX.cpp:251
vpMomentObject
Class for generic objects.
Definition: vpMomentObject.h:218
vpPlane::getD
double getD() const
Definition: vpPlane.h:107
vpPlane
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:57
vpPlane::changeFrame
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition: vpPlane.cpp:353
vpServo::setInteractionMatrixType
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:573
vpServo
Definition: vpServo.h:149
vpServo::vpServoIteractionMatrixType
vpServoIteractionMatrixType
Definition: vpServo.h:184
vpServo::computeControlLaw
vpColVector computeControlLaw()
Definition: vpServo.cpp:934
vpDisplay::flush
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay_uchar.cpp:715
vpImage< vpRGBa >
vpPoint
Class that defines what is a point.
Definition: vpPoint.h:57
vpDisplay::getClick
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
Definition: vpDisplay_uchar.cpp:764
vpColor
Class to define colors available for display functionnalities.
Definition: vpColor.h:118
vpHomogeneousMatrix
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition: vpHomogeneousMatrix.h:148
vpMomentCommon::getSurface
static double getSurface(vpMomentObject &object)
Definition: vpMomentCommon.cpp:148
vpAfma6::TOOL_CCMOP
Definition: vpAfma6.h:118
vpSimulatorCamera::setVelocity
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
Definition: vpSimulatorCamera.cpp:197
vpTRACE
#define vpTRACE
Definition: vpDebug.h:415
vpFeatureMomentCommon::getFeatureCInvariant
vpFeatureMomentCInvariant & getFeatureCInvariant()
Definition: vpFeatureMomentCommon.h:261
vpException
error that can be emited by ViSP classes.
Definition: vpException.h:70
vpMomentCommon::getAlpha
static double getAlpha(vpMomentObject &object)
Definition: vpMomentCommon.cpp:175
vpFeatureMomentCommon::getFeatureAn
vpFeatureMomentAreaNormalized & getFeatureAn()
Definition: vpFeatureMomentCommon.h:248
vpPlane::getA
double getA() const
Definition: vpPlane.h:101