Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoViper850Point2DCamVelocityKalman.cpp

Example of eye-in-hand control law. We control here a real robot, the ADEPT Viper 850 robot (arm, with 6 degrees of freedom). The velocity is computed in the camera frame. The visual feature is the center of gravity of a point. We use here a linear Kalman filter with a constant velocity state model to estimate the moving target motion.

/****************************************************************************
*
* ViSP, open source Visual Servoing Platform software.
* Copyright (C) 2005 - 2023 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 https://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 camera frame
*
*****************************************************************************/
#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/vpException.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpLinearKalmanFilterInstantiation.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/io/vpImageIo.h>
#include <visp3/robot/vpRobotViper850.h>
#include <visp3/sensor/vp1394TwoGrabber.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/vs/vpAdaptiveGain.h>
#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
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 2 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 EXIT_FAILURE;
}
}
std::string logfilename;
logfilename = logdirname + "/log.dat";
// Open the log file name
std::ofstream flog(logfilename.c_str());
vpServo task;
try {
// Initialize linear Kalman filter
// Initialize the kalman filter
unsigned int nsignal = 2; // The two values of dedt
double rho = 0.3;
vpColVector sigma_state;
vpColVector sigma_measure(nsignal);
unsigned int state_size = 0; // Kalman state vector size
state_size = kalman.getStateSize();
sigma_state.resize(state_size * nsignal);
sigma_state = 0.00001; // Same state variance for all signals
sigma_measure = 0.05; // Same measure variance for all the signals
double dummy = 0; // non used parameter dt for the velocity state model
kalman.initFilter(nsignal, sigma_state, sigma_measure, rho, dummy);
// Initialize the robot
bool reset = false;
vp1394TwoGrabber g(reset);
#if 1
#else
#endif
g.open(I);
double Tloop = 1. / 80.f;
g.getFramerate(fps);
switch (fps) {
Tloop = 1.f / 15.f;
break;
Tloop = 1.f / 30.f;
break;
Tloop = 1.f / 60.f;
break;
Tloop = 1.f / 120.f;
break;
default:
break;
}
#ifdef VISP_HAVE_X11
vpDisplayX display(I, (int)(100 + I.getWidth() + 30), 200, "Current image");
#elif defined(HAVE_OPENCV_HIGHGUI)
vpDisplayOpenCV display(I, (int)(100 + I.getWidth() + 30), 200, "Current image");
#elif defined(VISP_HAVE_GTK)
vpDisplayGTK display(I, (int)(100 + I.getWidth() + 30), 200, "Current image");
#endif
vpDot2 dot;
dot.setGraphics(true);
for (int i = 0; i < 10; i++)
g.acquire(I);
std::cout << "Click on a dot..." << std::endl;
dot.initTracking(I);
cog = dot.getCog();
// Update camera parameters
robot.getCameraParameters(cam, I);
// sets the current position of the visual feature
// retrieve x,y and Z of the vpPoint structure
// sets the desired position of the visual feature
pd.buildFrom(0, 0, 1);
// define the task
// - we want an eye-in-hand control law
// - robot is controlled in the camera frame
// - we want to see a point on a point
task.addFeature(p, pd);
// - set the constant gain
lambda.initStandard(4, 0.2, 30);
task.setLambda(lambda);
// Display task information
task.print();
// Now the robot will be controlled in velocity
std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
vpColVector v, v1, v2;
int iter = 0;
vpColVector vm(6);
double t_0, t_1, Tv;
vpColVector err(2), err_1(2);
vpColVector dedt_filt(2), dedt_mes(2);
dc1394video_frame_t *frame = NULL;
for (;;) {
try {
t_0 = vpTime::measureTimeMs(); // t_0: current time
// Update loop time in second
Tv = (double)(t_0 - t_1) / 1000.0;
// Update time for next iteration
t_1 = t_0;
// Acquire a new image from the camera
frame = g.dequeue(I);
// Display this image
// Achieve the tracking of the dot in the image
dot.track(I);
// Get the dot cog
cog = dot.getCog();
// Display a green cross at the center of gravity position in the
// image
// Update the point feature from the dot location
// Compute the visual servoing skew vector
v1 = task.computeControlLaw();
// Get the error ||s-s*||
err = task.getError();
if (iter == 0) {
err_1 = 0;
dedt_mes = 0;
} else {
dedt_mes = (err - err_1) / (Tv)-J1 * vm;
err_1 = err;
}
// Filter de/dt
if (iter < 2)
dedt_mes = 0;
kalman.filter(dedt_mes);
// Get the filtered values
for (unsigned int i = 0; i < nsignal; i++) {
dedt_filt[i] = kalman.Xest[i * state_size];
}
if (iter < 2)
dedt_filt = 0;
v2 = -J1p * dedt_filt;
// Update the robot camera velocity
v = v1 + v2;
// Display the current and desired feature points in the image display
vpServoDisplay::display(task, cam, I);
// Apply the computed camera velocities to the robot
iter++;
// Synchronize the loop with the image frame rate
vpTime::wait(t_0, 1000. * Tloop);
// Release the ring buffer used for the last image to start a new acq
g.enqueue(frame);
} catch (...) {
std::cout << "Tracking failed... Stop the robot." << std::endl;
v = 0;
// Stop robot
return EXIT_FAILURE;
}
// 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
// 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 feature point. For this feature
// point, we have 2 errors (along x and y axis). This error is
// expressed in meters in the camera frame
flog << (task.getError()).t() << std::endl; // s-s* for point
// Flush the display
}
flog.close(); // Close the log file
// Display task information
task.print();
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.
void getFramerate(vp1394TwoFramerateType &fps)
void acquire(vpImage< unsigned char > &I)
void setColorCoding(vp1394TwoColorCodingType coding)
void setVideoMode(vp1394TwoVideoModeType videomode)
void enqueue(dc1394video_frame_t *frame)
void setFramerate(vp1394TwoFramerateType fps)
dc1394video_frame_t * dequeue()
void open(vpImage< unsigned char > &I)
Adaptive gain computation.
void initStandard(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
static const vpColor blue
Definition vpColor.h:217
static const vpColor green
Definition vpColor.h:214
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
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:132
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:124
void track(const vpImage< unsigned char > &I, bool canMakeTheWindowGrow=true)
Definition vpDot2.cpp:441
void setGraphics(bool activate)
Definition vpDot2.h:311
vpImagePoint getCog() const
Definition vpDot2.h:177
void initTracking(const vpImage< unsigned char > &I, unsigned int size=0)
Definition vpDot2.cpp:252
error that can be emitted by ViSP classes.
Definition vpException.h:59
const char * getMessage() const
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 buildFrom(double x, double y, double Z)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
static bool checkDirectory(const std::string &dirname)
static std::string getUserName()
static void makeDirectory(const std::string &dirname)
vpColVector Xest
unsigned int getStateSize()
This class provides an implementation of some specific linear Kalman filters.
void initFilter(unsigned int nsignal, vpColVector &sigma_state, vpColVector &sigma_measure, double rho, double dt)
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
@ ARTICULAR_FRAME
Definition vpRobot.h:76
@ CAMERA_FRAME
Definition vpRobot.h:80
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition vpRobot.h:64
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition vpRobot.cpp:198
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:151
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition vpServo.cpp:299
void setLambda(double c)
Definition vpServo.h:403
void setServo(const vpServoType &servo_type)
Definition vpServo.cpp:210
vpMatrix getTaskJacobian() const
Definition vpServo.cpp:1765
vpColVector getError() const
Definition vpServo.h:276
@ PSEUDO_INVERSE
Definition vpServo.h:199
vpColVector computeControlLaw()
Definition vpServo.cpp:930
vpMatrix getTaskJacobianPseudoInverse() const
Definition vpServo.cpp:1785
@ DESIRED
Definition vpServo.h:183
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition vpServo.cpp:487
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()