Visual Servoing Platform  version 3.5.1 under development (2022-07-07)
servoPioneerPanSegment3D.cpp

Example that shows how to control the Pioneer mobile robot by IBVS visual servoing with respect to a segment. The segment consists in two horizontal dots. The current visual features that are used are ${\bf s} = (x_n, l_n, \alpha)$. The desired one are ${\bf s^*} = (0, l_n*, 0)$, with:

The degrees of freedom that are controlled are $(v_x, w_z, \dot{q})$, the translational and rotational velocity of the mobile platform at point M located at the middle between the two wheels, the head pan velocity respectively.

The depth of the points is estimated from the surface of the blob.

/****************************************************************************
*
* 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:
* IBVS on Pioneer P3DX mobile platform
*
* Authors:
* Fabien Spindler
*
*****************************************************************************/
#include <iostream>
#include <visp3/core/vpConfig.h>
#include <visp3/blob/vpDot2.h>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpVelocityTwistMatrix.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayX.h> // Should be included after vpRobotPioneer.h
#include <visp3/gui/vpPlot.h>
#include <visp3/robot/vpPioneerPan.h>
#include <visp3/robot/vpRobotBiclops.h>
#include <visp3/robot/vpRobotPioneer.h> // Include first to avoid build issues with Status, None, isfinite
#include <visp3/sensor/vp1394CMUGrabber.h>
#include <visp3/sensor/vp1394TwoGrabber.h>
#include <visp3/sensor/vpV4l2Grabber.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeatureSegment.h>
#include <visp3/vs/vpServo.h>
#define USE_REAL_ROBOT
#define USE_PLOTTER
#undef VISP_HAVE_V4L2 // To use a firewire camera
#if defined(VISP_HAVE_PIONEER) && defined(VISP_HAVE_BICLOPS)
int main(int argc, char **argv)
{
#if defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_CMU1394)
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
try {
vpImage<unsigned char> I; // Create a gray level image container
double lambda = 0.1;
// Scale parameter used to estimate the depth Z of the blob from its
// surface
// double coef = 0.9/14.85; // At 0.9m, the blob has a surface of 14.85
// (Logitec sphere)
double coef = 1.2 / 13.0; // At 1m, the blob has a surface of 11.3 (AVT Pike 032C)
double L = 0.21; // 3D horizontal segment length
double Z_d = 0.8; // Desired distance along Z between camera and segment
bool normalized = true; // segment normilized features are used
// Warning: To have a non singular task of rank 3, Y_d should be different
// from 0 so that the optical axis doesn't intersect the horizontal
// segment
double Y_d = -.11; // Desired distance along Y between camera and segment.
vpColVector qm(2); // Measured head position
qm = 0;
double qm_pan = 0; // Measured pan position (tilt is not handled in that example)
#ifdef USE_REAL_ROBOT
// Initialize the biclops head
vpRobotBiclops biclops("/usr/share/BiclopsDefault.cfg");
// Move to the initial position
q = 0;
// q[0] = vpMath::rad(63);
// q[1] = vpMath::rad(12); // introduce a tilt angle to compensate camera
// sphere tilt so that the camera is parallel to the plane
// biclops.setPositioningVelocity(50);
qm_pan = qm[0];
// Now the head will be controlled in velocity
// Initialize the pioneer robot
vpRobotPioneer pioneer;
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
// ArRobotConnector connects to the robot, get some initial data from it
// such as type and name, and then loads parameter files for this robot.
ArRobotConnector robotConnector(&parser, &pioneer);
if (!robotConnector.connectRobot()) {
ArLog::log(ArLog::Terse, "Could not connect to the pioneer robot.");
if (parser.checkHelpAndWarnUnparsed()) {
Aria::logOptions();
Aria::exit(1);
}
}
if (!Aria::parseArgs()) {
Aria::logOptions();
Aria::shutdown();
return false;
}
pioneer.useSonar(false); // disable the sonar device usage
// Wait 3 sec to be sure that the low level Aria thread used to control
// the robot is started. Without this delay we experienced a delay
// (arround 2.2 sec) between the velocity send to the robot and the
// velocity that is really applied to the wheels.
sleep(3);
std::cout << "Pioneer robot connected" << std::endl;
#endif
vpPioneerPan robot_pan; // Generic robot that computes the velocities for
// the pioneer and the biclops head
// Camera parameters. In this experiment we don't need a precise
// calibration of the camera
// Create the camera framegrabber
#if defined(VISP_HAVE_V4L2)
// Create a grabber based on v4l2 third party lib (for usb cameras under
// Linux)
g.setScale(1);
g.setInput(0);
g.setDevice("/dev/video1");
g.open(I);
// Logitec sphere parameters
cam.initPersProjWithoutDistortion(558, 555, 312, 210);
#elif defined(VISP_HAVE_DC1394)
// Create a grabber based on libdc1394-2.x third party lib (for firewire
// cameras under Linux)
vp1394TwoGrabber g(false);
// AVT Pike 032C parameters
cam.initPersProjWithoutDistortion(800, 795, 320, 216);
#elif defined(VISP_HAVE_CMU1394)
// Create a grabber based on CMU 1394 third party lib (for firewire
// cameras under windows)
g.setVideoMode(0, 5); // 640x480 MONO8
g.setFramerate(4); // 30 Hz
g.open(I);
// AVT Pike 032C parameters
cam.initPersProjWithoutDistortion(800, 795, 320, 216);
#endif
// Acquire an image from the grabber
g.acquire(I);
// Create an image viewer
#if defined(VISP_HAVE_X11)
vpDisplayX d(I, 10, 10, "Current frame");
#elif defined(VISP_HAVE_GDI)
vpDisplayGDI d(I, 10, 10, "Current frame");
#endif
// The 3D segment consists in two horizontal dots
vpDot2 dot[2];
for (int i = 0; i < 2; i++) {
dot[i].setGraphics(true);
dot[i].setComputeMoments(true);
dot[i].setEllipsoidShapePrecision(0.); // to track a blob without any constraint on the shape
dot[i].setGrayLevelPrecision(0.9); // to set the blob gray level bounds for binarisation
dot[i].setEllipsoidBadPointsPercentage(0.5); // to be accept 50% of bad
// inner and outside points
// with bad gray level
dot[i].initTracking(I);
}
vpServo task;
task.setLambda(lambda);
vpVelocityTwistMatrix cVe; // keep to identity
cVe = robot_pan.get_cVe();
task.set_cVe(cVe);
std::cout << "cVe: \n" << cVe << std::endl;
vpMatrix eJe;
// Update the robot jacobian that depends on the pan position
robot_pan.set_eJe(qm_pan);
// Get the robot jacobian
eJe = robot_pan.get_eJe();
task.set_eJe(eJe);
std::cout << "eJe: \n" << eJe << std::endl;
// Define a 3D horizontal segment an its cordinates in the image plane
vpPoint P[2];
P[0].setWorldCoordinates(-L / 2, 0, 0);
P[1].setWorldCoordinates(L / 2, 0, 0);
// Define the desired camera position
vpHomogeneousMatrix cMo(0, Y_d, Z_d, 0, 0,
0); // Here we are in front of the segment
for (int i = 0; i < 2; i++) {
P[i].changeFrame(cMo);
P[i].project(); // Here the x,y parameters obtained by perspective
// projection are computed
}
// Estimate the depth of the segment extremity points
double surface[2];
double Z[2]; // Depth of the segment points
for (int i = 0; i < 2; i++) {
// Surface of the blob estimated from the image moment m00 and converted
// in meters
surface[i] = 1. / sqrt(dot[i].m00 / (cam.get_px() * cam.get_py()));
// Initial depth of the blob
Z[i] = coef * surface[i];
}
// Use here a feature segment builder
vpFeatureSegment s_segment(normalized),
s_segment_d(normalized); // From the segment feature we use only alpha
vpFeatureBuilder::create(s_segment, cam, dot[0], dot[1]);
s_segment.setZ1(Z[0]);
s_segment.setZ2(Z[1]);
// Set the desired feature
vpFeatureBuilder::create(s_segment_d, P[0], P[1]);
s_segment.setZ1(P[0].get_Z()); // Desired depth
s_segment.setZ2(P[1].get_Z());
task.addFeature(s_segment, s_segment_d,
#ifdef USE_PLOTTER
// Create a window (500 by 500) at position (700, 10) with two graphics
vpPlot graph(2, 500, 500, 700, 10, "Curves...");
// The first graphic contains 3 curve and the second graphic contains 3
// curves
graph.initGraph(0, 3);
graph.initGraph(1, 3);
graph.setTitle(0, "Velocities");
graph.setTitle(1, "Error s-s*");
graph.setLegend(0, 0, "vx");
graph.setLegend(0, 1, "wz");
graph.setLegend(0, 2, "w_pan");
graph.setLegend(1, 0, "xm/l");
graph.setLegend(1, 1, "1/l");
graph.setLegend(1, 2, "alpha");
#endif
vpColVector v; // vz, wx
try {
unsigned int iter = 0;
while (1) {
#ifdef USE_REAL_ROBOT
// Get the new pan position
#endif
qm_pan = qm[0];
// Acquire a new image
g.acquire(I);
// Set the image as background of the viewer
// Display the desired position of the segment
for (int i = 0; i < 2; i++)
P[i].display(I, cam, vpColor::red, 3);
// Does the blob tracking
for (int i = 0; i < 2; i++)
dot[i].track(I);
for (int i = 0; i < 2; i++) {
// Surface of the blob estimated from the image moment m00 and
// converted in meters
surface[i] = 1. / sqrt(dot[i].m00 / (cam.get_px() * cam.get_py()));
// Initial depth of the blob
Z[i] = coef * surface[i];
}
// Update the features
vpFeatureBuilder::create(s_segment, cam, dot[0], dot[1]);
// Update the depth of the point. Useful only if current interaction
// matrix is used when task.setInteractionMatrixType(vpServo::CURRENT,
// vpServo::PSEUDO_INVERSE) is set
s_segment.setZ1(Z[0]);
s_segment.setZ2(Z[1]);
robot_pan.get_cVe(cVe);
task.set_cVe(cVe);
// Update the robot jacobian that depends on the pan position
robot_pan.set_eJe(qm_pan);
// Get the robot jacobian
eJe = robot_pan.get_eJe();
// Update the jacobian that will be used to compute the control law
task.set_eJe(eJe);
// Compute the control law. Velocities are computed in the mobile
// robot reference frame
v = task.computeControlLaw();
// std::cout << "-----" << std::endl;
// std::cout << "v: " << v.t() << std::endl;
// std::cout << "error: " << task.getError().t() << std::endl;
// std::cout << "L:\n " << task.getInteractionMatrix() <<
// std::endl; std::cout << "eJe:\n " << task.get_eJe() <<
// std::endl; std::cout << "cVe:\n " << task.get_cVe() <<
// std::endl; std::cout << "L_cVe_eJe:\n" <<
// task.getInteractionMatrix() * task.get_cVe() * task.get_eJe()
// << std::endl; task.print() ;
if (task.getTaskRank() != 3)
std::cout << "Warning: task is of rank " << task.getTaskRank() << std::endl;
#ifdef USE_PLOTTER
graph.plot(0, iter, v); // plot velocities applied to the robot
graph.plot(1, iter, task.getError()); // plot error vector
#endif
#ifdef USE_REAL_ROBOT
// Send the velocity to the robot
vpColVector v_pioneer(2); // vx, wz
v_pioneer[0] = v[0];
v_pioneer[1] = v[1];
vpColVector v_biclops(2); // qdot pan and tilt
v_biclops[0] = v[2];
v_biclops[1] = 0;
std::cout << "Send velocity to the pionner: " << v_pioneer[0] << " m/s " << vpMath::deg(v_pioneer[1])
<< " deg/s" << std::endl;
std::cout << "Send velocity to the biclops head: " << vpMath::deg(v_biclops[0]) << " deg/s" << std::endl;
#endif
// Draw a vertical line which corresponds to the desired x coordinate
// of the dot cog
vpDisplay::displayLine(I, 0, cam.get_u0(), 479, cam.get_u0(), vpColor::red);
// A click in the viewer to exit
if (vpDisplay::getClick(I, false))
break;
iter++;
// break;
}
} catch (...) {
}
#ifdef USE_REAL_ROBOT
std::cout << "Ending robot thread..." << std::endl;
pioneer.stopRunning();
// wait for the thread to stop
pioneer.waitForRunExit();
#endif
// Kill the servo task
task.print();
return EXIT_SUCCESS;
} catch (const vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return EXIT_FAILURE;
}
#endif
#endif
}
#else
int main()
{
std::cout << "ViSP is not able to control the Pioneer robot" << std::endl;
return EXIT_SUCCESS;
}
#endif
Firewire cameras video capture based on CMU 1394 Digital Camera SDK.
Class for firewire ieee1394 video devices using libdc1394-2.x api.
void setDenavitHartenbergModel(vpBiclops::DenavitHartenbergModel m=vpBiclops::DH1)
Definition: vpBiclops.h:185
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
static const vpColor red
Definition: vpColor.h:217
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:135
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)
This tracker is meant to track a blob (connex pixels with same gray level) on a vpImage.
Definition: vpDot2.h:127
void setGraphics(bool activate)
Definition: vpDot2.h:314
void setGrayLevelPrecision(const double &grayLevelPrecision)
Definition: vpDot2.cpp:736
void setEllipsoidBadPointsPercentage(const double &percentage=0.0)
Definition: vpDot2.h:290
void setEllipsoidShapePrecision(const double &ellipsoidShapePrecision)
Definition: vpDot2.cpp:807
void setComputeMoments(bool activate)
Definition: vpDot2.h:276
void initTracking(const vpImage< unsigned char > &I, unsigned int size=0)
Definition: vpDot2.cpp:253
error that can be emited by ViSP classes.
Definition: vpException.h:72
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class that defines a 2D segment visual features. This class allow to consider two sets of visual feat...
static unsigned int selectAlpha()
static unsigned int selectXc()
static unsigned int selectL()
Implementation of an homogeneous matrix and operations on such kind of matrices.
static double deg(double rad)
Definition: vpMath.h:110
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
Generic functions for Pioneer mobile robots equiped with a pan head.
Definition: vpPioneerPan.h:98
void set_eJe(double q_pan)
Definition: vpPioneerPan.h:146
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Definition: vpPlot.h:116
void initGraph(unsigned int graphNum, unsigned int curveNbr)
Definition: vpPlot.cpp:205
void setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
Definition: vpPlot.cpp:534
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
Definition: vpPlot.cpp:285
void setTitle(unsigned int graphNum, const std::string &title)
Definition: vpPlot.cpp:497
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
Definition: vpPoint.cpp:239
void setWorldCoordinates(double oX, double oY, double oZ)
Definition: vpPoint.cpp:113
Interface for the biclops, pan, tilt head control.
vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &q_dot)
Interface for Pioneer mobile robots based on Aria 3rd party library.
void useSonar(bool usage)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
@ REFERENCE_FRAME
Definition: vpRobot.h:77
@ ARTICULAR_FRAME
Definition: vpRobot.h:79
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:68
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:67
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:564
@ EYEINHAND_L_cVe_eJe
Definition: vpServo.h:159
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:448
unsigned int getTaskRank() const
Definition: vpServo.cpp:1783
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:303
void setLambda(double c)
Definition: vpServo.h:404
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:506
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:215
vpColVector getError() const
Definition: vpServo.h:278
@ PSEUDO_INVERSE
Definition: vpServo.h:202
vpColVector computeControlLaw()
Definition: vpServo.cpp:926
@ DESIRED
Definition: vpServo.h:186
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:487
vpMatrix get_eJe() const
Definition: vpUnicycle.h:107
vpVelocityTwistMatrix get_cVe() const
Definition: vpUnicycle.h:82
Class that is a wrapper over the Video4Linux2 (V4L2) driver.
void setFramerate(vpV4l2FramerateType framerate)
void setInput(unsigned input=vpV4l2Grabber::DEFAULT_INPUT)
void open(vpImage< unsigned char > &I)
void setScale(unsigned scale=vpV4l2Grabber::DEFAULT_SCALE)
void setDevice(const std::string &devname)
void acquire(vpImage< unsigned char > &I)