26 #include <Ice/ObjectAdapter.h>
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
45 usingProxy(getProperty<std::string>(
"LongtermMemoryName").getValue());
46 ARMARX_INFO <<
"successfully initialized DMP component" ;
56 longtermMemoryPrx = getProxy<memoryx::LongtermMemoryInterfacePrx>(getProperty<std::string>(
"LongtermMemoryName").getValue());
70 ARMARX_ERROR <<
"cannot get dmp segment of longterm memory";
74 ARMARX_INFO <<
"successfully connected DMP component";
89 DMPInstanceBasePrx DMPComponent::findInstancePrx(std::string name)
92 DMPInstanceBasePrx result = dmpInstPair.second;
100 return DMPInstanceBasePrx();
123 return findInstancePrx(dmpName);
128 ARMARX_INFO <<
"instantiate DMP with name " << dmpName;
130 boost::serialization::extended_type_info
const* eti = boost::serialization::extended_type_info::find(DMPType.c_str());
136 dmp.reset(
static_cast<DMP::DMPInterface*
>(eti->construct()));
140 DMPFactory dmpFactory;
141 dmp = dmpFactory.getDMP(DMPType, kernelSize);
146 dmpPtr->setCurrentTime(
ctime);
148 DMPInstanceBasePrx instprx = createDMPInstancePrx(dmpPtr);
152 ARMARX_INFO <<
"Successfully instantiate DMP ... ";
164 ARMARX_ERROR <<
"DMP with name " + dmpEntityName +
" does not exist in the database";
165 return DMPInstanceBasePrx();
171 std::string textStr = dmpEntity->getDMPtextStr();
173 std::stringstream istr;
176 boost::archive::text_iarchive ar(istr);
178 DMP::DMPInterface* readPtr;
180 ar >> boost::serialization::make_nvp(
"dmp", readPtr);
182 DMP::DMPInterfacePtr dptr(readPtr);
185 DMPInstanceBasePrx dmpPrx = createDMPInstancePrx(dmpPtr);
186 std::string dmpLocalName = dmpName;
187 if (dmpName ==
"UNKNOWN")
189 dmpLocalName = dmpPrx->ice_getIdentity().name;
194 ARMARX_ERROR <<
"DMP with DMPName " + dmpLocalName +
" exists in DMP pool. It is not allowed to overwrite it.";
198 dmpPtr->setCurrentTime(
ctime);
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.";
214 ARMARX_ERROR <<
"DMP with ID " + dbId +
" does not exist in the database";
215 return DMPInstanceBasePrx();
219 std::string dmpEntityName = dmpEntity->getDMPName();
229 if (!std::filesystem::exists(fileName))
231 ARMARX_ERROR <<
"The dmp file: " << fileName <<
" does not exist!!!";
232 return DMPInstanceBasePrx();
235 std::string ext = fileName.rfind(
".") == fileName.npos ? fileName : fileName.substr(fileName.rfind(
".") + 1);
237 std::ifstream file(fileName);
238 DMP::DMPInterface* readPtr;
242 boost::archive::xml_iarchive ar(file);
243 ar >> boost::serialization::make_nvp(
"dmp", readPtr);
247 ARMARX_ERROR <<
"The extension of the file cannot be recognized!!!";
248 return DMPInstanceBasePrx();
252 DMP::DMPInterfacePtr dptr(readPtr);
254 DMPInstanceBasePrx dmpPrx = createDMPInstancePrx(dmpPtr);
256 std::string dmpLocalName = dmpName;
257 if (dmpLocalName ==
"UNKNOWN" || dmpLocalName ==
"")
263 dmpLocalName = dmpPrx->ice_getIdentity().name;
267 dmpPtr->setCurrentTime(
ctime);
270 ARMARX_ERROR <<
"DMP with DMPName " + dmpLocalName +
" exists in DMP pool. It is not allowed to overwrite it.";
276 ARMARX_INFO <<
"DMP with name: " + dmpName +
" is loaded into DMP Pool with dmpName " + dmpLocalName +
" and dmpType " + dmpPtr->getDMPType();
287 std::string ext = fileName.rfind(
".") == fileName.npos ? fileName : fileName.substr(fileName.rfind(
".") + 1);
288 ofstream ofs(fileName);
291 DMPInterface* savedPtr = dmpPtr->getDMP().get();
296 boost::archive::xml_oarchive ar(ofs);
297 ar << boost::serialization::make_nvp(
"dmp", savedPtr);
301 ARMARX_ERROR <<
"The extension of the file cannot be recognized!!!";
305 ARMARX_INFO <<
"successfully stored DMP in the file!!!";
319 dmpEntity->setDMPType(dmpPtr->getDMP()->getDMPType());
320 dmpEntity->setDMPName(dmpName);
322 std::stringstream dmptext;
323 boost::archive::text_oarchive ar(dmptext);
325 DMPInterface* savedPtr = dmpPtr->getDMP().get();
326 ar << boost::serialization::make_nvp(
"dmp", savedPtr);
328 dmpEntity->setDMPtextStr(dmptext.str());
330 std::cout << dmpEntity << std::endl;
332 dmpEntity->setId(entityID);
345 ARMARX_ERROR <<
"DMP with name " + dmpEntityName +
" does not exist in the database";
354 ARMARX_INFO <<
"successfully removed dmp from database";
364 ARMARX_ERROR <<
"DMP with id " + dbId +
" does not exist in the database";
373 ARMARX_INFO <<
"successfully removed dmp from database";
382 ARMARX_INFO <<
"In Train DMP (name: " << dmpName <<
")";
384 DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
395 ARMARX_INFO <<
"Exit Train DMP (name: " << dmpName <<
")";
401 DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
405 dmpPrx->readTrajectoryFromFile(file, times);
409 ARMARX_ERROR <<
"readTrajectoryFromFile: DMP not found.";
419 DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
423 dmpPrx->setDMPState(state);
434 DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
438 dmpPrx->setAmpl(dim,
value);
449 DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
453 dmpPrx->setGoal(
value);
464 DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
468 dmpPrx->setStartPosition(
value);
478 DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
482 return dmpPrx->setCanonicalValues(
value);
493 DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
497 dmpPrx->setTemporalFactor(tau);
508 for (DMPMap::iterator it =
dmpPool.begin(); it !=
dmpPool.end(); it++)
510 it->second.first->setTimeStep(ts);
518 DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
522 return dmpPrx->getAmpl(dim);
535 DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
539 nextState = dmpPrx->getNextState(states);
554 DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
558 curState = dmpPrx->getCurrentState();
570 Ice::DoubleSeq canonicalValues;
572 DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
576 canonicalValues = dmpPrx->getCanonicalValues();
583 return canonicalValues;
588 double temporalFactor = 0;
590 DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
594 temporalFactor = dmpPrx->getTemporalFactor();
601 return temporalFactor;
607 DMPMap::iterator it =
dmpPool.begin();
609 return it->second.first->getCurrentTime();
616 double dampingFactor = 0;
617 DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
621 dampingFactor = dmpPrx->getDampingFactor();
627 return dampingFactor;
632 double springFactor{0};
633 DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
637 springFactor = dmpPrx->getSpringFactor();
649 DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
653 dim = dmpPrx->getDMPDim();
666 DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
670 dmpType = dmpPrx->getDMPType();
681 Ice::DoubleSeq startPositions;
682 DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
686 startPositions = dmpPrx->getStartPosition();
692 return startPositions;
698 DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
702 forceterm = dmpPrx->getForceTerm(canonicalValues, dim);
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&)
718 DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
722 return dmpPrx->calcTrajectory(startTime, timeStep, endTime, goal, states,
723 canonicalValues, temporalFactor);
737 for (DMPMap::iterator it =
dmpPool.begin(); it !=
dmpPool.end(); it++)
739 it->second.first->setCurrentTime(
ctime);
746 for (DMPMap::iterator it =
dmpPool.begin(); it !=
dmpPool.end(); it++)
748 ::Ice::DoubleSeq canvals;
750 if (dmpPtr->getDMPType() ==
"PeriodicDMP")
752 canvals.push_back(0.0);
753 dmpPtr->setCanonicalValues(canvals);
757 canvals.push_back(1.0);
758 dmpPtr->setCanonicalValues(canvals);
769 Ice::DoubleSeq trajGoal;
770 DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
774 trajGoal = dmpPrx->getTrajGoal();
786 cStateVec trajStartState;
787 DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
791 trajStartState = dmpPrx->getTrajStartState();
798 return trajStartState;
809 for (DMPMap::iterator it =
dmpPool.begin(); it !=
dmpPool.end(); it++)
811 nameList.push_back(it->first);
819 DMPMap::iterator dmpPoolIt =
dmpPool.find(dmpName);
821 if (dmpPoolIt !=
dmpPool.end())
842 DMPInstanceBasePrx DMPComponent::createDMPInstancePrx(
DMPInstancePtr dmpPtr)
845 Ice::ObjectPrx obj = adapter->addWithUUID(dmpPtr);
847 return DMPInstanceBasePrx::uncheckedCast(obj);