Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoViper850Point2DArtVelocity-jointAvoidance-gpa.cpp

Joint limits avoidance using a gradient projection approach.

Implemented from [Marchand96f] and section II.B in [Chaumette01c].

/****************************************************************************
*
* 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 articular
*
*****************************************************************************/
#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) && defined(VISP_HAVE_DISPLAY))
#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/vpMath.h>
#include <visp3/core/vpPoint.h>
#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/gui/vpPlot.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/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
int main()
{
try {
vpServo task;
bool reset = false;
vp1394TwoGrabber g(reset);
g.open(I);
g.acquire(I);
#ifdef VISP_HAVE_X11
vpDisplayX display(I, 800, 100, "Current image");
#elif defined(HAVE_OPENCV_HIGHGUI)
vpDisplayOpenCV display(I, 800, 100, "Current image");
#elif defined(VISP_HAVE_GTK)
vpDisplayGTK display(I, 800, 100, "Current image");
#endif
vpColVector jointMin(6), jointMax(6);
jointMin = robot.getJointMin();
jointMax = robot.getJointMax();
vpColVector Qmin(6), tQmin(6);
vpColVector Qmax(6), tQmax(6);
vpColVector Qmiddle(6);
vpColVector data(10);
double rho = 0.15;
for (unsigned int i = 0; i < 6; i++) {
Qmin[i] = jointMin[i] + 0.5 * rho * (jointMax[i] - jointMin[i]);
Qmax[i] = jointMax[i] - 0.5 * rho * (jointMax[i] - jointMin[i]);
}
Qmiddle = (Qmin + Qmax) / 2.;
double rho1 = 0.1;
for (unsigned int i = 0; i < 6; i++) {
tQmin[i] = Qmin[i] + 0.5 * (rho1) * (Qmax[i] - Qmin[i]);
tQmax[i] = Qmax[i] - 0.5 * (rho1) * (Qmax[i] - Qmin[i]);
}
// Create a window with two graphics
// - first graphic to plot q(t), Qmin, Qmax, tQmin and tQmax
// - second graphic to plot the cost function h_s
vpPlot plot(2);
// The first graphic contains 10 data to plot: q(t), Qmin, Qmax, tQmin and
// tQmax
plot.initGraph(0, 10);
// The second graphic contains 1 curve, the cost function h_s
plot.initGraph(1, 1);
// For the first graphic :
// - along the x axis the expected values are between 0 and 200
// - along the y axis the expected values are between -1.2 and 1.2
plot.initRange(0, 0., 200., -1.2, 1.2);
plot.setTitle(0, "Joint behavior");
// For the second graphic :
// - along the x axis the expected values are between 0 and 200 and
// the step is 1
// - along the y axis the expected values are between 0 and 0.0001 and the
// step is 0.00001
plot.initRange(1, 0., 200., 0., 1e-4);
plot.setTitle(1, "Cost function");
// For the first graphic, set the curves legend
std::string legend;
for (unsigned int i = 0; i < 6; i++) {
legend = "q" + i + 1;
plot.setLegend(0, i, legend);
}
plot.setLegend(0, 6, "tQmin");
plot.setLegend(0, 7, "tQmax");
plot.setLegend(0, 8, "Qmin");
plot.setLegend(0, 9, "Qmax");
// Set the curves color
plot.setColor(0, 0, vpColor::red);
plot.setColor(0, 1, vpColor::green);
plot.setColor(0, 2, vpColor::blue);
plot.setColor(0, 4, vpColor(0, 128, 0));
plot.setColor(0, 5, vpColor::cyan);
for (unsigned int i = 6; i < 10; i++)
plot.setColor(0, i, vpColor::black); // for Q and tQ [min,max]
// For the second graphic, set the curves legend
plot.setLegend(1, 0, "h_s");
double beta = 1;
// Set the amplitude of the control law due to the secondary task
std::cout << " Give the parameters beta (1) : ";
std::cin >> beta;
vpDot2 dot;
std::cout << "Click on a dot..." << std::endl;
dot.initTracking(I);
vpImagePoint cog = dot.getCog();
// Update camera parameters
robot.getCameraParameters(cam, I);
// sets the current position of the visual feature
vpFeatureBuilder::create(p, cam, dot); // retrieve x,y and Z of the vpPoint structure
p.set_Z(1);
// sets the desired position of the visual feature
pd.buildFrom(0, 0, 1);
// Define the task
// - we want an eye-in-hand control law
// - articular velocity are computed
robot.get_cVe(cVe);
std::cout << cVe << std::endl;
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..") ;
std::cout << std::endl;
task.addFeature(p, pd);
// - set the gain
task.setLambda(0.8);
// Display task information " ) ;
task.print();
int iter = 0;
std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
for (;;) {
iter++;
// Acquire a new image from the camera
g.acquire(I);
// Display this image
// Achieve the tracking of the dot in the image
dot.track(I);
cog = dot.getCog();
// Display a green cross at the center of gravity position in the image
// Get the measured joint positions of the robot
robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
// Update the point feature from the dot location
// Get the jacobian of the robot
robot.get_eJe(eJe);
// Update this jacobian in the task structure. It will be used to
// compute the velocity skew (as an articular velocity) qdot = -lambda *
// L^+ * cVe * eJe * (s-s*)
task.set_eJe(eJe);
vpColVector prim_task;
vpColVector e2(6);
// Compute the visual servoing skew vector
prim_task = task.computeControlLaw();
vpColVector sec_task(6);
double h_s = 0;
{
// joint limit avoidance with secondary task
vpColVector de2dt(6);
de2dt = 0;
e2 = 0;
for (unsigned int i = 0; i < 6; i++) {
double S = 0;
if (q[i] > tQmax[i])
S = q[i] - tQmax[i];
if (q[i] < tQmin[i])
S = q[i] - tQmin[i];
double D = (Qmax[i] - Qmin[i]);
h_s += vpMath::sqr(S) / D;
e2[i] = S / D;
}
h_s = beta * h_s / 2.0; // cost function
e2 *= beta;
std::cout << "Cost function h_s: " << h_s << std::endl;
sec_task = task.secondaryTask(e2, de2dt);
}
v = prim_task + sec_task;
// Display the current and desired feature points in the image display
vpServoDisplay::display(task, cam, I);
// Apply the computed joint velocities to the robot
{
// Add the material to plot curves
// q normalized between (entre -1 et 1)
for (unsigned int i = 0; i < 6; i++) {
data[i] = (q[i] - Qmiddle[i]);
data[i] /= (Qmax[i] - Qmin[i]);
data[i] *= 2;
}
unsigned int joint = 2;
data[6] = 2 * (tQmin[joint] - Qmiddle[joint]) / (Qmax[joint] - Qmin[joint]);
data[7] = 2 * (tQmax[joint] - Qmiddle[joint]) / (Qmax[joint] - Qmin[joint]);
data[8] = -1;
data[9] = 1;
plot.plot(0, iter, data); // plot q, Qmin, Qmax, tQmin, tQmax
plot.plot(1, 0, iter, h_s); // plot the cost function
}
}
// Display task information
task.print();
return EXIT_SUCCESS;
} catch (const vpException &e) {
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 acquire(vpImage< unsigned char > &I)
void setVideoMode(vp1394TwoVideoModeType videomode)
void setFramerate(vp1394TwoFramerateType fps)
void open(vpImage< unsigned char > &I)
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Class to define RGB colors available for display functionalities.
Definition vpColor.h:152
static const vpColor red
Definition vpColor.h:211
static const vpColor black
Definition vpColor.h:205
static const vpColor cyan
Definition vpColor.h:220
static const vpColor orange
Definition vpColor.h:221
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
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)
void set_Z(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
static double sqr(double x)
Definition vpMath.h:124
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Definition vpPlot.h:113
void initGraph(unsigned int graphNum, unsigned int curveNbr)
Definition vpPlot.cpp:202
void initRange(unsigned int graphNum, double xmin, double xmax, double ymin, double ymax)
Definition vpPlot.cpp:214
void setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
Definition vpPlot.cpp:545
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
Definition vpPlot.cpp:269
void setColor(unsigned int graphNum, unsigned int curveNum, vpColor color)
Definition vpPlot.cpp:245
void setTitle(unsigned int graphNum, const std::string &title)
Definition vpPlot.cpp:503
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void get_eJe(vpMatrix &eJe)
@ ARTICULAR_FRAME
Definition vpRobot.h:76
@ 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_L_cVe_eJe
Definition vpServo.h:155
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition vpServo.h:448
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 set_eJe(const vpMatrix &eJe_)
Definition vpServo.h:506
vpColVector secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator=false)
Definition vpServo.cpp:1454
void setServo(const vpServoType &servo_type)
Definition vpServo.cpp:210
@ PSEUDO_INVERSE
Definition vpServo.h:199
vpColVector computeControlLaw()
Definition vpServo.cpp:930
@ DESIRED
Definition vpServo.h:183
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition vpServo.cpp:487
vpVelocityTwistMatrix get_cVe() const
Definition vpUnicycle.h:79