Visual Servoing Platform  version 3.0.0
manSimu4Points.cpp

Visual servoing experiment on 4 points with a visualization from the camera and from an external view using vpSimulator.

/****************************************************************************
*
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2015 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
* ("GPL") version 2 as published by the Free Software Foundation.
* 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:
* Simulation of a visual servoing with visualization.
*
* Authors:
* Eric Marchand
* Fabien Spindler
*
*****************************************************************************/
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
#if (defined(VISP_HAVE_COIN3D_AND_GUI))
#include <visp3/core/vpImage.h>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpTime.h>
#include <visp3/ar/vpSimulator.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/vs/vpServo.h>
#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/io/vpParseArgv.h>
#include <visp3/core/vpIoTools.h>
static
void *mainLoop (void *_simu)
{
// pointer copy of the vpSimulator instance
vpSimulator *simu = (vpSimulator *)_simu ;
// Simulation initialization
// sets the initial camera location
vpHomogeneousMatrix cMo(-0.3,-0.2,3,
vpHomogeneousMatrix wMo; // Set to identity
vpHomogeneousMatrix wMc; // Camera position in the world frame
// Initialize the robot
robot.setSamplingTime(0.04); // 40ms
wMc = wMo * cMo.inverse();
robot.setPosition(wMc) ;
// Send the robot position to the visualizator
simu->setCameraPosition(cMo) ;
// Initialize the camera parameters
simu->getCameraParameters(cam);
// Desired visual features initialization
// sets the points coordinates in the object frame (in meter)
vpPoint point[4] ;
point[0].setWorldCoordinates(-0.1,-0.1,0) ;
point[1].setWorldCoordinates(0.1,-0.1,0) ;
point[2].setWorldCoordinates(0.1,0.1,0) ;
point[3].setWorldCoordinates(-0.1,0.1,0) ;
// sets the desired camera location
vpHomogeneousMatrix cMo_d(0,0,1,0,0,0) ;
// computes the 3D point coordinates in the camera frame and its 2D coordinates
for (int i = 0 ; i < 4 ; i++)
point[i].project(cMo_d) ;
// creates the associated features
for (int i = 0 ; i < 4 ; i++)
vpFeatureBuilder::create(pd[i],point[i]) ;
// Current visual features initialization
// computes the 3D point coordinates in the camera frame and its 2D coordinates
for (int i = 0 ; i < 4 ; i++)
point[i].project(cMo) ;
// creates the associated features
for (int i = 0 ; i < 4 ; i++)
vpFeatureBuilder::create(p[i],point[i]) ;
// Task defintion
vpServo task ;
// we want an eye-in-hand control law ;
// Set the position of the camera in the end-effector frame
task.set_cVe(cVe) ;
// Set the Jacobian (expressed in the end-effector frame)
vpMatrix eJe ;
robot.get_eJe(eJe) ;
task.set_eJe(eJe) ;
// we want to see a point on a point
for (int i = 0 ; i < 4 ; i++)
task.addFeature(p[i],pd[i]) ;
// Set the gain
task.setLambda(1.0) ;
// Print the current information about the task
task.print();
// The control loop
int k = 0;
while(k++ < 200){
double t = vpTime::measureTimeMs();
// Update the current features
for (int i = 0 ; i < 4 ; i++)
{
point[i].project(cMo) ;
vpFeatureBuilder::create(p[i],point[i]) ;
}
// Update the robot Jacobian
robot.get_eJe(eJe) ;
task.set_eJe(eJe) ;
// Compute the control law
// Send the computed velocity to the robot and compute the new robot position
wMc = robot.getPosition();
cMo = wMc.inverse() * wMo;
// Send the robot position to the visualizator
simu->setCameraPosition(cMo) ;
// Print the current information about the task
task.print();
// Wait 40 ms
vpTime::wait(t,40);
}
task.kill();
void *a=NULL ;
return a ;
// return (void *);
}
int
main()
{
try {
vpSimulator simu ;
// Internal view initialization : view from the robot camera
simu.initInternalViewer(480, 360) ;
// External view initialization : view from an external camera
simu.initExternalViewer(300, 300) ;
// Inernal camera paramters initialization
vpCameraParameters cam(800,800,240,180) ;
vpTime::wait(1000) ;
// Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
std::string ipath = vpIoTools::getViSPImagesDataPath();
std::string filename = "./4points.iv";
// Set the default input path
if (! ipath.empty())
filename = vpIoTools::createFilePath(ipath, "ViSP-images/iv/4points.iv");
std::cout << "Load : " << filename << std::endl
<< "This file should be in the working directory" << std::endl;
simu.load(filename.c_str());
// Run the main loop
simu.initApplication(&mainLoop) ;
// Run the simulator
simu.mainLoop() ;
return 0;
}
catch(vpException e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
#else
int
main()
{
vpTRACE("You should install Coin3D and/or GTK") ;
}
#endif