Visual Servoing Platform  version 3.3.0
servoSimuCylinder.cpp

Demonstration of the wireframe simulator with a simple visual servoing.

/****************************************************************************
*
* 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:
* Demonstration of the wireframe simulator with a simple visual servoing
*
* Authors:
* Nicolas Melchior
*
*****************************************************************************/
#include <stdlib.h>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpCylinder.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpTime.h>
#include <visp3/core/vpVelocityTwistMatrix.h>
#include <visp3/gui/vpDisplayD3D.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/robot/vpWireFrameSimulator.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/vs/vpServo.h>
#define GETOPTARGS "dh"
#ifdef VISP_HAVE_DISPLAY
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv, bool &display);
void usage(const char *name, const char *badparam)
{
fprintf(stdout, "\n\
Demonstration of the wireframe simulator with a simple visual servoing.\n\
\n\
The visual servoing consists in bringing the camera at a desired position\n\
from the object.\n\
\n\
The visual features used to compute the pose of the camera and \n\
thus the control law are two lines. These features are computed thanks \n\
to the equation of a cylinder.\n\
\n\
This demonstration explains also how to move the object around a world \n\
reference frame. Here, the movment is a rotation around the x and y axis \n\
at a given distance from the world frame. In fact the object trajectory \n\
is on a sphere whose center is the origin of the world frame.\n\
\n\
SYNOPSIS\n\
%s [-d] [-h]\n", name);
fprintf(stdout, "\n\
OPTIONS: \n\
-d \n\
Turn off the display.\n\
\n\
-h\n\
Print the help.\n");
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
}
bool getOptions(int argc, const char **argv, bool &display)
{
const char *optarg_;
int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
case 'd':
display = false;
break;
case 'h':
usage(argv[0], NULL);
return false;
break;
default:
usage(argv[0], optarg_);
return false;
break;
}
}
if ((c == 1) || (c == -1)) {
// standalone param or error
usage(argv[0], NULL);
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 {
bool opt_display = true;
// Read the command line options
if (getOptions(argc, argv, opt_display) == false) {
exit(-1);
}
vpImage<vpRGBa> Iint(480, 640, 255);
vpImage<vpRGBa> Iext(480, 640, 255);
#if defined VISP_HAVE_X11
vpDisplayX display[2];
#elif defined VISP_HAVE_OPENCV
vpDisplayOpenCV display[2];
#elif defined VISP_HAVE_GDI
vpDisplayGDI display[2];
#elif defined VISP_HAVE_D3D9
vpDisplayD3D display[2];
#elif defined VISP_HAVE_GTK
vpDisplayGTK display[2];
#endif
if (opt_display) {
// Display size is automatically defined by the image (I) size
display[0].init(Iint, 100, 100, "The internal view");
display[1].init(Iext, 100, 100, "The first external view");
}
vpServo task;
float sampling_time = 0.040f; // Sampling period in second
robot.setSamplingTime(sampling_time);
// Set initial position of the object in the camera frame
vpHomogeneousMatrix cMo(0, 0.1, 0.3, vpMath::rad(35), vpMath::rad(25), vpMath::rad(75));
// Set desired position of the object in the camera frame
vpHomogeneousMatrix cdMo(0.0, 0.0, 0.5, vpMath::rad(90), vpMath::rad(0), vpMath::rad(0));
// Set initial position of the object in the world frame
vpHomogeneousMatrix wMo(0.0, 0.0, 0, 0, 0, 0);
// Position of the camera in the world frame
wMc = wMo * cMo.inverse();
// Create a cylinder
vpCylinder cylinder(0, 0, 1, 0, 0, 0, 0.1);
// Projection of the cylinder
cylinder.track(cMo);
// Set the current visual feature
// Projection of the cylinder
cylinder.track(cdMo);
task.set_cVe(cVe);
vpMatrix eJe;
robot.get_eJe(eJe);
task.set_eJe(eJe);
for (int i = 0; i < 2; i++)
task.addFeature(l[i], ld[i]);
task.setLambda(1);
// Set the scene
// Initialize simulator frames
sim.set_fMo(wMo); // Position of the object in the world reference frame
sim.setCameraPositionRelObj(cMo); // Initial position of the object in the camera frame
sim.setDesiredCameraPosition(cdMo); // Desired position of the object in the camera frame
// Set the External camera position
// Set the parameters of the cameras (internal and external)
vpCameraParameters camera(1000, 1000, 320, 240);
int stop = 10;
if (opt_display) {
stop = 2500;
// Get the internal and external views
sim.getInternalImage(Iint);
sim.getExternalImage(Iext);
// Display the object frame (current and desired position)
vpDisplay::displayFrame(Iint, cMo, camera, 0.2, vpColor::none);
vpDisplay::displayFrame(Iint, cdMo, camera, 0.2, vpColor::none);
// Display the object frame the world reference frame and the camera
// frame
vpDisplay::displayFrame(Iext, camMf * sim.get_fMo() * cMo.inverse(), camera, 0.2, vpColor::none);
vpDisplay::displayFrame(Iext, camMf * sim.get_fMo(), camera, 0.2, vpColor::none);
vpDisplay::displayFrame(Iext, camMf, camera, 0.2, vpColor::none);
std::cout << "Click on a display" << std::endl;
while (!vpDisplay::getClick(Iint, false) && !vpDisplay::getClick(Iext, false)) {
};
}
robot.setPosition(wMc);
// Print the task
task.print();
int iter = 0;
// Set the secondary task parameters
vpColVector e1(6);
e1 = 0;
vpColVector e2(6);
e2 = 0;
vpColVector proj_e1;
vpColVector proj_e2;
iter = 0;
double rapport = 0;
double vitesse = 0.3;
int tempo = 600;
while (iter++ < stop) {
if (opt_display) {
}
double t = vpTime::measureTimeMs();
robot.get_eJe(eJe);
task.set_eJe(eJe);
wMc = robot.getPosition();
cMo = wMc.inverse() * wMo;
cylinder.track(cMo);
v = task.computeControlLaw();
// Compute the velocity with the secondary task
if (iter % tempo < 200 && iter % tempo >= 0) {
e2 = 0;
e1[0] = -fabs(vitesse);
proj_e1 = task.secondaryTask(e1);
rapport = -vitesse / proj_e1[0];
proj_e1 *= rapport;
v += proj_e1;
}
if (iter % tempo < 300 && iter % tempo >= 200) {
e1 = 0;
e2[1] = -fabs(vitesse);
proj_e2 = task.secondaryTask(e2);
rapport = -vitesse / proj_e2[1];
proj_e2 *= rapport;
v += proj_e2;
}
if (iter % tempo < 500 && iter % tempo >= 300) {
e2 = 0;
e1[0] = -fabs(vitesse);
proj_e1 = task.secondaryTask(e1);
rapport = vitesse / proj_e1[0];
proj_e1 *= rapport;
v += proj_e1;
}
if (iter % tempo < 600 && iter % tempo >= 500) {
e1 = 0;
e2[1] = -fabs(vitesse);
proj_e2 = task.secondaryTask(e2);
rapport = vitesse / proj_e2[1];
proj_e2 *= rapport;
v += proj_e2;
}
// Update the simulator frames
sim.set_fMo(wMo); // This line is not really requested since the object
// doesn't move
if (opt_display) {
// Get the internal and external views
sim.getInternalImage(Iint);
sim.getExternalImage(Iext);
// Display the object frame (current and desired position)
vpDisplay::displayFrame(Iint, cMo, camera, 0.2, vpColor::none);
vpDisplay::displayFrame(Iint, cdMo, camera, 0.2, vpColor::none);
// Display the object frame the world reference frame and the camera
// frame
vpDisplay::displayFrame(Iext, sim.getExternalCameraPosition() * sim.get_fMo() * cMo.inverse(), camera, 0.2,
;
}
vpTime::wait(t, sampling_time * 1000); // Wait 40 ms
std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
}
task.print();
task.kill();
return EXIT_SUCCESS;
} catch (const vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return EXIT_FAILURE;
}
}
#else
int main()
{
std::cout << "You do not have X11, or GDI (Graphical Device Interface), or GTK functionalities to display images..." << std::endl;
std::cout << "Tip if you are on a unix-like system:" << std::endl;
std::cout << "- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
std::cout << "Tip if you are on a windows-like system:" << std::endl;
std::cout << "- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
return EXIT_SUCCESS;
}
#endif
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
vpWireFrameSimulator::setExternalCameraPosition
void setExternalCameraPosition(const vpHomogeneousMatrix &cam_Mf)
Definition: vpWireFrameSimulator.h:546
vpWireFrameSimulator::D_STANDARD
Definition: vpWireFrameSimulator.h:211
vpServo::kill
void kill()
Definition: vpServo.cpp:191
vpTime::wait
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:172
vpWireFrameSimulator::get_fMo
vpHomogeneousMatrix get_fMo() const
Definition: vpWireFrameSimulator.h:424
vpDisplay::setWindowPosition
static void setWindowPosition(const vpImage< unsigned char > &I, int winx, int winy)
Definition: vpDisplay_uchar.cpp:1237
vpMath::rad
static double rad(double deg)
Definition: vpMath.h:107
vpCameraParameters
Generic class defining intrinsic camera parameters.
Definition: vpCameraParameters.h:232
vpServo::set_eJe
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:507
vpServo::setLambda
void setLambda(double c)
Definition: vpServo.h:405
vpSimulatorCamera::setPosition
void setPosition(const vpHomogeneousMatrix &wMc)
Definition: vpSimulatorCamera.cpp:241
vpWireFrameSimulator::getExternalImage
void getExternalImage(vpImage< unsigned char > &I)
Definition: vpWireFrameSimulator.cpp:1074
vpWireFrameSimulator
Implementation of a wire frame simulator. Compared to the vpSimulator class, it does not require thir...
Definition: vpWireFrameSimulator.h:153
vpDisplayGDI
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:127
vpFeatureBuilder::create
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Definition: vpFeatureBuilderPoint.cpp:92
vpWireFrameSimulator::set_fMo
void set_fMo(const vpHomogeneousMatrix &fMo_)
Definition: vpWireFrameSimulator.h:586
vpServo::set_cVe
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:449
vpWireFrameSimulator::setDesiredCameraPosition
void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo_)
Definition: vpWireFrameSimulator.h:513
vpDisplayD3D
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed....
Definition: vpDisplayD3D.h:105
vpSimulatorCamera
Class that defines the simplest robot: a free flying camera.
Definition: vpSimulatorCamera.h:106
vpColVector
Implementation of column vector and the associated operations.
Definition: vpColVector.h:129
vpWireFrameSimulator::setInternalCameraParameters
void setInternalCameraParameters(const vpCameraParameters &cam)
Definition: vpWireFrameSimulator.h:566
vpDisplay::displayFrame
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))
Definition: vpDisplay_uchar.cpp:383
vpDisplayOpenCV
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Definition: vpDisplayOpenCV.h:140
vpMatrix
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:163
vpServo::setServo
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:222
vpTime::measureTimeMs
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:125
vpWireFrameSimulator::initScene
void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject)
Definition: vpWireFrameSimulator.cpp:261
vpParseArgv::parse
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:68
vpCylinder
Class that defines what is a cylinder.
Definition: vpCylinder.h:95
vpSimulatorCamera::getPosition
vpHomogeneousMatrix getPosition() const
Definition: vpSimulatorCamera.cpp:118
vpServo::print
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:312
vpDisplay::display
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay_uchar.cpp:739
vpCylinder::line1
Definition: vpCylinder.h:99
vpDisplayGTK
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:136
vpWireFrameSimulator::setCameraPositionRelObj
void setCameraPositionRelObj(const vpHomogeneousMatrix &cMo_)
Definition: vpWireFrameSimulator.h:456
vpServo::secondaryTask
vpColVector secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator=false)
Definition: vpServo.cpp:1484
vpServo::getError
vpColVector getError() const
Definition: vpServo.h:281
vpServo::DESIRED
Definition: vpServo.h:189
vpVelocityTwistMatrix
Definition: vpVelocityTwistMatrix.h:165
vpFeatureLine
Class that defines a 2D line visual feature which is composed by two parameters that are and ,...
Definition: vpFeatureLine.h:194
vpSimulatorCamera::get_eJe
void get_eJe(vpMatrix &eJe)
Definition: vpSimulatorCamera.cpp:107
vpColor::none
static const vpColor none
Definition: vpColor.h:190
vpWireFrameSimulator::setExternalCameraParameters
void setExternalCameraParameters(const vpCameraParameters &cam)
Definition: vpWireFrameSimulator.h:535
vpRobot::CAMERA_FRAME
Definition: vpRobot.h:81
vpServo::EYEINHAND_L_cVe_eJe
Definition: vpServo.h:162
vpServo::addFeature
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:496
vpCylinder::line2
Definition: vpCylinder.h:100
vpServo::setInteractionMatrixType
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:573
vpServo
Definition: vpServo.h:149
vpServo::computeControlLaw
vpColVector computeControlLaw()
Definition: vpServo.cpp:934
vpDisplay::flush
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay_uchar.cpp:715
vpRobotSimulator::setSamplingTime
virtual void setSamplingTime(const double &delta_t)
Definition: vpRobotSimulator.h:90
vpHomogeneousMatrix::inverse
vpHomogeneousMatrix inverse() const
Definition: vpHomogeneousMatrix.cpp:640
vpImage< vpRGBa >
vpDisplay::getClick
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
Definition: vpDisplay_uchar.cpp:764
vpHomogeneousMatrix
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition: vpHomogeneousMatrix.h:148
vpSimulatorCamera::setVelocity
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
Definition: vpSimulatorCamera.cpp:197
vpException
error that can be emited by ViSP classes.
Definition: vpException.h:70
vpWireFrameSimulator::getExternalCameraPosition
vpHomogeneousMatrix getExternalCameraPosition() const
Definition: vpWireFrameSimulator.h:349
vpWireFrameSimulator::getInternalImage
void getInternalImage(vpImage< unsigned char > &I)
Definition: vpWireFrameSimulator.cpp:1000
vpWireFrameSimulator::CYLINDER
A cylinder of 80cm length and 10cm radius.
Definition: vpWireFrameSimulator.h:192