Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
manServoMomentsSimple.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See https://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Example of visual servoing with moments using a polygon as object container
33 *
34*****************************************************************************/
35
42#include <visp3/core/vpPoint.h> //the basic tracker
43
44#include <iostream> //some console output
45#include <limits>
46#include <vector> //store the polygon
47#include <visp3/core/vpException.h>
48#include <visp3/core/vpMomentCommon.h> //update the common database with the object
49#include <visp3/core/vpMomentObject.h> //transmit the polygon to the object
50#include <visp3/core/vpPlane.h>
51#include <visp3/robot/vpSimulatorCamera.h>
52#include <visp3/visual_features/vpFeatureMomentCommon.h> //init the feature database using the information about moment dependencies
53#include <visp3/vs/vpServo.h> //visual servoing task
54// this function converts the plane defined by the cMo to 1/Z=Ax+By+C plane
55// form
56
57void cMoToABC(vpHomogeneousMatrix &cMo, double &A, double &B, double &C);
58
59void cMoToABC(vpHomogeneousMatrix &cMo, double &A, double &B, double &C)
60{
61 vpPlane pl;
62 pl.setABCD(0, 0, 1.0, 0);
63 pl.changeFrame(cMo);
64
65 if (fabs(pl.getD()) < std::numeric_limits<double>::epsilon()) {
66 std::cout << "Invalid position:" << std::endl;
67 std::cout << cMo << std::endl;
68 std::cout << "Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
69 throw vpException(vpException::divideByZeroError, "invalid position!");
70 }
71 A = -pl.getA() / pl.getD();
72 B = -pl.getB() / pl.getD();
73 C = -pl.getC() / pl.getD();
74}
75
76int main()
77{
78 try {
79 double x[8] = { 1, 3, 4, -1, -3, -2, -1, 1 };
80 double y[8] = { 0, 1, 4, 4, -2, -2, 1, 0 };
81 double A, B, C, Ad, Bd, Cd;
82
83 int nbpoints = 8;
84 std::vector<vpPoint> vec_p,
85 vec_p_d; // vectors that contain the vertices of the contour polygon
86
87 vpHomogeneousMatrix cMo(0.1, 0.0, 1.0, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
89 vpHomogeneousMatrix wMo; // Set to identity
90 vpHomogeneousMatrix wMc; // Camera position in the world frame
91
92 cMoToABC(cMo, A, B, C);
93 cMoToABC(cdMo, Ad, Bd, Cd);
94 // Define source and destination polygons
95 for (int i = 0; i < nbpoints; i++) {
96 vpPoint p(x[i], y[i], 0.0);
97 p.track(cMo);
98 vec_p.push_back(p);
99 p.track(cdMo);
100 vec_p_d.push_back(p);
101 }
102
103 vpMomentObject cur(6); // Create a source moment object with 6 as maximum order
104 cur.setType(vpMomentObject::DENSE_POLYGON); // The object is defined by a
105 // countour polygon
106 cur.fromVector(vec_p); // Init the dense object with the source polygon
107
108 vpMomentObject dst(6); // Create a destination moment object with 6 as maximum order
109 dst.setType(vpMomentObject::DENSE_POLYGON); // The object is defined by a
110 // countour polygon
111 dst.fromVector(vec_p_d); // Init the dense object with the destination polygon
112
113 // init classic moment primitives (for source)
115 vpMomentCommon::getAlpha(dst)); // Init classic features
116 vpFeatureMomentCommon fmdb_cur(mdb_cur);
117
120 vpMomentCommon::getAlpha(dst)); // Init classic features
121 vpFeatureMomentCommon fmdb_dst(mdb_dst);
122
123 // update+compute moment primitives from object (for destination)
124 mdb_dst.updateAll(dst);
125 // update+compute features (+interaction matrixes) from plane
126 fmdb_dst.updateAll(Ad, Bd, Cd);
127
128 // define visual servoing task
129 vpServo task;
132 task.setLambda(1);
133
134 task.addFeature(fmdb_cur.getFeatureGravityNormalized(), fmdb_dst.getFeatureGravityNormalized());
135 task.addFeature(fmdb_cur.getFeatureAn(), fmdb_dst.getFeatureAn());
136 // the object is NOT symmetric
137 // select C4 and C6
138 task.addFeature(fmdb_cur.getFeatureCInvariant(), fmdb_dst.getFeatureCInvariant(),
140 task.addFeature(fmdb_cur.getFeatureAlpha(), fmdb_dst.getFeatureAlpha());
141
142 vpBasicFeature *al = new vpFeatureMomentAlpha(mdb_dst, 0, 0, 1.);
143 al->init();
144 al->error(*al);
145 // param robot
146 vpSimulatorCamera robot;
147 float sampling_time = 0.010f; // Sampling period in seconds
148 robot.setSamplingTime(sampling_time);
149 wMc = wMo * cMo.inverse();
150 robot.setPosition(wMc);
151
152 do {
153 wMc = robot.getPosition();
154 cMo = wMc.inverse() * wMo;
155 vec_p.clear();
156
157 for (int i = 0; i < nbpoints; i++) {
158 vpPoint p(x[i], y[i], 0.0);
159 p.track(cMo);
160 vec_p.push_back(p);
161 }
162 cMoToABC(cMo, A, B, C);
163
164 cur.fromVector(vec_p);
165 // update+compute moment primitives from object (for source)
166 mdb_cur.updateAll(cur);
167 // update+compute features (+interaction matrixes) from plane
168 fmdb_cur.updateAll(A, B, C);
169
171 task.print();
173 double t = vpTime::measureTimeMs();
174 vpTime::wait(t, sampling_time * 1000); // Wait 10 ms
175 } while ((task.getError()).sumSquare() > 0.005);
176 std::cout << "final error=" << (task.getError()).sumSquare() << std::endl;
177 return EXIT_SUCCESS;
178 }
179 catch (const vpException &e) {
180 std::cout << "Catch an exception: " << e << std::endl;
181 return EXIT_FAILURE;
182 }
183}
class that defines what is a visual feature
virtual vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
virtual void init()=0
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ divideByZeroError
Division by zero.
Definition vpException.h:82
Functionality computation for in-plane rotation moment feature : computes the interaction matrix asso...
This class allows to access common vpFeatureMoments in a pre-filled database.
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
static double rad(double deg)
Definition vpMath.h:116
This class initializes and allows access to commonly used moments.
static std::vector< double > getMu3(vpMomentObject &object)
static double getAlpha(vpMomentObject &object)
static double getSurface(vpMomentObject &object)
Class for generic objects.
This class defines the container for a plane geometrical structure.
Definition vpPlane.h:54
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition vpPlane.cpp:361
double getD() const
Definition vpPlane.h:106
double getA() const
Definition vpPlane.h:100
double getC() const
Definition vpPlane.h:104
void setABCD(double a, double b, double c, double d)
Definition vpPlane.h:88
double getB() const
Definition vpPlane.h:102
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 setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
@ CAMERA_FRAME
Definition vpRobot.h:80
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
vpColVector getError() const
Definition vpServo.h:276
vpColVector computeControlLaw()
Definition vpServo.cpp:930
@ CURRENT
Definition vpServo.h:179
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition vpServo.cpp:487
Class that defines the simplest robot: a free flying camera.
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()