 |
Visual Servoing Platform
version 3.2.0
|
51 #include <visp3/core/vpConfig.h>
52 #include <visp3/core/vpDebug.h>
62 #if (defined(VISP_HAVE_VIPER850) && defined(VISP_HAVE_DC1394) && defined(VISP_HAVE_DISPLAY))
64 #include <visp3/blob/vpDot2.h>
65 #include <visp3/core/vpDisplay.h>
66 #include <visp3/core/vpException.h>
67 #include <visp3/core/vpHomogeneousMatrix.h>
68 #include <visp3/core/vpImage.h>
69 #include <visp3/core/vpIoTools.h>
70 #include <visp3/core/vpMath.h>
71 #include <visp3/core/vpPoint.h>
72 #include <visp3/gui/vpDisplayGTK.h>
73 #include <visp3/gui/vpDisplayOpenCV.h>
74 #include <visp3/gui/vpDisplayX.h>
75 #include <visp3/gui/vpPlot.h>
76 #include <visp3/robot/vpRobotViper850.h>
77 #include <visp3/sensor/vp1394TwoGrabber.h>
78 #include <visp3/visual_features/vpFeatureBuilder.h>
79 #include <visp3/visual_features/vpFeaturePoint.h>
80 #include <visp3/vs/vpServo.h>
81 #include <visp3/vs/vpServoDisplay.h>
100 double Tloop = 1. / 60.f;
120 std::cout <<
"Tloop: " << Tloop << std::endl;
123 vpDisplayX display(I, 800, 100,
"Current image");
124 #elif defined(VISP_HAVE_OPENCV)
126 #elif defined(VISP_HAVE_GTK)
134 jointMin = robot.getJointMin();
135 jointMax = robot.getJointMax();
143 for (
unsigned int i = 0; i < 6; i++) {
144 Qmin[i] = jointMin[i] + 0.5 * rho * (jointMax[i] - jointMin[i]);
145 Qmax[i] = jointMax[i] - 0.5 * rho * (jointMax[i] - jointMin[i]);
147 Qmiddle = (Qmin + Qmax) / 2.;
150 for (
unsigned int i = 0; i < 6; i++) {
151 tQmin[i] = Qmin[i] + 0.5 * (rho1) * (Qmax[i] - Qmin[i]);
152 tQmax[i] = Qmax[i] - 0.5 * (rho1) * (Qmax[i] - Qmin[i]);
164 plot.initGraph(0, 10);
165 plot.initGraph(1, 6);
172 plot.initRange(0, 0, 200, 1, -1.2, 1.2, 0.1);
173 plot.setTitle(0,
"Joint behavior");
174 plot.initRange(1, 0, 200, 1, -0.01, 0.01, 0.05);
175 plot.setTitle(1,
"Joint velocity");
179 for (
unsigned int i = 0; i < 6; i++) {
180 sprintf(legend,
"q%u", i + 1);
181 plot.setLegend(0, i, legend);
182 sprintf(legend,
"q%u", i + 1);
183 plot.setLegend(1, i, legend);
185 plot.setLegend(0, 6,
"tQmin");
186 plot.setLegend(0, 7,
"tQmax");
187 plot.setLegend(0, 8,
"Qmin");
188 plot.setLegend(0, 9,
"Qmax");
195 plot.setColor(0, 4,
vpColor(0, 128, 0));
197 for (
unsigned int i = 6; i < 10; i++)
205 plot.setColor(1, 4,
vpColor(0, 128, 0));
209 std::cout <<
"Click on a dot..." << std::endl;
217 robot.getCameraParameters(cam, I);
236 std::cout << cVe << std::endl;
245 std::cout << std::endl;
262 std::cout <<
"\nHit CTRL-C to stop the loop...\n" << std::flush;
269 double Tv = (double)(t_0 - t_1) / 1000.0;
270 std::cout <<
"Tv: " << Tv << std::endl;
276 dc1394video_frame_t *frame = g.dequeue(I);
309 qpre += -lambda * prim_task * (4 * Tloop);
314 unsigned int npb = 0;
315 for (
unsigned int i = 0; i < 6; i++) {
317 if (fabs(Qmin[i] - q[i]) > fabs(Qmin[i] - qpre[i])) {
320 std::cout <<
"Joint " << i <<
" near limit " << std::endl;
322 if (q[i] > tQmax[i]) {
323 if (fabs(Qmax[i] - q[i]) > fabs(Qmax[i] - qpre[i])) {
326 std::cout <<
"Joint " << i <<
" near limit " << std::endl;
336 unsigned int dimKernelL = kernelJ1.
getCols();
344 for (
unsigned int j = 0; j < 6; j++)
346 if (std::fabs(pb[j] - 1) <= std::numeric_limits<double>::epsilon()) {
347 for (
unsigned int i = 0; i < dimKernelL; i++)
348 E[k][i] = kernelJ1[j][i];
350 S[k] = -prim_task[j];
355 Ep = E.
t() * (E * E.t()).pseudoInverse();
358 e2 = (kernelJ1 * a0);
366 v = -lambda * (prim_task + e2);
378 for (
unsigned int i = 0; i < 6; i++) {
379 data[i] = (q[i] - Qmiddle[i]);
380 data[i] /= (Qmax[i] - Qmin[i]);
383 unsigned int joint = 2;
384 data[6] = 2 * (tQmin[joint] - Qmiddle[joint]) / (Qmax[joint] - Qmin[joint]);
385 data[7] = 2 * (tQmax[joint] - Qmiddle[joint]) / (Qmax[joint] - Qmin[joint]);
389 plot.plot(0, iter, data);
390 plot.plot(1, iter, v);
407 std::cout <<
"Catch an exception: " << e.
getMessage() << std::endl;
415 std::cout <<
"You do not have an Viper 850 robot connected to your computer..." << std::endl;
Initialize the velocity controller.
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
vpImagePoint getCog() const
VISP_EXPORT int wait(double t0, double t)
static const vpColor orange
void get_cVe(vpVelocityTwistMatrix &cVe) const
void buildFrom(const double x, const double y, const double Z)
Generic class defining intrinsic camera parameters.
void track(const vpImage< unsigned char > &I)
Control of Irisa's Viper S850 robot named Viper850.
void set_eJe(const vpMatrix &eJe_)
This tracker is meant to track a blob (connex pixels with same gray level) on a vpImage.
vpMatrix getTaskJacobian() const
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void set_cVe(const vpVelocityTwistMatrix &cVe_)
void set_Z(const double Z)
Implementation of column vector and the associated operations.
static const vpColor cyan
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Implementation of a matrix and operations on matrices.
void setServo(const vpServoType &servo_type)
VISP_EXPORT double measureTimeMs()
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
unsigned int getCols() const
vpHomogeneousMatrix getPosition() const
static const vpColor green
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
static void display(const vpImage< unsigned char > &I)
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
unsigned int kernel(vpMatrix &kerAt, double svThreshold=1e-6) const
static const vpColor black
void initTracking(const vpImage< unsigned char > &I, unsigned int size=0)
Class for firewire ieee1394 video devices using libdc1394-2.x api.
void get_eJe(vpMatrix &eJe)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
const char * getMessage(void) const
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
static const vpColor blue
vpColVector computeControlLaw()
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
static void flush(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)
Class to define colors available for display functionnalities.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
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)
error that can be emited by ViSP classes.
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...