Visual Servoing Platform  version 3.3.0
servoViper650FourPoints2DCamVelocityLs_cur-SR300.cpp

Example of eye-in-hand control law. We control here a real robot, the Viper S650 robot (arm with 6 degrees of freedom). The velocity is computed in camera frame. The inverse jacobian that converts cartesian velocities in joint velocities is implemented in the robot low level controller. Visual features are the image coordinates of 4 points. The target is made of 4 dots arranged as a 10cm by 10cm square.The device used to acquire images is a Realsense SR300 device.

Camera extrinsic (eMc) parameters are read from SR300-eMc.cnf file located besides this code.

Camera intrinsic parameters are retrieved from the Realsense SDK.

/****************************************************************************
*
* 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:
* tests the control law
* eye-in-hand control
* velocity computed in the camera frame
*
* Authors:
* Fabien Spindler
*
*****************************************************************************/
#include <fstream>
#include <iostream>
#include <sstream>
#include <stdio.h>
#include <stdlib.h>
#include <visp3/core/vpConfig.h>
#if defined(VISP_HAVE_VIPER650) && defined(VISP_HAVE_REALSENSE) && defined(VISP_HAVE_X11)
#include <visp3/blob/vpDot2.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpPoint.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/robot/vpRobotViper650.h>
#include <visp3/sensor/vpRealSense.h>
#include <visp3/vision/vpPose.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
#define L 0.05 // to deal with a 10cm by 10cm square
void compute_pose(std::vector<vpPoint> &point, std::vector<vpDot2> &dot, vpCameraParameters cam,
vpHomogeneousMatrix &cMo, bool init)
{
vpHomogeneousMatrix cMo_dementhon; // computed pose with dementhon method
vpHomogeneousMatrix cMo_lagrange; // computed pose with lagrange method
vpPose pose;
for (size_t i = 0; i < point.size(); i++) {
double x = 0, y = 0;
vpImagePoint cog = dot[i].getCog();
y); // pixel to meter conversion
point[i].set_x(x); // projection perspective p
point[i].set_y(y);
pose.addPoint(point[i]);
}
if (init == true) {
pose.computePose(vpPose::DEMENTHON, cMo_dementhon);
// Compute and return the residual expressed in meter for the pose matrix
double residual_dementhon = pose.computeResidual(cMo_dementhon);
pose.computePose(vpPose::LAGRANGE, cMo_lagrange);
double residual_lagrange = pose.computeResidual(cMo_lagrange);
// Select the best pose to initialize the lowe pose computation
if (residual_lagrange < residual_dementhon)
cMo = cMo_lagrange;
else
cMo = cMo_dementhon;
}
}
int main()
{
// Log file creation in /tmp/$USERNAME/log.dat
// This file contains by line:
// - the 6 computed camera velocities (m/s, rad/s) to achieve the task
// - the 6 mesured joint velocities (m/s, rad/s)
// - the 6 mesured joint positions (m, rad)
// - the 8 values of s - s*
std::string username;
// Get the user login name
// Create a log filename to save velocities...
std::string logdirname;
logdirname = "/tmp/" + username;
// Test if the output path exist. If no try to create it
if (vpIoTools::checkDirectory(logdirname) == false) {
try {
// Create the dirname
} catch (...) {
std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << logdirname << std::endl;
return (-1);
}
}
std::string logfilename;
logfilename = logdirname + "/log.dat";
// Open the log file name
std::ofstream flog(logfilename.c_str());
try {
// Load the end-effector to camera frame transformation from SR300-eMc.cnf
// file
robot.init(vpRobotViper650::TOOL_CUSTOM, "./SR300-eMc.cnf");
robot.get_eMc(eMc);
std::cout << "Camera extrinsic parameters (eMc): \n" << eMc << std::endl;
vpServo task;
// Enable the RealSense device to acquire only color images with size
// 640x480
g.setEnableStream(rs::stream::color, true);
g.setEnableStream(rs::stream::depth, false);
g.setEnableStream(rs::stream::infrared, false);
g.setEnableStream(rs::stream::infrared2, false);
g.setStreamSettings(rs::stream::color, vpRealSense::vpRsStreamParams(640, 480, rs::format::rgba8, 30));
g.open();
// Update camera parameters
std::cout << "Camera intrinsic parameters: \n" << cam << std::endl;
g.acquire(I);
vpDisplayX display(I, 100, 100, "Current image");
std::vector<vpDot2> dot(4);
std::cout << "Click on the 4 dots clockwise starting from upper/left dot..." << std::endl;
for (size_t i = 0; i < dot.size(); i++) {
dot[i].setGraphics(true);
dot[i].initTracking(I);
vpImagePoint cog = dot[i].getCog();
}
// Sets the current position of the visual feature
for (size_t i = 0; i < dot.size(); i++)
vpFeatureBuilder::create(p[i], cam, dot[i]); // retrieve x,y of the vpFeaturePoint structure
// Set the position of the square target in a frame which origin is
// centered in the middle of the square
std::vector<vpPoint> point(4);
point[0].setWorldCoordinates(-L, -L, 0);
point[1].setWorldCoordinates(L, -L, 0);
point[2].setWorldCoordinates(L, L, 0);
point[3].setWorldCoordinates(-L, L, 0);
// Compute target initial pose
compute_pose(point, dot, cam, cMo, true);
std::cout << "Initial camera pose (cMo): \n" << cMo << std::endl;
// Initialise a desired pose to compute s*, the desired 2D point features
vpHomogeneousMatrix cMo_d(vpTranslationVector(0, 0, 0.5), // tz = 0.5 meter
vpRotationMatrix()); // no rotation
// Sets the desired position of the 2D visual feature
// Compute the desired position of the features from the desired pose
for (int i = 0; i < 4; i++) {
vpColVector cP, p;
point[i].changeFrame(cMo_d, cP);
point[i].projection(cP, p);
pd[i].set_x(p[0]);
pd[i].set_y(p[1]);
pd[i].set_Z(cP[2]);
}
// We want to see a point on a point
for (size_t i = 0; i < dot.size(); i++)
task.addFeature(p[i], pd[i]);
// Set the proportional gain
task.setLambda(0.3);
// Define the task
// - we want an eye-in-hand control law
// - camera velocities are computed
task.print();
// Initialise the velocity control of the robot
std::cout << "\nHit CTRL-C or click in the image to stop the loop...\n" << std::flush;
for (;;) {
// Acquire a new image from the camera
g.acquire(I);
// Display this image
try {
// For each point...
for (size_t i = 0; i < dot.size(); i++) {
// Achieve the tracking of the dot in the image
dot[i].track(I);
// Display a green cross at the center of gravity position in the
// image
vpImagePoint cog = dot[i].getCog();
}
} catch (...) {
std::cout << "Error detected while tracking visual features.." << std::endl;
break;
}
// During the servo, we compute the pose using a non linear method. For
// the initial pose used in the non linear minimisation we use the pose
// computed at the previous iteration.
compute_pose(point, dot, cam, cMo, false);
for (size_t i = 0; i < dot.size(); i++) {
// Update the point feature from the dot location
vpFeatureBuilder::create(p[i], cam, dot[i]);
// Set the feature Z coordinate from the pose
point[i].changeFrame(cMo, cP);
p[i].set_Z(cP[2]);
}
// Compute the visual servoing skew vector
// Display the current and desired feature points in the image display
vpServoDisplay::display(task, cam, I);
// Apply the computed joint velocities to the robot
// Save velocities applied to the robot in the log file
// v[0], v[1], v[2] correspond to camera translation velocities in m/s
// v[3], v[4], v[5] correspond to camera rotation velocities in rad/s
flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " ";
// Get the measured joint velocities of the robot
robot.getVelocity(vpRobot::ARTICULAR_FRAME, qvel);
// Save measured joint velocities of the robot in the log file:
// - qvel[0], qvel[1], qvel[2] correspond to measured joint translation
// velocities in m/s
// - qvel[3], qvel[4], qvel[5] correspond to measured joint rotation
// velocities in rad/s
flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] << " " << qvel[4] << " " << qvel[5] << " ";
// Get the measured joint positions of the robot
// Save measured joint positions of the robot in the log file
// - q[0], q[1], q[2] correspond to measured joint translation
// positions in m
// - q[3], q[4], q[5] correspond to measured joint rotation
// positions in rad
flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] << " " << q[5] << " ";
// Save feature error (s-s*) for the 4 feature points. For each feature
// point, we have 2 errors (along x and y axis). This error is
// expressed in meters in the camera frame
flog << task.getError() << std::endl;
vpDisplay::displayText(I, 10, 10, "Click to quit...", vpColor::red);
if (vpDisplay::getClick(I, false))
break;
// Flush the display
// std::cout << "\t\t || s - s* || = " << ( task.getError()
// ).sumSquare() << std::endl;
}
std::cout << "Display task information: " << std::endl;
task.print();
task.kill();
flog.close(); // Close the log file
return EXIT_SUCCESS;
} catch (const vpException &e) {
flog.close(); // Close the log file
std::cout << "Catch an exception: " << e.getMessage() << std::endl;
return EXIT_FAILURE;
}
}
#else
int main()
{
std::cout << "You do not have an Viper 650 robot connected to your computer..." << std::endl;
return EXIT_SUCCESS;
}
#endif
vpRobot::STATE_VELOCITY_CONTROL
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:66
vpIoTools::getUserName
static std::string getUserName()
Definition: vpIoTools.cpp:318
vpDisplayX
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:150
vpRobot::ARTICULAR_FRAME
@ ARTICULAR_FRAME
Definition: vpRobot.h:78
vpFeaturePoint::set_x
void set_x(double x)
Definition: vpFeaturePoint.cpp:118
vpIoTools::checkDirectory
static bool checkDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:422
vpServo::kill
void kill()
Definition: vpServo.cpp:192
vpServo::CURRENT
@ CURRENT
Definition: vpServo.h:186
vpRealSense
Definition: vpRealSense.h:333
vpCameraParameters
Generic class defining intrinsic camera parameters.
Definition: vpCameraParameters.h:233
vpRobotViper650
Control of Irisa's Viper S650 robot named Viper650.
Definition: vpRobotViper650.h:336
vpServo::setLambda
void setLambda(double c)
Definition: vpServo.h:406
vpServo::EYEINHAND_CAMERA
@ EYEINHAND_CAMERA
Definition: vpServo.h:159
vpRealSense::acquire
void acquire(std::vector< vpColVector > &pointcloud)
Definition: vpRealSense.cpp:421
vpFeatureBuilder::create
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Definition: vpFeatureBuilderPoint.cpp:93
vpPose::addPoint
void addPoint(const vpPoint &P)
Definition: vpPose.cpp:149
vpPose::LOWE
@ LOWE
Definition: vpPose.h:87
vpTranslationVector
Class that consider the case of a translation vector.
Definition: vpTranslationVector.h:119
vpColVector
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
vpFeaturePoint::set_y
void set_y(double y)
Definition: vpFeaturePoint.cpp:138
vpPose::LAGRANGE
@ LAGRANGE
Definition: vpPose.h:85
vpPose::computePose
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition: vpPose.cpp:374
vpServo::setServo
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:223
vpRobot::setRobotState
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
vpSimulatorCamera::getPosition
vpHomogeneousMatrix getPosition() const
Definition: vpSimulatorCamera.cpp:119
vpColor::green
static const vpColor green
Definition: vpColor.h:182
vpRealSense::open
void open()
Definition: vpRealSense.cpp:98
vpServo::print
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:313
vpFeaturePoint::set_Z
void set_Z(double Z)
Definition: vpFeaturePoint.cpp:97
vpDisplay::display
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay_uchar.cpp:740
vpDisplay::displayText
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Definition: vpDisplay_uchar.cpp:664
vpIoTools::makeDirectory
static void makeDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:572
vpRotationMatrix
Implementation of a rotation matrix and operations on such kind of matrices.
Definition: vpRotationMatrix.h:122
vpPixelMeterConversion::convertPoint
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Definition: vpPixelMeterConversion.h:103
vpRealSense::getCameraParameters
vpCameraParameters getCameraParameters(const rs::stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion) const
Definition: vpRealSense.cpp:287
vpServo::getError
vpColVector getError() const
Definition: vpServo.h:282
vpImagePoint
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
vpException::getMessage
const char * getMessage(void) const
Definition: vpException.cpp:90
vpViper650::TOOL_CUSTOM
@ TOOL_CUSTOM
Definition: vpViper650.h:132
vpPose::computeResidual
double computeResidual(const vpHomogeneousMatrix &cMo) const
Compute and return the sum of squared residuals expressed in meter^2 for the pose matrix cMo.
Definition: vpPose.cpp:336
vpRobot::CAMERA_FRAME
@ CAMERA_FRAME
Definition: vpRobot.h:82
vpServo::addFeature
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:497
vpFeaturePoint
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
Definition: vpFeaturePoint.h:181
vpPose
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:80
vpServo::setInteractionMatrixType
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:574
vpColor::blue
static const vpColor blue
Definition: vpColor.h:185
vpServo
Definition: vpServo.h:150
vpServo::computeControlLaw
vpColVector computeControlLaw()
Definition: vpServo.cpp:935
vpDisplay::flush
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay_uchar.cpp:716
vpPose::DEMENTHON
@ DEMENTHON
Definition: vpPose.h:86
vpDisplay::displayCross
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
Definition: vpDisplay_uchar.cpp:180
vpImage< unsigned char >
vpDisplay::getClick
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
Definition: vpDisplay_uchar.cpp:765
vpRealSense::setStreamSettings
void setStreamSettings(const rs::stream &stream, const rs::preset &preset)
Definition: vpRealSense.cpp:607
vpHomogeneousMatrix
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition: vpHomogeneousMatrix.h:149
vpSimulatorCamera::setVelocity
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
Definition: vpSimulatorCamera.cpp:198
vpCameraParameters::perspectiveProjWithDistortion
@ perspectiveProjWithDistortion
Definition: vpCameraParameters.h:242
vpServo::PSEUDO_INVERSE
@ PSEUDO_INVERSE
Definition: vpServo.h:206
vpServoDisplay::display
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)
Definition: vpServoDisplay.cpp:80
vpException
error that can be emited by ViSP classes.
Definition: vpException.h:71
vpColor::red
static const vpColor red
Definition: vpColor.h:179
vpRealSense::setEnableStream
void setEnableStream(const rs::stream &stream, bool status)
Definition: vpRealSense.cpp:669
vpRealSense::vpRsStreamParams
Definition: vpRealSense.h:418