ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
manServoMomentsSimple.cpp
1 /****************************************************************************
2  *
3  * $Id: servoMomentPolygon.cpp 3323 2011-09-13 15:23:56Z fnovotny $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Example of visual servoing with moments using a polygon as object container
36  *
37  * Authors:
38  * Filip Novotny
39  *
40  *****************************************************************************/
41 
48 #include <visp/vpPoint.h> //the basic tracker
49 
50 #include <vector> //store the polygon
51 #include <visp/vpMomentObject.h> //transmit the polygon to the object
52 #include <visp/vpMomentCommon.h> //update the common database with the object
53 #include <visp/vpFeatureMomentCommon.h> //init the feature database using the information about moment dependencies
54 #include <visp/vpServo.h> //visual servoing task
55 #include <visp/vpRobotCamera.h>
56 #include <visp/vpPlane.h>
57 #include <visp/vpException.h>
58 #include <limits>
59 #include <iostream> //some console output
60 //this function converts the plane defined by the cMo to 1/Z=Ax+By+C plane form
61 void cMoToABC(vpHomogeneousMatrix& cMo, double& A,double& B, double& C){
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  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,vec_p_d; // vectors that contain the vertices of the contour polygon
85 
86  vpHomogeneousMatrix cMo(0.1,0.0,1.0,vpMath::rad(0),vpMath::rad(0),vpMath::rad(0));
88 
89  cMoToABC(cMo,A,B,C);
90  cMoToABC(cdMo,Ad,Bd,Cd);
91  // Define source and destination polygons
92  for (int i = 0 ; i < nbpoints ; i++){
93  vpPoint p;
94  p.setWorldCoordinates(x[i],y[i],0.0);
95  p.track(cMo) ;
96  vec_p.push_back(p);
97  p.track(cdMo) ;
98  vec_p_d.push_back(p);
99  }
100 
101  vpMomentObject cur(6); // Create a source moment object with 6 as maximum order
102  cur.setType(vpMomentObject::DENSE_POLYGON); // The object is defined by a countour polygon
103  cur.fromVector(vec_p); // Init the dense object with the source polygon
104 
105  vpMomentObject dst(6); // Create a destination moment object with 6 as maximum order
106  dst.setType(vpMomentObject::DENSE_POLYGON); // The object is defined by a countour polygon
107  dst.fromVector(vec_p_d); // Init the dense object with the destination polygon
108 
109  //init classic moment primitives (for source)
110  vpMomentCommon mdb_cur(vpMomentCommon::getSurface(dst),vpMomentCommon::getMu3(dst),vpMomentCommon::getAlpha(dst)); //Init classic features
111  vpFeatureMomentCommon fmdb_cur(mdb_cur);
112 
114  vpMomentCommon mdb_dst(vpMomentCommon::getSurface(dst),vpMomentCommon::getMu3(dst),vpMomentCommon::getAlpha(dst)); //Init classic features
115  vpFeatureMomentCommon fmdb_dst(mdb_dst);
116 
117 
118  //update+compute moment primitives from object (for destination)
119  mdb_dst.updateAll(dst);
120  //update+compute features (+interaction matrixes) from plane
121  fmdb_dst.updateAll(Ad,Bd,Cd);
122 
123  //define visual servoing task
124  vpServo task;
127  task.setLambda(1) ;
128 
129  task.addFeature(fmdb_cur.getFeatureGravityNormalized(),fmdb_dst.getFeatureGravityNormalized());
130  task.addFeature(fmdb_cur.getFeatureAn(),fmdb_dst.getFeatureAn());
131  //the object is NOT symmetric
132  //select C4 and C6
133  task.addFeature(fmdb_cur.getFeatureCInvariant(),fmdb_dst.getFeatureCInvariant(),
135  task.addFeature(fmdb_cur.getFeatureAlpha(),fmdb_dst.getFeatureAlpha());
136 
137  vpBasicFeature *al = new vpFeatureMomentAlpha(mdb_dst,0,0,1.);
138  al->init();
139  al->error(*al);
140  //param robot
141  vpRobotCamera robot ;
142  float sampling_time = 0.010f; // Sampling period in seconds
143  robot.setSamplingTime(sampling_time);
144  robot.setPosition(cMo);
145 
146  do{
147  robot.getPosition(cMo);
148  vec_p.clear();
149 
150  for (int i = 0 ; i < nbpoints ; i++){
151  vpPoint p;
152  p.setWorldCoordinates(x[i],y[i],0.0);
153  p.track(cMo) ;
154  vec_p.push_back(p);
155  }
156  cMoToABC(cMo,A,B,C);
157 
158  cur.fromVector(vec_p);
159  //update+compute moment primitives from object (for source)
160  mdb_cur.updateAll(cur);
161  //update+compute features (+interaction matrixes) from plane
162  fmdb_cur.updateAll(A,B,C);
163 
164  vpColVector v = task.computeControlLaw();
165  task.print();
167  double t = vpTime::measureTimeMs();
168  vpTime::wait(t, sampling_time * 1000); // Wait 10 ms
169  }while(( task.getError() ).sumSquare()>0.005);
170  std::cout << "final error=" << ( task.getError() ).sumSquare() << std::endl;
171 
172  return 0;
173 }
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
void setPosition(const vpHomogeneousMatrix &cMw)
static double getAlpha(vpMomentObject &objec)
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
create a new ste of two visual features
Definition: vpServo.cpp:444
This class allows to access common vpFeatureMoments in a pre-filled database.
void setLambda(double _lambda)
set the gain lambda
Definition: vpServo.h:253
error that can be emited by ViSP classes.
Definition: vpException.h:75
Class for generic objects.
void track(const vpHomogeneousMatrix &cMo)
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.
static double measureTimeMs()
Definition: vpTime.cpp:86
static int wait(double t0, double t)
Definition: vpTime.cpp:149
void setType(vpObjectType type)
void setABCD(const double A, const double B, const double C, const double D)
Definition: vpPlane.h:99
Class that defines what is a point.
Definition: vpPoint.h:65
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:301
vpColVector computeControlLaw()
compute the desired control law
Definition: vpServo.cpp:883
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition: vpPlane.cpp:352
Class that defines the simplest robot: a free flying camera.
static std::vector< double > getMu3(vpMomentObject &object)
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)
Set the type of the interaction matrix (current, mean, desired, user).
Definition: vpServo.cpp:509
static double rad(double deg)
Definition: vpMath.h:100
void getPosition(vpHomogeneousMatrix &cMw) const
This class initializes and allows access to commonly used moments.
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
double getB() const
Definition: vpPlane.h:114
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:258
double getA() const
Definition: vpPlane.h:112
double getC() const
Definition: vpPlane.h:116
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:67
Class required to compute the visual servoing control law descbribed in and .
Definition: vpServo.h:153
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v)
void setServo(vpServoType _servo_type)
Choice of the visual servoing control law.
Definition: vpServo.cpp:214
double getD() const
Definition: vpPlane.h:118
void setWorldCoordinates(const double ox, const double oy, const double oz)
Set the point world coordinates. We mean here the coordinates of the point in the object frame...
Definition: vpPoint.cpp:74