FunctionApproximator.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @package RobotComponents::ArmarXObjects::FunctionApproximator
17  * @author zhou ( you dot zhou at kit dot edu )
18  * @date 2016
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #include "FunctionApproximator.h"
24 
25 
26 using namespace armarx;
27 
28 
30 {
31  ARMARX_INFO << "initializing DMP component";
32 
33 
34 
35  ARMARX_INFO << "successfully initialized DMP component" ;
36 
37 }
38 
39 
41 {
42 
43 }
44 
45 
47 {
48  ARMARX_INFO << "disconnecting FunctionApproximator component";
49 
50 }
51 
52 
54 {
55  ARMARX_INFO << "exiting FunctionApproximator component";
56 
57 }
58 
59 void FunctionApproximator::learn(const std::string& name, const DVector2d& x, const DVector2d& y, const Ice::Current&)
60 {
61 
62  DMP::FunctionApproximationInterfacePtr approximator = funcAppPool.find(name)->second;
63  if (approximator)
64  {
65  if (x.size() != y.size())
66  {
67  ARMARX_ERROR << "The factors dimension should be the same as the responds dimension.";
68  }
69 
70 
71  DMP::DVec2d xx;
72  DMP::DVec2d yy;
73 
74  xx.resize(x.size());
75  yy.resize(y.size());
76 
77  for (size_t i = 0; i < x.size(); ++i)
78  {
79  DMP::DVec cx;
80  DMP::DVec cy;
81 
82  for (size_t j = 0; j < x[i].size(); ++j)
83  {
84  cx.push_back(x[i][j]);
85  cy.push_back(y[i][j]);
86  }
87 
88  xx[i] = cx;
89  yy[i] = cy;
90 
91  }
92 
93  approximator->learn(xx, yy);
94 
95  }
96  else
97  {
98  ARMARX_ERROR << "predict: function approximator not found.";
99  return;
100  }
101 }
102 
103 Ice::Double FunctionApproximator::predict(const std::string& name, const Ice::DoubleSeq& x, const Ice::Current&)
104 {
105  DMP::FunctionApproximationInterfacePtr approximator = funcAppPool.find(name)->second;
106  if (approximator)
107  {
108  DMP::DVec xx;
109 
110  for (size_t i = 0; i < x.size(); ++i)
111  {
112  xx.push_back(x.at(i));
113  }
114 
115  double res = (*approximator)(xx).at(0);
116 
117  return res;
118  }
119  else
120  {
121  ARMARX_ERROR << "predict: function approximator not found.";
122  return 0;
123  }
124 }
125 
126 void FunctionApproximator::ilearn(const std::string& name, const Ice::DoubleSeq& x, Ice::Double y, const Ice::Current&)
127 {
128  DMP::FunctionApproximationInterfacePtr approximator = funcAppPool.find(name)->second;
129  if (approximator)
130  {
131  DMP::DVec factors;
132 
133  for (size_t i = 0; i < x.size(); ++i)
134  {
135  factors.push_back(x.at(i));
136  }
137 
138  approximator->ilearn(factors, y);
139  }
140  else
141  {
142  ARMARX_ERROR << "ilearn: function approximator not found.";
143  return;
144  }
145 }
146 
147 void FunctionApproximator::blearn(const std::string& name, const Ice::DoubleSeq& x, const Ice::DoubleSeq& y, const Ice::Current&)
148 {
149  DMP::FunctionApproximationInterfacePtr approximator = funcAppPool.find(name)->second;
150  if (approximator)
151  {
152  DMP::DVec2d factors;
153  DMP::DVec2d responds;
154 
155  factors.push_back(DMP::DVec(x));
156  responds.push_back(DMP::DVec(y));
157 
158  approximator->learn(factors, responds);
159  }
160  else
161  {
162  ARMARX_ERROR << "ilearn: function approximator not found.";
163  return;
164  }
165 }
166 
167 Ice::DoubleSeq FunctionApproximator::bpredict(const std::string& name, const Ice::DoubleSeq& x, const Ice::Current&)
168 {
169  DMP::FunctionApproximationInterfacePtr approximator = funcAppPool.find(name)->second;
170  if (approximator)
171  {
172  Ice::DoubleSeq responds;
173 
174  for (size_t i = 0; i < x.size(); ++i)
175  {
176  responds.push_back((*approximator)(x.at(i)).at(0));
177  }
178 
179  return responds;
180  }
181  else
182  {
183  ARMARX_ERROR << "ilearn: function approximator not found.";
184  return std::vector<double>();
185  }
186 }
187 
188 void FunctionApproximator::reset(const Ice::Current&)
189 {
190 
191  for (FuncAppMap::iterator it = funcAppPool.begin(); it != funcAppPool.end(); ++it)
192  {
193  it->second.reset(new DMP::RBFInterpolator<DMP::GaussRadialBasisFunction>(2));
194  }
195 
196 }
197 
198 
199 //armarx::PropertyDefinitionsPtr FunctionApproximator::createPropertyDefinitions()
200 //{
201 // return armarx::PropertyDefinitionsPtr(new FunctionApproximatorPropertyDefinitions(
202 // getConfigIdentifier()));
203 //}
204 
205 
206 
207 void armarx::FunctionApproximator::initialize(const std::string& fappName, const Ice::DoubleSeq& /*widthFactors*/, const Ice::Current&)
208 {
209  //int dim = widthFactors.size();
210 
211  DMP::FunctionApproximationInterfacePtr approximator(new DMP::LWR<DMP::GaussKernel>(100));
212 
213  funcAppPool.insert(FuncAppPair(fappName, approximator));
214 }
215 
216 
217 void armarx::FunctionApproximator::getFunctionApproximatorFromFile(const std::string& funcName, const std::string& name, const Ice::Current&)
218 {
219  std::ifstream file(name);
220 
221  DMP::FunctionApproximationInterface* readPtr;
222 
223  boost::archive::xml_iarchive ar(file);
224  ar >> boost::serialization::make_nvp("approximator", readPtr);
225 
226  file.close();
227 
228  DMP::FunctionApproximationInterfacePtr approximator(readPtr);
229  funcAppPool.insert(FuncAppPair(funcName, approximator));
230 }
231 
232 void armarx::FunctionApproximator::getFunctionApproximatorsFromFile(const std::vector<std::string>& funcName, const std::string& filename, const Ice::Current&)
233 {
234  std::ifstream file(filename);
235 
236  std::vector<DMP::FunctionApproximationInterface*> readPtr;
237 
238  boost::archive::xml_iarchive ar(file);
239  ar >> boost::serialization::make_nvp("approximator", readPtr);
240 
241  file.close();
242 
243  for (size_t i = 0; i < readPtr.size(); i++)
244  {
245 
246  DMP::FunctionApproximationInterfacePtr approximator(readPtr[i]);
247  funcAppPool.insert(FuncAppPair(funcName[i], approximator));
248  }
249 }
250 
251 
252 void FunctionApproximator::saveFunctionApproximatorInFile(const std::string& funcName, const std::string& name, const Ice::Current&)
253 {
254 
255  std::cout << "save function approximator in file ... " << std::endl;
256  std::ofstream file(name);
257 
258  boost::archive::xml_oarchive ar(file);
259 
260  DMP::FunctionApproximationInterface* savedPtr = funcAppPool[funcName].get();
261 
262  ar << boost::serialization::make_nvp("approximator", savedPtr);
263 
264  file.close();
265 }
266 
267 
268 
269 void armarx::FunctionApproximator::initializeTest(const std::string& fappName, const Ice::DoubleSeq& widthFactors)
270 {
271  int dim = widthFactors.size();
272 
273  DMP::FunctionApproximationInterfacePtr approximator(new DMP::RBFInterpolator<DMP::GaussRadialBasisFunction>(dim, widthFactors));
274 
275  funcAppPool.insert(FuncAppPair(fappName, approximator));
276 }
277 
278 
279 void FunctionApproximator::saveFunctionApproximatorInFileTest(const std::string& funcName, const std::string& name)
280 {
281 
282  std::cout << "save function approximator in file ... " << std::endl;
283  std::ofstream file(name);
284  DMP::FunctionApproximationInterfacePtr approximator = funcAppPool[funcName];
285 
286  boost::archive::text_oarchive ar(file);
287  DMP::FunctionApproximationInterface* savedPtr = approximator.get();
288  ar << boost::serialization::make_nvp("approximator", savedPtr);
289 
290  DMP::FunctionApproximationInterface* appPtr = approximator.get();
291  std::stringstream str;
292  {
293  boost::archive::xml_oarchive ar1(str);
294  ar1 << boost::serialization::make_nvp("approximator", appPtr);
295  }
296  std::cout << str.str() << std::endl;
297 
298  boost::archive::xml_iarchive ari(str);
299  DMP::FunctionApproximationInterface* newPtr;
300  ari >> boost::serialization::make_nvp("approximator", newPtr);
301 
302 
303  file.close();
304 }
305 
armarx::FuncAppPair
std::pair< std::string, DMP::FunctionApproximationInterfacePtr > FuncAppPair
Definition: FunctionApproximator.h:72
str
std::string str(const T &t)
Definition: UserAssistedSegmenterGuiWidgetController.cpp:42
armarx::FunctionApproximator::onInitComponent
void onInitComponent() override
Definition: FunctionApproximator.cpp:29
armarx::FunctionApproximator::onDisconnectComponent
void onDisconnectComponent() override
Definition: FunctionApproximator.cpp:46
armarx::FunctionApproximator::predict
Ice::Double predict(const std::string &name, const Ice::DoubleSeq &, const Ice::Current &) override
Definition: FunctionApproximator.cpp:103
armarx::FunctionApproximator::bpredict
Ice::DoubleSeq bpredict(const std::string &name, const Ice::DoubleSeq &x, const Ice::Current &) override
Definition: FunctionApproximator.cpp:167
armarx::FunctionApproximator::learn
void learn(const std::string &name, const DVector2d &, const DVector2d &, const Ice::Current &) override
Definition: FunctionApproximator.cpp:59
armarx::FunctionApproximator::onExitComponent
void onExitComponent() override
Definition: FunctionApproximator.cpp:53
armarx::FunctionApproximator::saveFunctionApproximatorInFile
void saveFunctionApproximatorInFile(const std::string &funcName, const std::string &name, const Ice::Current &) override
Definition: FunctionApproximator.cpp:252
armarx::VariantType::Double
const VariantTypeId Double
Definition: Variant.h:919
armarx::FunctionApproximator::onConnectComponent
void onConnectComponent() override
Definition: FunctionApproximator.cpp:40
armarx::control::common::mp::DVec
Ice::DoubleSeq DVec
Definition: MP.h:46
armarx::FunctionApproximator::reset
void reset(const Ice::Current &) override
Definition: FunctionApproximator.cpp:188
armarx::FunctionApproximator::saveFunctionApproximatorInFileTest
void saveFunctionApproximatorInFileTest(const std::string &funcName, const std::string &name)
Definition: FunctionApproximator.cpp:279
filename
std::string filename
Definition: VisualizationRobot.cpp:83
armarx::FunctionApproximator::initialize
void initialize(const std::string &fappName, const Ice::DoubleSeq &factors, const Ice::Current &) override
Definition: FunctionApproximator.cpp:207
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
armarx::FunctionApproximator::blearn
void blearn(const std::string &name, const Ice::DoubleSeq &, const Ice::DoubleSeq &, const Ice::Current &) override
Definition: FunctionApproximator.cpp:147
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
FunctionApproximator.h
armarx::FunctionApproximator::ilearn
void ilearn(const std::string &name, const Ice::DoubleSeq &, Ice::Double, const Ice::Current &) override
Definition: FunctionApproximator.cpp:126
armarx::FunctionApproximator::getFunctionApproximatorFromFile
void getFunctionApproximatorFromFile(const std::string &funcName, const std::string &name, const Ice::Current &) override
Definition: FunctionApproximator.cpp:217
armarx::FunctionApproximator::getFunctionApproximatorsFromFile
void getFunctionApproximatorsFromFile(const std::vector< std::string > &funcNameList, const std::string &filename, const Ice::Current &) override
Definition: FunctionApproximator.cpp:232
armarx::FunctionApproximator::initializeTest
void initializeTest(const std::string &fappName, const Ice::DoubleSeq &widthFactors)
Definition: FunctionApproximator.cpp:269
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28