iCub-main
functionEncoder.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
3  * Author: Ugo Pattacini
4  * email: ugo.pattacini@iit.it
5  * website: www.robotcub.org
6  * Permission is granted to copy, distribute, and/or modify this program
7  * under the terms of the GNU General Public License, version 2 or any
8  * later version published by the Free Software Foundation.
9  *
10  * A copy of the license can be found at
11  * http://www.robotcub.org/icub/license/gpl.txt
12  *
13  * This program is distributed in the hope that it will be useful, but
14  * WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
16  * Public License for more details
17 */
18 
19 #include <algorithm>
20 
21 #include <gsl/gsl_integration.h>
22 
23 #include <yarp/math/Math.h>
25 
26 #define CAST_GSLFUNC(x) (static_cast<gsl_function*>(x))
27 #define CAST_GSLWS(x) (static_cast<gsl_integration_workspace*>(x))
28 #define WAVELET_LUP_SIZE 57
29 
30 using namespace yarp::os;
31 using namespace yarp::sig;
32 using namespace yarp::math;
33 using namespace iCub::ctrl;
34 
35 namespace iCub
36 {
37 
38 namespace ctrl
39 {
40  double waveLUP[WAVELET_LUP_SIZE][2]={ { 0.000, 0.0 },
41  { 0.125, 0.0345833411415645 },
42  { 0.250, 0.107309738113335 },
43  { 0.375, 0.202014885729746 },
44  { 0.500, 0.328773960240219 },
45  { 0.625, 0.467801596736295 },
46  { 0.750, 0.618433317340402 },
47  { 0.875, 0.799799587363102 },
48  { 1.000, 1.00839956631986 },
49  { 1.125, 1.11213550554368 },
50  { 1.250, 1.10075377628576 },
51  { 1.375, 1.03456741914056 },
52  { 1.500, 0.876691003658645 },
53  { 1.625, 0.708936729237485 },
54  { 1.750, 0.535149009932799 },
55  { 1.875, 0.281586312944196 },
56  { 2.000, -0.0358782406601501 },
57  { 2.125, -0.233040642106083 },
58  { 2.250, -0.302412553008806 },
59  { 2.375, -0.321628766530846 },
60  { 2.500, -0.241966742214465 },
61  { 2.625, -0.190865137334676 },
62  { 2.750, -0.175103704342541 },
63  { 2.875, -0.0890678401406110 },
64  { 3.000, 0.0407115008395991 },
65  { 3.125, 0.108682072349430 },
66  { 3.250, 0.118185639560005 },
67  { 3.375, 0.101870197888310 },
68  { 3.500, 0.0341494900045930 },
69  { 3.625, 0.00431657421191742 },
70  { 3.750, 0.0162890023018952 },
71  { 3.875, 0.00436750867994683 },
72  { 4.000, -0.0120853649259263 },
73  { 4.125, -0.0194095628614931 },
74  { 4.250, -0.0234198437934784 },
75  { 4.375, -0.0169172631594144 },
76  { 4.500, 0.00224836903406590 },
77  { 4.625, 0.00948830118465152 },
78  { 4.750, 0.00525142022644811 },
79  { 4.875, 0.00340625270534754 },
80  { 5.000, -0.00116770339087780 },
81  { 5.125, -0.00296115923392665 },
82  { 5.250, -0.000413390931405029 },
83  { 5.375, 9.35269316433299e-05 },
84  { 5.500, 0.000103919276941685 },
85  { 5.625, 0.000321935964326630 },
86  { 5.750, -1.90454590045895e-05 },
87  { 5.875, -9.18215519817765e-05 },
88  { 6.000, 2.02418174948211e-05 },
89  { 6.125, 1.04451668231589e-05 },
90  { 6.250, -3.36622541321180e-06 },
91  { 6.375, 0.0 },
92  { 6.500, 0.0 },
93  { 6.625, 0.0 },
94  { 6.750, 0.0 },
95  { 6.875, 0.0 },
96  { 7.000, 0.0 } };
97 
98  /************************************************************************/
99  double waveletIntegrand(double x, void *params)
100  {
101  WaveletEncoder *pWavEnc=(WaveletEncoder*)params;
102  const Vector &values=*pWavEnc->pVal;
103  unsigned int n=pWavEnc->iCoeff;
104  double R=pWavEnc->resolution;
105  return (pWavEnc->interpFunction(values,x)-values[0]) * (R*pWavEnc->interpWavelet(R*x-n));
106  }
107 }
108 
109 }
110 
111 
112 /************************************************************************/
113 WaveletEncoder::WaveletEncoder()
114 {
115  resolution=(WAVELET_LUP_SIZE-1)/2.0;
116  w=gsl_integration_workspace_alloc(1000);
117  F=new gsl_function;
118 
119  CAST_GSLFUNC(F)->function=&waveletIntegrand;
120  CAST_GSLFUNC(F)->params=(void*)this;
121 }
122 
123 
124 /************************************************************************/
125 double WaveletEncoder::interpWavelet(const double x)
126 {
127  if ((x<=waveLUP[0][0]) || (x>=waveLUP[WAVELET_LUP_SIZE-1][0]))
128  return 0.0;
129  else
130  {
131  unsigned int i=(unsigned int)(x*(double)(WAVELET_LUP_SIZE-1)/waveLUP[WAVELET_LUP_SIZE-1][0]);
132  double x0=waveLUP[i][0];
133  double y0=waveLUP[i][1];
134  double x1=waveLUP[i+1][0];
135  double y1=waveLUP[i+1][1];
136 
137  return ((x-x0)/(x1-x0))*(y1-y0)+y0;
138  }
139 }
140 
141 
142 /************************************************************************/
143 double WaveletEncoder::interpFunction(const Vector &values, const double x)
144 {
145  // x shall be in [0,1]
146  double L=(double)values.size()-1.0;
147 
148  if (x<=0.0)
149  return values[0];
150  else if (x>=1.0)
151  return values[(size_t)L];
152  else
153  {
154  size_t i=(size_t)(x*L);
155  double x0=((double)i)/L;
156  double y0=values[i];
157 
158  if (++i>L)
159  i=(size_t)L;
160 
161  double x1=((double)i)/L;
162  double y1=values[i];
163 
164  return ((x-x0)/(x1-x0))*(y1-y0)+y0;
165  }
166 }
167 
168 
169 /************************************************************************/
170 bool WaveletEncoder::setEncoderOptions(const Property &options)
171 {
172  if (options.check("resolution"))
173  {
174  double R=options.find("resolution").asDouble();
175 
176  // apply saturation
177  resolution=std::max(0.0,std::min(R,double(WAVELET_LUP_SIZE-1)));
178  return true;
179  }
180  else
181  return false;
182 }
183 
184 
185 /************************************************************************/
186 Property WaveletEncoder::getEncoderOptions()
187 {
188  Property options;
189  options.put("resolution",resolution);
190  return options;
191 }
192 
193 
194 /************************************************************************/
195 Code WaveletEncoder::encode(const Vector &values)
196 {
197  Code code;
198  pVal=&values;
199  unsigned int N=(unsigned int)resolution+1;
200  code.coefficients.resize(N+1);
201  double error;
202 
203  code.coefficients[0]=values[0];
204  for (iCoeff=0; iCoeff<N; iCoeff++)
205  {
206  double tau1=(waveLUP[0][0]+iCoeff)/resolution;
207  double tau2=(waveLUP[WAVELET_LUP_SIZE-1][0]+iCoeff)/resolution;
208 
209  gsl_integration_qag(CAST_GSLFUNC(F),tau1,tau2,1e-3,1e-2,1000,GSL_INTEG_GAUSS61,CAST_GSLWS(w),
210  &code.coefficients[iCoeff+1],&error);
211  }
212 
213  return code;
214 }
215 
216 
217 /************************************************************************/
218 double WaveletEncoder::decode(const Code &code, const double x)
219 {
220  unsigned int N=(unsigned int)resolution+1;
221  double decodedVal=code.coefficients[0];
222 
223  // compute the linear combination of wavelets
224  for (unsigned int n=0; n<N; n++)
225  decodedVal+=code.coefficients[n+1]*interpWavelet(resolution*x-n);
226 
227  return decodedVal;
228 }
229 
230 
231 /************************************************************************/
232 WaveletEncoder::~WaveletEncoder()
233 {
234  gsl_integration_workspace_free(CAST_GSLWS(w));
235  delete CAST_GSLFUNC(F);
236 }
237 
238 
239 
double waveletIntegrand(double x, void *params)
#define CAST_GSLFUNC(x)
const yarp::sig::Vector * pVal
#define WAVELET_LUP_SIZE
int n
const FSC max
Definition: strain.h:48
const FSC min
Definition: strain.h:49
yarp::sig::Vector coefficients
The vector of coefficients encoding the function.
double interpWavelet(const double x)
double waveLUP[WAVELET_LUP_SIZE][2]
This file contains the definition of unique IDs for the body parts and the skin parts of the robot...
double interpFunction(const yarp::sig::Vector &values, const double x)
#define CAST_GSLWS(x)
Encode any given function as a set of wavelet coefficients.