Visual Servoing Platform  version 3.5.1 under development (2022-12-02)
servoViper850FourPoints2DCamVelocityLs_cur.cpp

Example of eye-in-hand control law. We control here a real robot, the Viper S850 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.

/****************************************************************************
*
* 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 <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h> // Debug trace
#include <fstream>
#include <iostream>
#include <sstream>
#include <stdio.h>
#include <stdlib.h>
#if (defined(VISP_HAVE_VIPER850) && defined(VISP_HAVE_DC1394))
#include <visp3/blob/vpDot2.h>
#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpPoint.h>
#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/robot/vpRobotViper850.h>
#include <visp3/sensor/vp1394TwoGrabber.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(vpPoint point[], vpDot2 dot[], int ndot, vpCameraParameters cam, vpHomogeneousMatrix &cMo,
{
vpHomogeneousMatrix cMo_dementhon; // computed pose with dementhon
vpHomogeneousMatrix cMo_lagrange; // computed pose with dementhon
vpPose pose;
for (int i = 0; i < ndot; i++) {
double x = 0, y = 0;
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
// 'cMo'
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;
} else { // init = false; use of the previous pose to initialise LOWE
cRo.buildFrom(cro);
cMo.buildFrom(cto, cRo);
}
cMo.extract(cto);
cMo.extract(cRo);
cro.buildFrom(cRo);
}
int main()
{
// Log file creation in /tmp/$USERNAME/log.dat
// This file contains by line:
// - the 6 computed joint 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 obtained
// using a camera intrinsic model with distortion
vpServo task;
int i;
bool reset = false;
vp1394TwoGrabber g(reset);
g.open(I);
g.acquire(I);
#ifdef VISP_HAVE_X11
vpDisplayX display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_OPENCV)
vpDisplayOpenCV display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_GTK)
vpDisplayGTK display(I, 100, 100, "Current image");
#endif
std::cout << std::endl;
std::cout << "-------------------------------------------------------" << std::endl;
std::cout << " Test program for vpServo " << std::endl;
std::cout << " Eye-in-hand task control, velocity computed in the camera space" << std::endl;
std::cout << " Use of the Viper850 robot " << std::endl;
std::cout << " task : servo 4 points on a square with dimention " << L << " meters" << std::endl;
std::cout << "-------------------------------------------------------" << std::endl;
std::cout << std::endl;
vpDot2 dot[4];
std::cout << "Click on the 4 dots clockwise starting from upper/left dot..." << std::endl;
for (i = 0; i < 4; i++) {
dot[i].setGraphics(true);
dot[i].initTracking(I);
cog = dot[i].getCog();
}
// Update camera parameters
robot.getCameraParameters(cam, I);
// Sets the current position of the visual feature
for (i = 0; i < 4; 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
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);
// Initialise a desired pose to compute s*, the desired 2D point features
vpTranslationVector cto(0, 0, 0.5); // tz = 0.5 meter
vpRotationMatrix cRo(cro); // Build the rotation matrix
cMo.buildFrom(cto, cRo); // Build the homogeneous matrix
// 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, 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 (i = 0; i < 4; i++)
task.addFeature(p[i], pd[i]);
// Set the proportional gain
task.setLambda(0.3);
// Display task information
task.print();
// Define the task
// - we want an eye-in-hand control law
// - articular velocity are computed
task.print();
// Initialise the velocity control of the robot
std::cout << "\nHit CTRL-C 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 (i = 0; i < 4; 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
cog = dot[i].getCog();
}
} catch (...) {
flog.close(); // Close the log file
vpTRACE("Error detected while tracking visual features");
robot.stopMotion();
return (1);
}
// During the servo, we compute the pose using LOWE method. For the
// initial pose used in the non linear minimisation we use the pose
// computed at the previous iteration.
compute_pose(point, dot, 4, cam, cMo, cto, cro, false);
for (i = 0; i < 4; 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
v = task.computeControlLaw();
// 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 joint translation velocities in m/s
// v[3], v[4], v[5] correspond to joint 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
// 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
robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
// 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;
// Flush the display
// std::cout << "|| s - s* || = " << ( task.getError() ).sumSquare() <<
// std::endl;
}
std::cout << "Display task information: " << std::endl;
task.print();
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 850 robot connected to your computer..." << std::endl;
return EXIT_SUCCESS;
}
#endif
Class for firewire ieee1394 video devices using libdc1394-2.x api.
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
static const vpColor blue
Definition: vpColor.h:223
static const vpColor green
Definition: vpColor.h:220
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:135
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
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 void display(const vpImage< unsigned char > &I)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
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 track(const vpImage< unsigned char > &I, bool canMakeTheWindowGrow=true)
Definition: vpDot2.cpp:442
void setGraphics(bool activate)
Definition: vpDot2.h:314
vpImagePoint getCog() const
Definition: vpDot2.h:180
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
const char * getMessage() const
Definition: vpException.cpp:87
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void set_y(double y)
void set_x(double x)
void set_Z(double Z)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void extract(vpRotationMatrix &R) const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:89
void init(unsigned int h, unsigned int w, Type value)
Definition: vpImage.h:631
static bool checkDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:431
static std::string getUserName()
Definition: vpIoTools.cpp:327
static void makeDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:581
static double rad(double deg)
Definition: vpMath.h:117
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
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 set_x(double x)
Set the point x coordinate in the image plane.
Definition: vpPoint.cpp:511
void projection(const vpColVector &_cP, vpColVector &_p) const
Definition: vpPoint.cpp:222
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
Definition: vpPoint.cpp:239
void setWorldCoordinates(double oX, double oY, double oZ)
Definition: vpPoint.cpp:113
void set_y(double y)
Set the point y coordinate in the image plane.
Definition: vpPoint.cpp:513
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:90
void addPoint(const vpPoint &P)
Definition: vpPose.cpp:148
@ DEMENTHON
Definition: vpPose.h:95
@ LAGRANGE
Definition: vpPose.h:94
@ LOWE
Definition: vpPose.h:96
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:335
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition: vpPose.cpp:373
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
@ ARTICULAR_FRAME
Definition: vpRobot.h:79
@ CAMERA_FRAME
Definition: vpRobot.h:83
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:67
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
Implementation of a rotation matrix and operations on such kind of matrices.
vpRotationMatrix buildFrom(const vpHomogeneousMatrix &M)
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:184
vpRxyzVector buildFrom(const vpRotationMatrix &R)
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)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:564
@ EYEINHAND_CAMERA
Definition: vpServo.h:155
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 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
@ CURRENT
Definition: vpServo.h:182
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:487
Class that consider the case of a translation vector.
@ TOOL_PTGREY_FLEA2_CAMERA
Definition: vpViper850.h:130
#define vpTRACE
Definition: vpDebug.h:416