ViSP
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 - 2014 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 
62 void cMoToABC(vpHomogeneousMatrix& cMo, double& A,double& B, double& C);
63 
64 void cMoToABC(vpHomogeneousMatrix& cMo, double& A,double& B, double& C)
65 {
66  vpPlane pl;
67  pl.setABCD(0,0,1.0,0);
68  pl.changeFrame(cMo);
69 
70  if(fabs(pl.getD())<std::numeric_limits<double>::epsilon()){
71  std::cout << "Invalid position:" << std::endl;
72  std::cout << cMo << std::endl;
73  std::cout << "Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
74  throw vpException(vpException::divideByZeroError,"invalid position!");
75  }
76  A=-pl.getA()/pl.getD();
77  B=-pl.getB()/pl.getD();
78  C=-pl.getC()/pl.getD();
79 }
80 
81 int main()
82 {
83  try {
84  double x[8] = { 1,3, 4,-1 ,-3,-2,-1,1};
85  double y[8] = { 0,1, 4, 4, -2,-2, 1,0};
86  double A,B,C,Ad,Bd,Cd;
87 
88  int nbpoints = 8;
89  std::vector<vpPoint> vec_p,vec_p_d; // vectors that contain the vertices of the contour polygon
90 
91  vpHomogeneousMatrix cMo(0.1,0.0,1.0,vpMath::rad(0),vpMath::rad(0),vpMath::rad(0));
93 
94  cMoToABC(cMo,A,B,C);
95  cMoToABC(cdMo,Ad,Bd,Cd);
96  // Define source and destination polygons
97  for (int i = 0 ; i < nbpoints ; i++){
98  vpPoint p;
99  p.setWorldCoordinates(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 countour polygon
108  cur.fromVector(vec_p); // Init the dense object with the source polygon
109 
110  vpMomentObject dst(6); // Create a destination moment object with 6 as maximum order
111  dst.setType(vpMomentObject::DENSE_POLYGON); // The object is defined by a countour polygon
112  dst.fromVector(vec_p_d); // Init the dense object with the destination polygon
113 
114  //init classic moment primitives (for source)
115  vpMomentCommon mdb_cur(vpMomentCommon::getSurface(dst),vpMomentCommon::getMu3(dst),vpMomentCommon::getAlpha(dst)); //Init classic features
116  vpFeatureMomentCommon fmdb_cur(mdb_cur);
117 
119  vpMomentCommon mdb_dst(vpMomentCommon::getSurface(dst),vpMomentCommon::getMu3(dst),vpMomentCommon::getAlpha(dst)); //Init classic features
120  vpFeatureMomentCommon fmdb_dst(mdb_dst);
121 
122  //update+compute moment primitives from object (for destination)
123  mdb_dst.updateAll(dst);
124  //update+compute features (+interaction matrixes) from plane
125  fmdb_dst.updateAll(Ad,Bd,Cd);
126 
127  //define visual servoing task
128  vpServo task;
131  task.setLambda(1) ;
132 
133  task.addFeature(fmdb_cur.getFeatureGravityNormalized(),fmdb_dst.getFeatureGravityNormalized());
134  task.addFeature(fmdb_cur.getFeatureAn(),fmdb_dst.getFeatureAn());
135  //the object is NOT symmetric
136  //select C4 and C6
137  task.addFeature(fmdb_cur.getFeatureCInvariant(),fmdb_dst.getFeatureCInvariant(),
139  task.addFeature(fmdb_cur.getFeatureAlpha(),fmdb_dst.getFeatureAlpha());
140 
141  vpBasicFeature *al = new vpFeatureMomentAlpha(mdb_dst,0,0,1.);
142  al->init();
143  al->error(*al);
144  //param robot
145  vpRobotCamera robot ;
146  float sampling_time = 0.010f; // Sampling period in seconds
147  robot.setSamplingTime(sampling_time);
148  robot.setPosition(cMo);
149 
150  do{
151  robot.getPosition(cMo);
152  vec_p.clear();
153 
154  for (int i = 0 ; i < nbpoints ; i++){
155  vpPoint p;
156  p.setWorldCoordinates(x[i],y[i],0.0);
157  p.track(cMo) ;
158  vec_p.push_back(p);
159  }
160  cMoToABC(cMo,A,B,C);
161 
162  cur.fromVector(vec_p);
163  //update+compute moment primitives from object (for source)
164  mdb_cur.updateAll(cur);
165  //update+compute features (+interaction matrixes) from plane
166  fmdb_cur.updateAll(A,B,C);
167 
168  vpColVector v = task.computeControlLaw();
169  task.print();
171  double t = vpTime::measureTimeMs();
172  vpTime::wait(t, sampling_time * 1000); // Wait 10 ms
173  } while(( task.getError() ).sumSquare()>0.005);
174  std::cout << "final error=" << ( task.getError() ).sumSquare() << std::endl;
175  return 0;
176  }
177  catch(vpException e) {
178  std::cout << "Catch an exception: " << e << std::endl;
179  return 1;
180  }
181 }
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
void setPosition(const vpHomogeneousMatrix &cMw)
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:449
This class allows to access common vpFeatureMoments in a pre-filled database.
error that can be emited by ViSP classes.
Definition: vpException.h:76
void setABCD(const double a, const double b, const double c, const double d)
Definition: vpPlane.h:102
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
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:257
vpColVector computeControlLaw()
Definition: vpServo.cpp:902
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition: vpPlane.cpp:376
Class that defines the simplest robot: a free flying camera.
static std::vector< double > getMu3(vpMomentObject &object)
void setLambda(double c)
Definition: vpServo.h:370
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:522
static double rad(double deg)
Definition: vpMath.h:100
void getPosition(vpHomogeneousMatrix &cMw) const
This class initializes and allows access to commonly used moments.
static double getAlpha(vpMomentObject &object)
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:117
void setType(vpObjectType input_type)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:251
double getA() const
Definition: vpPlane.h:115
double getC() const
Definition: vpPlane.h:119
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:67
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:220
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v)
double getD() const
Definition: vpPlane.h:121
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