DMPComponent.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2015-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package RobotAPI::ArmarXObjects::DMPComponent
19  * @author Mirko Waechter ( mirko dot waechter at kit dot edu )
20  * @date 2015
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
25 #include "DMPComponent.h"
26 #include <Ice/ObjectAdapter.h>
27 
28 #pragma GCC diagnostic push
29 #pragma GCC diagnostic ignored "-Wunknown-pragmas"
30 #pragma GCC diagnostic ignored "-Wunused-but-set-variable"
31 #include <dmp/io/MMMConverter.h>
32 #include <dmp/testing/testdataset.h>
33 #pragma GCC diagnostic pop
34 
37 
38 using namespace armarx;
39 
40 
41 
43 {
44  ARMARX_INFO << "initializing DMP component";
45  usingProxy(getProperty<std::string>("LongtermMemoryName").getValue());
46  ARMARX_INFO << "successfully initialized DMP component" ;
47 }
48 
49 
51 {
52  ARMARX_INFO << "connecting DMP component";
53 
54  try
55  {
56  longtermMemoryPrx = getProxy<memoryx::LongtermMemoryInterfacePrx>(getProperty<std::string>("LongtermMemoryName").getValue());
57  }
58  catch (...)
59  {
60  ARMARX_ERROR << "cannot get longterm memory proxy";
61  return;
62  }
63 
64  try
65  {
66  dmpDataMemoryPrx = longtermMemoryPrx->getDMPSegment();
67  }
68  catch (...)
69  {
70  ARMARX_ERROR << "cannot get dmp segment of longterm memory";
71  return;
72  }
73 
74  ARMARX_INFO << "successfully connected DMP component";
75 }
76 
77 
79 {
80  ARMARX_INFO << "disconnecting DMP component";
81 }
82 
83 
85 {
86  ARMARX_INFO << "exiting DMP component";
87 }
88 
89 DMPInstanceBasePrx DMPComponent::findInstancePrx(std::string name)
90 {
91  DMPInstancePair dmpInstPair = dmpPool.find(name)->second;
92  DMPInstanceBasePrx result = dmpInstPair.second;
93  if (result)
94  {
95  return result;
96  }
97  else
98  {
99  ARMARX_INFO << "DMP not found.";
100  return DMPInstanceBasePrx();
101  }
102 }
103 
104 DMPInstancePtr DMPComponent::findInstancePtr(std::string name)
105 {
106  DMPInstancePair dmpInstPair = dmpPool.find(name)->second;
107  DMPInstancePtr result = dmpInstPair.first;
108  if (result)
109  {
110  return result;
111  }
112  else
113  {
114  //TODO Correct?
115  ARMARX_INFO << "DMP not found.";
116  return nullptr;
117  }
118 }
119 
120 
121 DMPInstanceBasePrx DMPComponent::getDMP(const std::string& dmpName, const Ice::Current&)
122 {
123  return findInstancePrx(dmpName);
124 }
125 
126 DMPInstanceBasePrx DMPComponent::instantiateDMP(const std::string& dmpName, const std::string& DMPType, int kernelSize, const Ice::Current&)
127 {
128  ARMARX_INFO << "instantiate DMP with name " << dmpName;
129 
130  boost::serialization::extended_type_info const* eti = boost::serialization::extended_type_info::find(DMPType.c_str());
131 
132  DMPInterfacePtr dmp;
133 
134  if (eti)
135  {
136  dmp.reset(static_cast<DMP::DMPInterface*>(eti->construct()));
137  }
138  else
139  {
140  DMPFactory dmpFactory;
141  dmp = dmpFactory.getDMP(DMPType, kernelSize);
142  }
143 
144  DMPInstancePtr dmpPtr = new DMPInstance(dmp, DMPType);
145  dmpPtr->setTimeStep(timestep);
146  dmpPtr->setCurrentTime(ctime);
147 
148  DMPInstanceBasePrx instprx = createDMPInstancePrx(dmpPtr);
149  DMPInstancePair dmpInstPair(dmpPtr, instprx);
150  dmpPool.insert(DMPPair(dmpName, dmpInstPair));
151 
152  ARMARX_INFO << "Successfully instantiate DMP ... ";
153  return instprx;
154 }
155 
156 
157 // Database related functions
158 DMPInstanceBasePrx DMPComponent::getDMPFromDatabase(const std::string& dmpEntityName, const std::string& dmpName, const Ice::Current&)
159 {
160  ARMARX_INFO << "getting DMP from database";
161 
162  if (!dmpDataMemoryPrx->hasEntityByName(dmpEntityName))
163  {
164  ARMARX_ERROR << "DMP with name " + dmpEntityName + " does not exist in the database";
165  return DMPInstanceBasePrx();
166  }
167 
168  memoryx::DMPEntityPtr dmpEntity = memoryx::DMPEntityPtr::dynamicCast(dmpDataMemoryPrx->getDMPEntityByName(dmpEntityName));
169 
170 
171  std::string textStr = dmpEntity->getDMPtextStr();
172  ARMARX_INFO << textStr;
173  std::stringstream istr;
174  istr.str(textStr);
175 
176  boost::archive::text_iarchive ar(istr);
177 
178  DMP::DMPInterface* readPtr;
179 
180  ar >> boost::serialization::make_nvp("dmp", readPtr);
181 
182  DMP::DMPInterfacePtr dptr(readPtr);
183  DMPInstancePtr dmpPtr(new DMPInstance(dptr));
184 
185  DMPInstanceBasePrx dmpPrx = createDMPInstancePrx(dmpPtr);
186  std::string dmpLocalName = dmpName;
187  if (dmpName == "UNKNOWN")
188  {
189  dmpLocalName = dmpPrx->ice_getIdentity().name;
190  }
191 
192  if (dmpPool.find(dmpLocalName) != dmpPool.end())
193  {
194  ARMARX_ERROR << "DMP with DMPName " + dmpLocalName + " exists in DMP pool. It is not allowed to overwrite it.";
195  return dmpPrx;
196  }
197  dmpPtr->setTimeStep(timestep);
198  dmpPtr->setCurrentTime(ctime);
199  dmpPool.insert(DMPPair(dmpLocalName, DMPInstancePair(dmpPtr, dmpPrx)));
200 
201 
202  ARMARX_INFO << "DMP with name: " + dmpEntityName + " is loaded into DMP Pool with dmpName " + dmpLocalName + " and dmpType " + dmpPtr->getDMPType();
203  ARMARX_INFO << "successfully got dmp from database.";
204 
205  return dmpPrx;
206 }
207 
208 DMPInstanceBasePrx DMPComponent::getDMPFromDatabaseById(const std::string& dbId, const Ice::Current&)
209 {
210  ARMARX_INFO << "getting DMP from database";
211 
212  if (!dmpDataMemoryPrx->hasEntityById(dbId))
213  {
214  ARMARX_ERROR << "DMP with ID " + dbId + " does not exist in the database";
215  return DMPInstanceBasePrx();
216  }
217 
218  memoryx::DMPEntityPtr dmpEntity = memoryx::DMPEntityPtr::dynamicCast(dmpDataMemoryPrx->getDMPEntityById(dbId));
219  std::string dmpEntityName = dmpEntity->getDMPName();
220 
221  return getDMPFromDatabase(dmpEntityName);
222 
223 }
224 
225 DMPInstanceBasePrx DMPComponent::getDMPFromFile(const std::string& fileName, const std::string& dmpName, const Ice::Current&)
226 {
227  ARMARX_INFO << "getting DMP from file";
228 
229  if (!std::filesystem::exists(fileName))
230  {
231  ARMARX_ERROR << "The dmp file: " << fileName << " does not exist!!!";
232  return DMPInstanceBasePrx();
233  }
234 
235  std::string ext = fileName.rfind(".") == fileName.npos ? fileName : fileName.substr(fileName.rfind(".") + 1);
236 
237  std::ifstream file(fileName);
238  DMP::DMPInterface* readPtr;
239 
240  if (ext == "xml")
241  {
242  boost::archive::xml_iarchive ar(file);
243  ar >> boost::serialization::make_nvp("dmp", readPtr);
244  }
245  else
246  {
247  ARMARX_ERROR << "The extension of the file cannot be recognized!!!";
248  return DMPInstanceBasePrx();
249  }
250 
251 
252  DMP::DMPInterfacePtr dptr(readPtr);
253  DMPInstancePtr dmpPtr(new DMPInstance(dptr));
254  DMPInstanceBasePrx dmpPrx = createDMPInstancePrx(dmpPtr);
255 
256  std::string dmpLocalName = dmpName;
257  if (dmpLocalName == "UNKNOWN" || dmpLocalName == "")
258  {
259  // dmpLocalName = dptr->getDMPName();
260 
261  // if (dmpLocalName == "UNKNOWN")
262  // {
263  dmpLocalName = dmpPrx->ice_getIdentity().name;
264  // }
265  }
266  dmpPtr->setTimeStep(timestep);
267  dmpPtr->setCurrentTime(ctime);
268  if (dmpPool.find(dmpLocalName) != dmpPool.end())
269  {
270  ARMARX_ERROR << "DMP with DMPName " + dmpLocalName + " exists in DMP pool. It is not allowed to overwrite it.";
271  return dmpPrx;
272  }
273 
274  dmpPool.insert(DMPPair(dmpLocalName, DMPInstancePair(dmpPtr, dmpPrx)));
275 
276  ARMARX_INFO << "DMP with name: " + dmpName + " is loaded into DMP Pool with dmpName " + dmpLocalName + " and dmpType " + dmpPtr->getDMPType();
277  ARMARX_INFO << "successfully got dmp from file.";
278  file.close();
279 
280  return dmpPrx;
281 }
282 
283 void DMPComponent::storeDMPInFile(const std::string& fileName, const std::string& dmpName, const Ice::Current&)
284 {
285  ARMARX_INFO << "storing DMP in the file";
286 
287  std::string ext = fileName.rfind(".") == fileName.npos ? fileName : fileName.substr(fileName.rfind(".") + 1);
288  ofstream ofs(fileName);
289 
290  DMPInstancePtr dmpPtr = findInstancePtr(dmpName);
291  DMPInterface* savedPtr = dmpPtr->getDMP().get();
292  // savedPtr->setDMPName(dmpName);
293 
294  if (ext == "xml")
295  {
296  boost::archive::xml_oarchive ar(ofs);
297  ar << boost::serialization::make_nvp("dmp", savedPtr);
298  }
299  else
300  {
301  ARMARX_ERROR << "The extension of the file cannot be recognized!!!";
302  return;
303  }
304 
305  ARMARX_INFO << "successfully stored DMP in the file!!!";
306  ofs.close();
307 
308  return;
309 }
310 
311 void DMPComponent::storeDMPInDatabase(const std::string& dmpName, const std::string& dmpEntityName, const ::Ice::Current&)
312 {
313 
314  ARMARX_INFO << "storing DMP in the database";
315  memoryx::DMPEntityPtr dmpEntity = new memoryx::DMPEntity(dmpEntityName);
316 
317  DMPInstancePtr dmpPtr = findInstancePtr(dmpName);
318 
319  dmpEntity->setDMPType(dmpPtr->getDMP()->getDMPType());
320  dmpEntity->setDMPName(dmpName);
321 
322  std::stringstream dmptext;
323  boost::archive::text_oarchive ar(dmptext);
324 
325  DMPInterface* savedPtr = dmpPtr->getDMP().get();
326  ar << boost::serialization::make_nvp("dmp", savedPtr);
327 
328  dmpEntity->setDMPtextStr(dmptext.str());
329 
330  std::cout << dmpEntity << std::endl;
331  const std::string entityID = dmpDataMemoryPrx->addEntity(dmpEntity);
332  dmpEntity->setId(entityID);
333 
334  ARMARX_INFO << "successfully stored DMP";
335 
336 }
337 
338 void DMPComponent::removeDMPFromDatabase(const std::string& dmpEntityName, const ::Ice::Current&)
339 {
340 
341  ARMARX_INFO << "removing DMP from database";
342 
343  if (!dmpDataMemoryPrx->hasEntityByName(dmpEntityName))
344  {
345  ARMARX_ERROR << "DMP with name " + dmpEntityName + " does not exist in the database";
346  return;
347  }
348 
349  memoryx::DMPEntityPtr dmpEntity = memoryx::DMPEntityPtr::dynamicCast(dmpDataMemoryPrx->getDMPEntityByName(dmpEntityName));
350  dmpDataMemoryPrx->removeEntity(dmpEntity->getId());
351 
352  if (!dmpDataMemoryPrx->hasEntityByName(dmpEntityName))
353  {
354  ARMARX_INFO << "successfully removed dmp from database";
355  }
356 }
357 
358 void DMPComponent::removeDMPFromDatabaseById(const std::string& dbId, const Ice::Current&)
359 {
360  ARMARX_INFO << "removing DMP from database";
361 
362  if (!dmpDataMemoryPrx->hasEntityById(dbId))
363  {
364  ARMARX_ERROR << "DMP with id " + dbId + " does not exist in the database";
365  return;
366  }
367 
368  memoryx::DMPEntityPtr dmpEntity = memoryx::DMPEntityPtr::dynamicCast(dmpDataMemoryPrx->getDMPEntityById(dbId));
369  dmpDataMemoryPrx->removeEntity(dmpEntity->getId());
370 
371  if (!dmpDataMemoryPrx->hasEntityById(dbId))
372  {
373  ARMARX_INFO << "successfully removed dmp from database";
374  }
375 }
376 
377 
378 // DMP trainner
379 void DMPComponent::trainDMP(const std::string& dmpName, const ::Ice::Current&)
380 {
381  // learn dmp
382  ARMARX_INFO << "In Train DMP (name: " << dmpName << ")";
383 
384  DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
385 
386  if (dmpPrx)
387  {
388  dmpPrx->trainDMP();
389  }
390  else
391  {
392  ARMARX_ERROR << "trainDMP: DMP not found.";
393  return;
394  }
395  ARMARX_INFO << "Exit Train DMP (name: " << dmpName << ")";
396 }
397 
398 void DMPComponent::readTrajectoryFromFile(const std::string& dmpName, const std::string& file, double times, const Ice::Current&)
399 {
400 
401  DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
402 
403  if (dmpPrx)
404  {
405  dmpPrx->readTrajectoryFromFile(file, times);
406  }
407  else
408  {
409  ARMARX_ERROR << "readTrajectoryFromFile: DMP not found.";
410  return;
411  }
412 
413 }
414 
415 
416 // convenient setter functions
417 void DMPComponent::setDMPState(const std::string& dmpName, const ::armarx::cStateVec& state, const ::Ice::Current&)
418 {
419  DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
420 
421  if (dmpPrx)
422  {
423  dmpPrx->setDMPState(state);
424  }
425  else
426  {
427  ARMARX_INFO << "setDMPState: DMP not found.";
428  return;
429  }
430 }
431 
432 void DMPComponent::setAmpl(const std::string& dmpName, int dim, double value, const Ice::Current&)
433 {
434  DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
435 
436  if (dmpPrx)
437  {
438  dmpPrx->setAmpl(dim, value);
439  }
440  else
441  {
442  ARMARX_ERROR << "setAmpl: DMP not found.";
443  return;
444  }
445 }
446 
447 void DMPComponent::setGoal(const std::string& dmpName, const Ice::DoubleSeq& value, const Ice::Current&)
448 {
449  DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
450 
451  if (dmpPrx)
452  {
453  dmpPrx->setGoal(value);
454  }
455  else
456  {
457  ARMARX_ERROR << "setGoal: DMP not found.";
458  return;
459  }
460 }
461 
462 void DMPComponent::setStartPosition(const std::string& dmpName, const Ice::DoubleSeq& value, const Ice::Current&)
463 {
464  DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
465 
466  if (dmpPrx)
467  {
468  dmpPrx->setStartPosition(value);
469  }
470  else
471  {
472  ARMARX_ERROR << "setStartPosition: DMP not found.";
473  }
474 }
475 
476 void DMPComponent::setCanonicalValues(const std::string& dmpName, const Ice::DoubleSeq& value, const Ice::Current&)
477 {
478  DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
479 
480  if (dmpPrx)
481  {
482  return dmpPrx->setCanonicalValues(value);
483  }
484  else
485  {
486  ARMARX_ERROR << "setCanonicalValues: DMP not found.";
487  return;
488  }
489 }
490 
491 void DMPComponent::setTemporalFactor(const std::string& dmpName, double tau, const Ice::Current&)
492 {
493  DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
494 
495  if (dmpPrx)
496  {
497  dmpPrx->setTemporalFactor(tau);
498  }
499  else
500  {
501  ARMARX_ERROR << "getTemporalFactor: DMP not found.";
502  }
503 }
504 
505 void DMPComponent::setTimeStep(double ts, const Ice::Current&)
506 {
507  timestep = ts;
508  for (DMPMap::iterator it = dmpPool.begin(); it != dmpPool.end(); it++)
509  {
510  it->second.first->setTimeStep(ts);
511  }
512 }
513 
514 // convenient getter functions
515 double DMPComponent::getAmpl(const std::string& dmpName, int dim, const Ice::Current&)
516 {
517 
518  DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
519 
520  if (dmpPrx)
521  {
522  return dmpPrx->getAmpl(dim);
523  }
524  else
525  {
526  ARMARX_ERROR << "getAmpl: DMP not found.";
527  return 0;
528  }
529 
530 }
531 
532 cStateVec DMPComponent::getNextState(const std::string& dmpName, const cStateVec& states, const ::Ice::Current&)
533 {
534  cStateVec nextState;
535  DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
536 
537  if (dmpPrx)
538  {
539  nextState = dmpPrx->getNextState(states);
540  }
541  else
542  {
543  ARMARX_ERROR << "getNextState: DMP not found.";
544  }
545 
546  ARMARX_INFO << "Successfully got next state";
547 
548  return nextState;
549 }
550 
551 cStateVec DMPComponent::getCurrentState(const std::string& dmpName, const Ice::Current&)
552 {
553  cStateVec curState;
554  DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
555 
556  if (dmpPrx)
557  {
558  curState = dmpPrx->getCurrentState();
559  }
560  else
561  {
562  ARMARX_ERROR << "getCurrentState: DMP not found.";
563  }
564 
565  return curState;
566 }
567 
568 Ice::DoubleSeq DMPComponent::getCanonicalValues(const std::string& dmpName, const Ice::Current&)
569 {
570  Ice::DoubleSeq canonicalValues;
571 
572  DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
573 
574  if (dmpPrx)
575  {
576  canonicalValues = dmpPrx->getCanonicalValues();
577  }
578  else
579  {
580  ARMARX_ERROR << "getCanonicalValues: DMP not found.";
581  }
582 
583  return canonicalValues;
584 }
585 
586 double DMPComponent::getTemporalFactor(const std::string& dmpName, const Ice::Current&)
587 {
588  double temporalFactor = 0;
589 
590  DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
591 
592  if (dmpPrx)
593  {
594  temporalFactor = dmpPrx->getTemporalFactor();
595  }
596  else
597  {
598  ARMARX_ERROR << "getTemporalFactor: DMP not found.";
599  }
600 
601  return temporalFactor;
602 }
603 
604 double DMPComponent::getCurrentTime(const Ice::Current&)
605 {
606 
607  DMPMap::iterator it = dmpPool.begin();
608 
609  return it->second.first->getCurrentTime();
610 
611 
612 }
613 
614 double DMPComponent::getDampingFactor(const std::string& dmpName, const Ice::Current&)
615 {
616  double dampingFactor = 0;
617  DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
618 
619  if (dmpPrx)
620  {
621  dampingFactor = dmpPrx->getDampingFactor();
622  }
623  else
624  {
625  ARMARX_ERROR << "getDampingFactor: DMP not found.";
626  }
627  return dampingFactor;
628 }
629 
630 double DMPComponent::getSpringFactor(const std::string& dmpName, const Ice::Current&)
631 {
632  double springFactor{0};
633  DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
634 
635  if (dmpPrx)
636  {
637  springFactor = dmpPrx->getSpringFactor();
638  }
639  else
640  {
641  ARMARX_ERROR << "getSpringFactor: DMP not found.";
642  }
643  return springFactor;
644 }
645 
646 int DMPComponent::getDMPDim(const std::string& dmpName, const Ice::Current&)
647 {
648  int dim = -1;
649  DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
650 
651  if (dmpPrx)
652  {
653  dim = dmpPrx->getDMPDim();
654  }
655  else
656  {
657  ARMARX_ERROR << "getDMPDim: DMP not found.";
658  }
659 
660  return dim;
661 }
662 
663 string DMPComponent::getDMPType(const std::string& dmpName, const Ice::Current&)
664 {
665  std::string dmpType;
666  DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
667 
668  if (dmpPrx)
669  {
670  dmpType = dmpPrx->getDMPType();
671  }
672  else
673  {
674  ARMARX_ERROR << "getDMPType: DMP not found.";
675  }
676  return dmpType;
677 }
678 
679 Ice::DoubleSeq DMPComponent::getStartPosition(const std::string& dmpName, const Ice::Current&)
680 {
681  Ice::DoubleSeq startPositions;
682  DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
683 
684  if (dmpPrx)
685  {
686  startPositions = dmpPrx->getStartPosition();
687  }
688  else
689  {
690  ARMARX_ERROR << "getStartPosition: DMP not found.";
691  }
692  return startPositions;
693 }
694 
695 double DMPComponent::getForceTerm(const std::string& dmpName, const Ice::DoubleSeq& canonicalValues, int dim, const Ice::Current&)
696 {
697  double forceterm{0};
698  DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
699 
700  if (dmpPrx)
701  {
702  forceterm = dmpPrx->getForceTerm(canonicalValues, dim);
703  }
704  else
705  {
706  ARMARX_ERROR << "getDMPDim: DMP not found.";
707  }
708 
709  return forceterm;
710 }
711 
712 
713 
714 
715 
716 Vec2D DMPComponent::calcTrajectory(const std::string& dmpName, double startTime, double timeStep, double endTime, const Ice::DoubleSeq& goal, const cStateVec& states, const Ice::DoubleSeq& canonicalValues, double temporalFactor, const Ice::Current&)
717 {
718  DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
719 
720  if (dmpPrx)
721  {
722  return dmpPrx->calcTrajectory(startTime, timeStep, endTime, goal, states,
723  canonicalValues, temporalFactor);
724  }
725  else
726  {
727  ARMARX_ERROR << "getTemporalFactor: DMP not found.";
728  }
729  return {};
730 }
731 
732 
733 
734 void DMPComponent::resetTime(const Ice::Current&)
735 {
736  ctime = 0;
737  for (DMPMap::iterator it = dmpPool.begin(); it != dmpPool.end(); it++)
738  {
739  it->second.first->setCurrentTime(ctime);
740  }
741 
742 }
743 
744 void DMPComponent::resetCanonicalValues(const Ice::Current&)
745 {
746  for (DMPMap::iterator it = dmpPool.begin(); it != dmpPool.end(); it++)
747  {
748  ::Ice::DoubleSeq canvals;
749  DMPInstancePtr dmpPtr = it->second.first;
750  if (dmpPtr->getDMPType() == "PeriodicDMP")
751  {
752  canvals.push_back(0.0);
753  dmpPtr->setCanonicalValues(canvals);
754  }
755  else
756  {
757  canvals.push_back(1.0);
758  dmpPtr->setCanonicalValues(canvals);
759  }
760 
761  }
762 
763 }
764 
765 
766 // Trajectory related
767 Ice::DoubleSeq DMPComponent::getTrajGoal(const std::string& dmpName, const Ice::Current&)
768 {
769  Ice::DoubleSeq trajGoal;
770  DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
771 
772  if (dmpPrx)
773  {
774  trajGoal = dmpPrx->getTrajGoal();
775  }
776  else
777  {
778  ARMARX_ERROR << "getTrajGoal: DMP not found.";
779  }
780 
781  return trajGoal;
782 }
783 
784 cStateVec DMPComponent::getTrajStartState(const std::string& dmpName, const Ice::Current&)
785 {
786  cStateVec trajStartState;
787  DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
788 
789  if (dmpPrx)
790  {
791  trajStartState = dmpPrx->getTrajStartState();
792  }
793  else
794  {
795  ARMARX_ERROR << "getTrajStartState: DMP not found.";
796  }
797 
798  return trajStartState;
799 
800 }
801 
802 
803 
804 // DMP manager
805 SVector DMPComponent::getDMPNameList(const Ice::Current&)
806 {
807  SVector nameList;
808 
809  for (DMPMap::iterator it = dmpPool.begin(); it != dmpPool.end(); it++)
810  {
811  nameList.push_back(it->first);
812  }
813 
814  return nameList;
815 }
816 
817 void DMPComponent::eraseDMP(const std::string& dmpName, const Ice::Current&)
818 {
819  DMPMap::iterator dmpPoolIt = dmpPool.find(dmpName);
820 
821  if (dmpPoolIt != dmpPool.end())
822  {
823  dmpPool.erase(dmpPoolIt);
824  }
825 }
826 
827 bool DMPComponent::isDMPExist(const std::string& dmpName, const Ice::Current&)
828 {
829  if (dmpPool.find(dmpName) == dmpPool.end())
830  {
831  return false;
832  }
833  else
834  {
835  return true;
836  }
837 }
838 
839 
840 
841 // local functions
842 DMPInstanceBasePrx DMPComponent::createDMPInstancePrx(DMPInstancePtr dmpPtr)
843 {
845  Ice::ObjectPrx obj = adapter->addWithUUID(dmpPtr);
846 
847  return DMPInstanceBasePrx::uncheckedCast(obj);
848 }
849 
850 //PropertyDefinitionsPtr DMPComponent::createPropertyDefinitions()
851 //{
852 // return PropertyDefinitionsPtr(new DMPComponentPropertyDefinitions(
853 // getConfigIdentifier()));
854 //}
855 
DMPComponent.h
armarx::DMPComponent::getTrajStartState
::armarx::cStateVec getTrajStartState(const std::string &dmpName, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DMPComponent.cpp:784
armarx::DMPComponent::calcTrajectory
Vec2D calcTrajectory(const std::string &dmpName, double startTime, double timeStep, double endTime, const ::Ice::DoubleSeq &goal, const cStateVec &states, const ::Ice::DoubleSeq &canonicalValues, double temporalFactor, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DMPComponent.cpp:716
armarx::DMPComponent::instantiateDMP
DMPInstanceBasePrx instantiateDMP(const std::string &dmpName, const std::string &DMPType, int kernelSize, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DMPComponent.cpp:126
armarx::DMPComponent::getDMPType
std::string getDMPType(const std::string &dmpName, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DMPComponent.cpp:663
armarx::DMPInstancePair
std::pair< DMPInstancePtr, DMPInstanceBasePrx > DMPInstancePair
Definition: DMPComponent.h:105
armarx::DMPComponent::setTemporalFactor
void setTemporalFactor(const std::string &dmpName, double tau, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DMPComponent.cpp:491
armarx::ManagedIceObject::getObjectAdapter
Ice::ObjectAdapterPtr getObjectAdapter() const
Returns object's Ice adapter.
Definition: ManagedIceObject.cpp:140
armarx::DMPComponent::setAmpl
void setAmpl(const std::string &dmpName, int dim, double value, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DMPComponent.cpp:432
armarx::DMPComponent::dmpPool
DMPMap dmpPool
Definition: DMPComponent.h:231
armarx::DMPComponent::getDMPFromDatabaseById
DMPInstanceBasePrx getDMPFromDatabaseById(const std::string &dbId, const Ice::Current &) override
Definition: DMPComponent.cpp:208
armarx::DMPComponent::isDMPExist
bool isDMPExist(const std::string &dmpName, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DMPComponent.cpp:827
armarx::DMPComponent::ctime
double ctime
Definition: DMPComponent.h:221
armarx::DMPComponent::storeDMPInFile
void storeDMPInFile(const std::string &fileName, const std::string &dmpName, const Ice::Current &) override
Definition: DMPComponent.cpp:283
armarx::DMPComponent::setTimeStep
void setTimeStep(double ts, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DMPComponent.cpp:505
armarx::DMPComponent::longtermMemoryPrx
memoryx::LongtermMemoryInterfacePrx longtermMemoryPrx
Definition: DMPComponent.h:233
armarx::DMPComponent::setGoal
void setGoal(const std::string &dmpName, const Ice::DoubleSeq &value, const Ice::Current &=Ice::emptyCurrent) override
Definition: DMPComponent.cpp:447
armarx::DMPComponent::setStartPosition
void setStartPosition(const std::string &dmpName, const Ice::DoubleSeq &value, const Ice::Current &=Ice::emptyCurrent) override
Definition: DMPComponent.cpp:462
armarx::DMPComponent::trainDMP
void trainDMP(const std::string &dmpName, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DMPComponent.cpp:379
armarx::DMPComponent::onConnectComponent
void onConnectComponent() override
Definition: DMPComponent.cpp:50
armarx::DMPComponent::getDMP
DMPInstanceBasePrx getDMP(const std::string &dmpName, const Ice::Current &=Ice::emptyCurrent) override
Definition: DMPComponent.cpp:121
IceInternal::Handle
Definition: forward_declarations.h:8
armarx::DMPInstance
Definition: DMPInstance.h:82
armarx::DMPComponent::setCanonicalValues
void setCanonicalValues(const std::string &dmpName, const Ice::DoubleSeq &value, const Ice::Current &=Ice::emptyCurrent) override
Definition: DMPComponent.cpp:476
armarx::DMPComponent::getCurrentTime
double getCurrentTime(const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DMPComponent.cpp:604
memoryx::DMPEntity
Definition: DMPEntity.h:37
cxxopts::value
std::shared_ptr< Value > value()
Definition: cxxopts.hpp:926
armarx::DMPComponent::removeDMPFromDatabaseById
void removeDMPFromDatabaseById(const std::string &dbId, const Ice::Current &) override
Definition: DMPComponent.cpp:358
armarx::DMPComponent::onExitComponent
void onExitComponent() override
Definition: DMPComponent.cpp:84
MemoryXCoreObjectFactories.h
armarx::DMPComponent::dmpDataMemoryPrx
memoryx::PersistentDMPDataSegmentBasePrx dmpDataMemoryPrx
Definition: DMPComponent.h:234
armarx::DMPComponent::getDMPFromFile
DMPInstanceBasePrx getDMPFromFile(const std::string &fileName, const std::string &dmpName="UNKNOWN", const Ice::Current &=Ice::emptyCurrent) override
Definition: DMPComponent.cpp:225
armarx::DMPComponent::getCanonicalValues
Ice::DoubleSeq getCanonicalValues(const std::string &dmpName, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DMPComponent.cpp:568
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
armarx::DMPComponent::setDMPState
void setDMPState(const std::string &dmpName, const ::armarx::cStateVec &state, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DMPComponent.cpp:417
armarx::DMPComponent::timestep
double timestep
Definition: DMPComponent.h:222
armarx::DMPComponent::getAmpl
double getAmpl(const std::string &dmpName, int dim, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DMPComponent.cpp:515
armarx::DMPComponent::onDisconnectComponent
void onDisconnectComponent() override
Definition: DMPComponent.cpp:78
armarx::DMPComponent::getDMPFromDatabase
DMPInstanceBasePrx getDMPFromDatabase(const std::string &dmpEntityName, const std::string &dmpName="UNKNOWN", const Ice::Current &=Ice::emptyCurrent) override
Definition: DMPComponent.cpp:158
armarx::DMPComponent::readTrajectoryFromFile
void readTrajectoryFromFile(const std::string &dmpName, const std::string &file, double times=1, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DMPComponent.cpp:398
armarx::DMPComponent::getDMPDim
int getDMPDim(const std::string &dmpName, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DMPComponent.cpp:646
armarx::DMPPair
std::pair< std::string, std::pair< DMPInstancePtr, DMPInstanceBasePrx > > DMPPair
Definition: DMPComponent.h:106
armarx::DMPComponent::onInitComponent
void onInitComponent() override
Definition: DMPComponent.cpp:42
armarx::DMPComponent::getDampingFactor
double getDampingFactor(const std::string &dmpName, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DMPComponent.cpp:614
armarx::DMPComponent::getCurrentState
cStateVec getCurrentState(const std::string &dmpName, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DMPComponent.cpp:551
armarx::DMPComponent::eraseDMP
void eraseDMP(const std::string &dmpName, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DMPComponent.cpp:817
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
armarx::DMPComponent::getStartPosition
Ice::DoubleSeq getStartPosition(const std::string &dmpName, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DMPComponent.cpp:679
armarx::DMPComponent::resetCanonicalValues
void resetCanonicalValues(const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DMPComponent.cpp:744
MemoryXTypesObjectFactories.h
armarx::DMPComponent::removeDMPFromDatabase
void removeDMPFromDatabase(const std::string &name, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DMPComponent.cpp:338
armarx::DMPComponent::getTemporalFactor
double getTemporalFactor(const std::string &dmpName, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DMPComponent.cpp:586
armarx::DMPComponent::storeDMPInDatabase
void storeDMPInDatabase(const std::string &dmpName, const std::string &name, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DMPComponent.cpp:311
armarx::DMPComponent::getSpringFactor
double getSpringFactor(const std::string &dmpName, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DMPComponent.cpp:630
armarx::DMPComponent::getDMPNameList
SVector getDMPNameList(const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DMPComponent.cpp:805
armarx::DMPComponent::getNextState
cStateVec getNextState(const std::string &dmpName, const cStateVec &states, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DMPComponent.cpp:532
armarx::DMPComponent::getTrajGoal
Ice::DoubleSeq getTrajGoal(const std::string &dmpName, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DMPComponent.cpp:767
armarx::ManagedIceObject::usingProxy
bool usingProxy(const std::string &name, const std::string &endpoints="")
Registers a proxy for retrieval after initialization and adds it to the dependency list.
Definition: ManagedIceObject.cpp:151
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::DMPComponent::resetTime
void resetTime(const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DMPComponent.cpp:734
armarx::DMPComponent::getForceTerm
double getForceTerm(const std::string &dmpName, const Ice::DoubleSeq &canonicalValues, int dim, const Ice::Current &=Ice::emptyCurrent) override
Definition: DMPComponent.cpp:695