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