void SensingController::gatherData(std::string completePath) {
vector<KUKADU_SHARED_PTR<ControlQueue> > castedQueues;
for(int i = 0; i < queues.size(); ++i) {
KUKADU_SHARED_PTR<ControlQueue> queue = queues.at(i);
castedQueues.push_back(queue);
}
SensorStorage store(castedQueues, hands, 100);
prepare();
KUKADU_SHARED_PTR<kukadu_thread> storageThread = store.startDataStorage(completePath);
performCore();
store.stopDataStorage();
storageThread->join();
}
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();
}