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

Example of robot pose usage.

Show how to compute rMo = rMc * cMo with cMo obtained by pose computation and rMc from the robot position.

/****************************************************************************
*
* 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:
* Test for Afma 6 dof robot.
*
*****************************************************************************/
#include <iostream>
#include <visp3/blob/vpDot.h>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpPixelMeterConversion.h>
#include <visp3/core/vpPoint.h>
#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/robot/vpRobotAfma6.h>
#include <visp3/sensor/vp1394TwoGrabber.h>
#include <visp3/vision/vpPose.h>
#if defined(VISP_HAVE_AFMA6) && defined(VISP_HAVE_DC1394)
int main()
{
try {
// Create an image B&W container
// Create a firewire grabber based on libdc1394-2.x
// Grab an image from the firewire camera
g.acquire(I);
// Create an image viewer for the image
#ifdef VISP_HAVE_X11
vpDisplayX display(I, 100, 100, "Current image");
#elif defined(HAVE_OPENCV_HIGHGUI)
vpDisplayOpenCV display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_GTK)
vpDisplayGTK display(I, 100, 100, "Current image");
#endif
// Display the image
// Define a squared target
// The target is made of 4 planar points (square dim = 0.077m)
double sdim = 0.077; // square width and height
vpPoint target[4];
// Set the point world coordinates (x,y,z) in the object frame
// o ----> x
// |
// |
// \/
// y
target[0].setWorldCoordinates(-sdim / 2., -sdim / 2., 0);
target[1].setWorldCoordinates(sdim / 2., -sdim / 2., 0);
target[2].setWorldCoordinates(sdim / 2., sdim / 2., 0);
target[3].setWorldCoordinates(-sdim / 2., sdim / 2., 0);
// Image processing to extract the 2D coordinates in sub-pixels of the 4
// points from the image acquired by the camera
// Creation of 4 trackers
vpDot dot[4];
for (int i = 0; i < 4; i++) {
dot[i].setGraphics(true); // to display the tracking results
std::cout << "Click on dot " << i << std::endl;
dot[i].initTracking(I);
// The tracker computes the sub-pixels coordinates in the image
// i ----> u
// |
// |
// \/
// v
std::cout << " Coordinates: " << dot[i].getCog() << std::endl;
// Flush the tracking results in the viewer
}
// Create an intrinsic camera parameters structure
// Create a robot access
vpRobotAfma6 robot;
// Load the end-effector to camera frame transformation obtained
// using a camera intrinsic model with distortion
// Get the intrinsic camera parameters associated to the image
robot.getCameraParameters(cam, I);
// Using the camera parameters, compute the perspective projection
// (transform the dot sub-pixel coordinates into coordinates in the camera
// frame in meter)
for (int i = 0; i < 4; i++) {
double x = 0, y = 0; // coordinates of the dots in the camera frame
// c ----> x
// |
// |
// \/
// y
// pixel to meter conversion
cog = dot[i].getCog();
target[i].set_x(x);
target[i].set_y(y);
}
// From now, in target[i], we have the 3D coordinates of a point in the
// object frame, and their correspondances in the camera frame. We can now
// compute the pose cMo between the camera and the object.
vpPose pose;
// Add the 4 points to compute the pose
for (int i = 0; i < 4; i++) {
pose.addPoint(target[i]);
}
// Create an homogeneous matrix for the camera to object transformation
// computed just bellow
// Compute the pose: initialisation is done by Dementhon or Lagrange method, and the
// final pose is computed by the more accurate Virtual Visual Servoing method.
std::cout << "Pose cMo: " << std::endl << cMo;
cMo.extract(R);
r.buildFrom(R);
std::cout << " rotation: " << vpMath::deg(r[0]) << " " << vpMath::deg(r[1]) << " " << vpMath::deg(r[2]) << " deg"
<< std::endl
<< std::endl;
// Get the robot position in the reference frame
vpColVector p; // position x,y,z,rx,ry,rz
robot.getPosition(vpRobotAfma6::REFERENCE_FRAME, p);
std::cout << "Robot pose in reference frame: " << p << std::endl;
t[0] = p[0];
t[1] = p[1];
t[2] = p[2];
r[0] = p[3];
r[1] = p[4];
r[2] = p[5];
R.buildFrom(r);
rMc.buildFrom(t, R);
std::cout << "Pose rMc: " << std::endl << rMc;
rMc.extract(R);
r.buildFrom(R);
std::cout << " rotation: " << vpMath::deg(r[0]) << " " << vpMath::deg(r[1]) << " " << vpMath::deg(r[2]) << " deg"
<< std::endl
<< std::endl;
robot.getPosition(vpRobotAfma6::ARTICULAR_FRAME, p);
std::cout << "Robot pose in articular: " << p << std::endl;
robot.get_fMc(p, rMc);
std::cout << "Pose rMc from MGD: " << std::endl << rMc;
rMc.extract(R);
r.buildFrom(R);
std::cout << " rotation: " << vpMath::deg(r[0]) << " " << vpMath::deg(r[1]) << " " << vpMath::deg(r[2]) << " deg"
<< std::endl
<< std::endl;
rMo = rMc * cMo;
std::cout << "Pose rMo = rMc * cMo: " << std::endl << rMo;
rMo.extract(R);
r.buildFrom(R);
std::cout << " rotation: " << vpMath::deg(r[0]) << " " << vpMath::deg(r[1]) << " " << vpMath::deg(r[2]) << " deg"
<< std::endl
<< std::endl;
return EXIT_SUCCESS;
} catch (const vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return EXIT_FAILURE;
}
}
#else
int main()
{
std::cout << "Sorry, test not valid. You should have an Afma6 robot..." << std::endl;
return EXIT_SUCCESS;
}
#endif
Class for firewire ieee1394 video devices using libdc1394-2.x api.
void acquire(vpImage< unsigned char > &I)
@ TOOL_CCMOP
Definition vpAfma6.h:124
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithDistortion
Perspective projection with distortion model.
Implementation of column vector and the associated operations.
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 flush(const vpImage< unsigned char > &I)
This tracker is meant to track a dot (connected pixels with same gray level) on a vpImage.
Definition vpDot.h:112
void initTracking(const vpImage< unsigned char > &I)
Definition vpDot.cpp:617
void setGraphics(bool activate)
Definition vpDot.h:357
vpImagePoint getCog() const
Definition vpDot.h:243
error that can be emitted by ViSP classes.
Definition vpException.h:59
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 of the vpImage class member functions.
Definition vpImage.h:135
static double deg(double rad)
Definition vpMath.h:106
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:77
void set_x(double x)
Set the point x coordinate in the image plane.
Definition vpPoint.cpp:508
void setWorldCoordinates(double oX, double oY, double oZ)
Definition vpPoint.cpp:110
void set_y(double y)
Set the point y coordinate in the image plane.
Definition vpPoint.cpp:510
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition vpPose.h:81
void addPoint(const vpPoint &P)
Definition vpPose.cpp:140
@ DEMENTHON_LAGRANGE_VIRTUAL_VS
Definition vpPose.h:102
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition vpPose.cpp:469
Control of Irisa's gantry robot named Afma6.
@ REFERENCE_FRAME
Definition vpRobot.h:74
@ ARTICULAR_FRAME
Definition vpRobot.h:76
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.
vpRxyzVector buildFrom(const vpRotationMatrix &R)
Class that consider the case of a translation vector.