DictionaryTrajectory::DictionaryTrajectory(std::string baseFolder, double az, double bz) : Trajectory() {
this->baseFolder = baseFolder;
vector<string> files = getFilesInDirectory(baseFolder);
queryFiles = sortPrefix(files, "query");
trajFiles = sortPrefix(files, "traj");
dmpFiles = sortPrefix(files, "dmp");
if(dmpFiles.size() == 0) {
// learn dmps
queryPoints = mapFiles(queryFiles, trajFiles, "query", "traj");
KUKADU_SHARED_PTR<JointDMPLearner> dmpLearner;
vector<mat> jointsVec;
double tMax = 0.0;
for(int i = 0; i < queryPoints.size(); ++i) {
mat joints = readMovements((string(baseFolder) + string(queryPoints.at(i).getFileDataPath())).c_str());
degOfFreedom = joints.n_cols - 1;
queryPoints.at(i).setQueryPoint(readQuery(string(baseFolder) + string(queryPoints.at(i).getFileQueryPath())));
jointsVec.push_back(joints);
double currentTMax = joints(joints.n_rows - 1, 0);
if(tMax < currentTMax)
tMax = currentTMax;
}
for(int i = 0; i < jointsVec.size(); ++i) {
QueryPoint currentQueryPoint = queryPoints.at(i);
mat joints = jointsVec.at(i);
joints = fillTrajectoryMatrix(joints, tMax);
dmpLearner = KUKADU_SHARED_PTR<JointDMPLearner>(new JointDMPLearner(az, bz, joints));
KUKADU_SHARED_PTR<Dmp> learnedDmps = dmpLearner->fitTrajectories();
learnedDmps->serialize(baseFolder + currentQueryPoint.getFileDmpPath());
queryPoints.at(i).setDmp(learnedDmps);
startingPos = queryPoints.at(i).getDmp()->getY0();
cout << "(DMPGeneralizer) goals for query point [" << currentQueryPoint.getQueryPoint().t() << "]" << endl << "\t [";
cout << currentQueryPoint.getDmp()->getG().t() << "]" << endl;
//delete dmpLearner;
dmpLearner = KUKADU_SHARED_PTR<JointDMPLearner>();
}
} else {
queryPoints = mapFiles(queryFiles, trajFiles, dmpFiles, "query", "traj", "dmp");
}
degOfFreedom = queryPoints.at(0).getDmp()->getDegreesOfFreedom();
}
int main(int argc, char** args) {
double tau, az, bz, dmpStepSize, tolAbsErr, tolRelErr, ac;
string storeDir = resolvePath("/tmp/kukadu_demo_guided");
string prefix = "real";
string arm = "left";
// Declare the supported options.
po::options_description desc("Allowed options");
desc.add_options()
("dmp.tau", po::value<double>(), "tau")
("dmp.az", po::value<double>(), "az")
("dmp.bz", po::value<double>(), "bz")
("dmp.dmpStepSize", po::value<double>(), "dmp time step size")
("dmp.tolAbsErr", po::value<double>(), "tolerated absolute error")
("dmp.tolRelErr", po::value<double>(), "tolerated relative error")
("dmp.ac", po::value<double>(), "ac")
;
ifstream parseFile(resolvePath("$KUKADU_HOME/cfg/guided.prop").c_str(), std::ifstream::in);
po::variables_map vm;
po::store(po::parse_config_file(parseFile, desc), vm);
po::notify(vm);
if (vm.count("dmp.tau")) tau = vm["dmp.tau"].as<double>();
else return 1;
if (vm.count("dmp.az")) az = vm["dmp.az"].as<double>();
else return 1;
if (vm.count("dmp.bz")) bz = vm["dmp.bz"].as<double>();
else return 1;
if (vm.count("dmp.dmpStepSize")) dmpStepSize = vm["dmp.dmpStepSize"].as<double>();
else return 1;
if (vm.count("dmp.tolAbsErr")) tolAbsErr = vm["dmp.tolAbsErr"].as<double>();
else return 1;
if (vm.count("dmp.tolRelErr")) tolRelErr = vm["dmp.tolRelErr"].as<double>();
else return 1;
if (vm.count("dmp.ac")) ac = vm["dmp.ac"].as<double>();
else return 1;
cout << "all properties loaded" << endl;
ros::init(argc, args, "kukadu"); ros::NodeHandle* node = new ros::NodeHandle(); usleep(1e6);
ros::AsyncSpinner spinner(10);
spinner.start();
KUKADU_SHARED_PTR<ControlQueue> leftQueue = KUKADU_SHARED_PTR<ControlQueue>(new KukieControlQueue(dmpStepSize, "real", arm + string("_arm"), *node));
KUKADU_SHARED_PTR<ControlQueue> simLeftQueue = KUKADU_SHARED_PTR<ControlQueue>(new KukieControlQueue(dmpStepSize, "simulation", arm + string("_arm"), *node));
vector<KUKADU_SHARED_PTR<ControlQueue> > queueVectors;
queueVectors.push_back(leftQueue);
deleteDirectory(storeDir);
cout << "press enter to measure trajectory" << endl;
getchar();
leftQueue->stopCurrentMode();
KUKADU_SHARED_PTR<kukadu_thread> laThr = leftQueue->startQueueThread();
if(!prefix.compare("simulation")) {
leftQueue->switchMode(KukieControlQueue::KUKA_JNT_POS_MODE);
} else {
leftQueue->setStiffness(0.2, 0.01, 0.2, 15000, 150, 1500);
leftQueue->switchMode(KukieControlQueue::KUKA_JNT_IMP_MODE);
}
cout << "starting measurement" << endl;
SensorStorage scaredOfSenka(queueVectors, std::vector<KUKADU_SHARED_PTR<GenericHand> >(), 1000);
scaredOfSenka.setExportMode(SensorStorage::STORE_TIME | SensorStorage::STORE_RBT_CART_POS | SensorStorage::STORE_RBT_JNT_POS);
scaredOfSenka.startDataStorage(storeDir);
cout << "measuerment started" << endl;
if(!prefix.compare("simulation")) {
cout << "moving arm in simulation" << endl;
leftQueue->jointPtp(createJointsVector(-0.9692263007164001, 1.113829493522644, 1.1473214626312256, -1.444376826286316, -0.28663957118988037, -0.8957559466362, -0.2651996612548828));
cout << "movement done" << endl;
} else {
ros::Rate r(1);
for(int i = 0; i < 12; ++i) {
r.sleep();
cout << i << endl;
}
}
scaredOfSenka.stopDataStorage();
mes_result finalJoints = leftQueue->getCurrentJoints();
for(int i = 0; i < 10; ++i)
leftQueue->addJointsPosToQueue(finalJoints.joints);
sleep(0.5);
leftQueue->stopCurrentMode();
cout << "press enter to execute in simulation" << endl;
getchar();
KUKADU_SHARED_PTR<Dmp> dmpFinalPush;
KUKADU_SHARED_PTR<SensorData> dataFinalPush;
KUKADU_SHARED_PTR<JointDMPLearner> learnerFinalPush;
arma::vec timesFinalPush;
dataFinalPush = SensorStorage::readStorage(simLeftQueue, storeDir + string("/kuka_lwr_") + prefix + string("_left_arm_0"));
timesFinalPush = dataFinalPush->getTimes();
learnerFinalPush = KUKADU_SHARED_PTR<JointDMPLearner>(new JointDMPLearner(az, bz, join_rows(timesFinalPush, dataFinalPush->getJointPos())));
dmpFinalPush = learnerFinalPush->fitTrajectories();
// simLeftQueue->setStiffness(KukieControlQueue::KUKA_STD_XYZ_STIFF, KukieControlQueue::KUKA_STD_ABC_STIFF, KukieControlQueue::KUKA_STD_CPDAMPING, 15000, 150, 1500);
simLeftQueue->switchMode(KukieControlQueue::KUKA_JNT_POS_MODE);
laThr = simLeftQueue->startQueueThread();
DMPExecutor execFinalPush(dmpFinalPush, simLeftQueue);
execFinalPush.executeTrajectory(ac, 0, dmpFinalPush->getTmax(), tolAbsErr, tolRelErr);
simLeftQueue->stopCurrentMode();
simLeftQueue->setFinish();
laThr->join();
cout << "press enter to execute on robot" << endl;
getchar();
// leftQueue->setStiffness(KukieControlQueue::KUKA_STD_XYZ_STIFF, KukieControlQueue::KUKA_STD_ABC_STIFF, KukieControlQueue::KUKA_STD_CPDAMPING, 15000, 150, 1500);
if(!prefix.compare("simulation")) {
leftQueue->switchMode(KukieControlQueue::KUKA_JNT_POS_MODE);
} else {
leftQueue->setStiffness(KukieControlQueue::KUKA_STD_XYZ_STIFF, KukieControlQueue::KUKA_STD_ABC_STIFF, KukieControlQueue::KUKA_STD_CPDAMPING, 15000, 150, 1500);
leftQueue->switchMode(KukieControlQueue::KUKA_JNT_IMP_MODE);
}
laThr = leftQueue->startQueueThread();
DMPExecutor execFinalPush2(dmpFinalPush, leftQueue);
execFinalPush2.executeTrajectory(ac, 0, dmpFinalPush->getTmax(), tolAbsErr, tolRelErr);
leftQueue->stopCurrentMode();
leftQueue->setFinish();
laThr->join();
}