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);
406 std::cout <<
"Catch an exception: " << e.
getMessage() << std::endl;
414 std::cout <<
"You do not have an Viper 850 robot connected to your computer..." << std::endl;
Class for firewire ieee1394 video devices using libdc1394-2.x api.
@ vpVIDEO_MODE_640x480_MONO8
unsigned int getCols() const
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Class to define RGB colors available for display functionnalities.
static const vpColor black
static const vpColor cyan
static const vpColor orange
static const vpColor blue
static const vpColor green
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...
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.
void track(const vpImage< unsigned char > &I, bool canMakeTheWindowGrow=true)
vpImagePoint getCog() const
void initTracking(const vpImage< unsigned char > &I, unsigned int size=0)
error that can be emited by ViSP classes.
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 ...
Implementation of a matrix and operations on matrices.
unsigned int kernel(vpMatrix &kerAt, double svThreshold=1e-6) const
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void get_eJe(vpMatrix &eJe)
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
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)
void set_cVe(const vpVelocityTwistMatrix &cVe_)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
void set_eJe(const vpMatrix &eJe_)
void setServo(const vpServoType &servo_type)
vpMatrix getTaskJacobian() const
vpColVector computeControlLaw()
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
vpVelocityTwistMatrix get_cVe() const
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()