Visual Servoing Platform  version 3.3.0
manServoMomentsSimple.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 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 http://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  * Authors:
35  * Filip Novotny
36  *
37  *****************************************************************************/
38 
45 #include <visp3/core/vpPoint.h> //the basic tracker
46 
47 #include <iostream> //some console output
48 #include <limits>
49 #include <vector> //store the polygon
50 #include <visp3/core/vpException.h>
51 #include <visp3/core/vpMomentCommon.h> //update the common database with the object
52 #include <visp3/core/vpMomentObject.h> //transmit the polygon to the object
53 #include <visp3/core/vpPlane.h>
54 #include <visp3/robot/vpSimulatorCamera.h>
55 #include <visp3/visual_features/vpFeatureMomentCommon.h> //init the feature database using the information about moment dependencies
56 #include <visp3/vs/vpServo.h> //visual servoing task
57 // this function converts the plane defined by the cMo to 1/Z=Ax+By+C plane
58 // form
59 
60 void cMoToABC(vpHomogeneousMatrix &cMo, double &A, double &B, double &C);
61 
62 void cMoToABC(vpHomogeneousMatrix &cMo, double &A, double &B, double &C)
63 {
64  vpPlane pl;
65  pl.setABCD(0, 0, 1.0, 0);
66  pl.changeFrame(cMo);
67 
68  if (fabs(pl.getD()) < std::numeric_limits<double>::epsilon()) {
69  std::cout << "Invalid position:" << std::endl;
70  std::cout << cMo << std::endl;
71  std::cout << "Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
72  throw vpException(vpException::divideByZeroError, "invalid position!");
73  }
74  A = -pl.getA() / pl.getD();
75  B = -pl.getB() / pl.getD();
76  C = -pl.getC() / pl.getD();
77 }
78 
79 int main()
80 {
81  try {
82  double x[8] = {1, 3, 4, -1, -3, -2, -1, 1};
83  double y[8] = {0, 1, 4, 4, -2, -2, 1, 0};
84  double A, B, C, Ad, Bd, Cd;
85 
86  int nbpoints = 8;
87  std::vector<vpPoint> vec_p,
88  vec_p_d; // vectors that contain the vertices of the contour polygon
89 
90  vpHomogeneousMatrix cMo(0.1, 0.0, 1.0, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
92  vpHomogeneousMatrix wMo; // Set to identity
93  vpHomogeneousMatrix wMc; // Camera position in the world frame
94 
95  cMoToABC(cMo, A, B, C);
96  cMoToABC(cdMo, Ad, Bd, Cd);
97  // Define source and destination polygons
98  for (int i = 0; i < nbpoints; i++) {
99  vpPoint p(x[i], y[i], 0.0);
100  p.track(cMo);
101  vec_p.push_back(p);
102  p.track(cdMo);
103  vec_p_d.push_back(p);
104  }
105 
106  vpMomentObject cur(6); // Create a source moment object with 6 as maximum order
107  cur.setType(vpMomentObject::DENSE_POLYGON); // The object is defined by a
108  // countour polygon
109  cur.fromVector(vec_p); // Init the dense object with the source polygon
110 
111  vpMomentObject dst(6); // Create a destination moment object with 6 as maximum order
112  dst.setType(vpMomentObject::DENSE_POLYGON); // The object is defined by a
113  // countour polygon
114  dst.fromVector(vec_p_d); // Init the dense object with the destination polygon
115 
116  // init classic moment primitives (for source)
118  vpMomentCommon::getAlpha(dst)); // Init classic features
119  vpFeatureMomentCommon fmdb_cur(mdb_cur);
120 
123  vpMomentCommon::getAlpha(dst)); // Init classic features
124  vpFeatureMomentCommon fmdb_dst(mdb_dst);
125 
126  // update+compute moment primitives from object (for destination)
127  mdb_dst.updateAll(dst);
128  // update+compute features (+interaction matrixes) from plane
129  fmdb_dst.updateAll(Ad, Bd, Cd);
130 
131  // define visual servoing task
132  vpServo task;
135  task.setLambda(1);
136 
137  task.addFeature(fmdb_cur.getFeatureGravityNormalized(), fmdb_dst.getFeatureGravityNormalized());
138  task.addFeature(fmdb_cur.getFeatureAn(), fmdb_dst.getFeatureAn());
139  // the object is NOT symmetric
140  // select C4 and C6
141  task.addFeature(fmdb_cur.getFeatureCInvariant(), fmdb_dst.getFeatureCInvariant(),
143  task.addFeature(fmdb_cur.getFeatureAlpha(), fmdb_dst.getFeatureAlpha());
144 
145  vpBasicFeature *al = new vpFeatureMomentAlpha(mdb_dst, 0, 0, 1.);
146  al->init();
147  al->error(*al);
148  // param robot
149  vpSimulatorCamera robot;
150  float sampling_time = 0.010f; // Sampling period in seconds
151  robot.setSamplingTime(sampling_time);
152  wMc = wMo * cMo.inverse();
153  robot.setPosition(wMc);
154 
155  do {
156  wMc = robot.getPosition();
157  cMo = wMc.inverse() * wMo;
158  vec_p.clear();
159 
160  for (int i = 0; i < nbpoints; i++) {
161  vpPoint p(x[i], y[i], 0.0);
162  p.track(cMo);
163  vec_p.push_back(p);
164  }
165  cMoToABC(cMo, A, B, C);
166 
167  cur.fromVector(vec_p);
168  // update+compute moment primitives from object (for source)
169  mdb_cur.updateAll(cur);
170  // update+compute features (+interaction matrixes) from plane
171  fmdb_cur.updateAll(A, B, C);
172 
173  vpColVector v = task.computeControlLaw();
174  task.print();
176  double t = vpTime::measureTimeMs();
177  vpTime::wait(t, sampling_time * 1000); // Wait 10 ms
178  } while ((task.getError()).sumSquare() > 0.005);
179  std::cout << "final error=" << (task.getError()).sumSquare() << std::endl;
180  return EXIT_SUCCESS;
181  } catch (const vpException &e) {
182  std::cout << "Catch an exception: " << e << std::endl;
183  return EXIT_FAILURE;
184  }
185 }
vpException::divideByZeroError
Division by zero.
Definition: vpException.h:93
vpFeatureMomentCommon
This class allows to access common vpFeatureMoments in a pre-filled database.
Definition: vpFeatureMomentCommon.h:225
vpTime::wait
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:172
vpMomentObject::setType
void setType(vpObjectType input_type)
Definition: vpMomentObject.h:293
vpServo::CURRENT
Definition: vpServo.h:185
vpMath::rad
static double rad(double deg)
Definition: vpMath.h:107
vpMomentCommon
This class initializes and allows access to commonly used moments.
Definition: vpMomentCommon.h:103
vpServo::setLambda
void setLambda(double c)
Definition: vpServo.h:405
vpSimulatorCamera::setPosition
void setPosition(const vpHomogeneousMatrix &wMc)
Definition: vpSimulatorCamera.cpp:241
vpBasicFeature::init
virtual void init()=0
vpServo::EYEINHAND_CAMERA
Definition: vpServo.h:158
vpMomentObject::DENSE_POLYGON
Definition: vpMomentObject.h:227
vpSimulatorCamera
Class that defines the simplest robot: a free flying camera.
Definition: vpSimulatorCamera.h:106
vpColVector
Implementation of column vector and the associated operations.
Definition: vpColVector.h:129
vpServo::setServo
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:222
vpPlane::setABCD
void setABCD(double a, double b, double c, double d)
Definition: vpPlane.h:89
vpTime::measureTimeMs
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:125
vpSimulatorCamera::getPosition
vpHomogeneousMatrix getPosition() const
Definition: vpSimulatorCamera.cpp:118
vpFeatureMomentCInvariant::selectC4
static unsigned int selectC4()
Definition: vpFeatureMomentCInvariant.h:280
vpServo::print
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:312
vpFeatureMomentCInvariant::selectC6
static unsigned int selectC6()
Definition: vpFeatureMomentCInvariant.h:288
vpMomentObject::fromVector
void fromVector(std::vector< vpPoint > &points)
Definition: vpMomentObject.cpp:229
vpPlane::getB
double getB() const
Definition: vpPlane.h:103
vpServo::getError
vpColVector getError() const
Definition: vpServo.h:281
vpPlane::getC
double getC() const
Definition: vpPlane.h:105
vpRobot::CAMERA_FRAME
Definition: vpRobot.h:81
vpMomentCommon::getMu3
static std::vector< double > getMu3(vpMomentObject &object)
Definition: vpMomentCommon.cpp:198
vpBasicFeature::error
virtual vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
Definition: vpBasicFeature.cpp:150
vpServo::addFeature
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:496
vpFeatureMomentAlpha
Functionality computation for in-plane rotation moment feature : computes the interaction matrix asso...
Definition: vpFeatureMomentAlpha.h:103
vpMomentObject
Class for generic objects.
Definition: vpMomentObject.h:218
vpPlane::getD
double getD() const
Definition: vpPlane.h:107
vpPlane
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:57
vpPlane::changeFrame
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition: vpPlane.cpp:353
vpServo::setInteractionMatrixType
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:573
vpServo
Definition: vpServo.h:149
vpServo::computeControlLaw
vpColVector computeControlLaw()
Definition: vpServo.cpp:934
vpRobotSimulator::setSamplingTime
virtual void setSamplingTime(const double &delta_t)
Definition: vpRobotSimulator.h:90
vpHomogeneousMatrix::inverse
vpHomogeneousMatrix inverse() const
Definition: vpHomogeneousMatrix.cpp:640
vpPoint
Class that defines what is a point.
Definition: vpPoint.h:57
vpHomogeneousMatrix
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition: vpHomogeneousMatrix.h:148
vpMomentCommon::getSurface
static double getSurface(vpMomentObject &object)
Definition: vpMomentCommon.cpp:148
vpSimulatorCamera::setVelocity
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
Definition: vpSimulatorCamera.cpp:197
vpException
error that can be emited by ViSP classes.
Definition: vpException.h:70
vpBasicFeature
class that defines what is a visual feature
Definition: vpBasicFeature.h:76
vpMomentCommon::getAlpha
static double getAlpha(vpMomentObject &object)
Definition: vpMomentCommon.cpp:175
vpPlane::getA
double getA() const
Definition: vpPlane.h:101