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();
}
ActionClip::ActionClip(int actionId, int perceptDimensionality, std::string label, KUKADU_SHARED_PTR<kukadu_mersenne_twister> generator) : Clip(CLIP_H_LEVEL_FINAL, generator, KUKADU_SHARED_PTR<vector<int> >(new vector<int>()), INT_MAX) {
this->actionId = actionId;
this->label = label;
KUKADU_SHARED_PTR<vector<int> > dummyVals = KUKADU_SHARED_PTR<vector<int> >(new vector<int>());
for(int i = 0; i < perceptDimensionality; ++i)
dummyVals->push_back(-actionId - 1);
setClipDimensionValues(dummyVals);
}
bool Kinematics::checkAllRestrictions(arma::vec currentState, geometry_msgs::Pose pose) {
for(int i = 0; i < getRestrictionsCount(); ++i) {
KUKADU_SHARED_PTR<Restriction> currRest = getRestrictionByIdx(i);
if(!currRest->stateOk(currentState, pose))
return false;
}
return true;
}
void Kinect::visualizeCurrentTransformedPc(KUKADU_SHARED_PTR<PCTransformator> transformator) {
pcl::PointCloud<pcl::PointXYZ> currentPc = sensorMsgsPcToPclPc(getCurrentPointCloud());
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed = transformator->transformPc(currentPc.makeShared());
visPublisher.publish(pclPcToSensorMsgsPc(transformed));
}
IntermediateEventClip::IntermediateEventClip(KUKADU_SHARED_PTR<SensingController> sensingEvent,
int level, KUKADU_SHARED_PTR<kukadu_mersenne_twister> generator, std::string clipValues, int immunity) : Clip(level, generator, clipValues, immunity) {
this->caption = sensingEvent->getCaption();
this->sensingEvent = sensingEvent;
}
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();
}
KUKADU_SHARED_PTR<Trajectory> PoWER::updateStep() {
KUKADU_SHARED_PTR<Trajectory> lastUp = getLastUpdate();
vector<KUKADU_SHARED_PTR<Trajectory> > lastDmps = getLastRolloutParameters();
vector<double> lastRewards = getLastRolloutCost();
// add rollouts to history
for(int i = 0; i < lastRewards.size(); ++i) {
pair <double, KUKADU_SHARED_PTR<Trajectory> > p(lastRewards.at(i), lastDmps.at(i));
sampleHistory.push_back(p);
}
// sort by reward...
sort(sampleHistory.begin(), sampleHistory.end(), rewardComparator);
// ...and discard bad samples
int histSize = sampleHistory.size();
for(int i = (importanceSamplingCount - 1); i < histSize; ++i)
sampleHistory.pop_back();
double totalReward = 0.0;
for(int i = 0; i < sampleHistory.size(); ++i)
totalReward = totalReward + sampleHistory.at(i).first;
vector<vec> lastUpCoeffs = lastUp->getCoefficients();
vec newCoeffsJ(lastUpCoeffs.at(0).n_elem);
vector<vec> newCoeffs;
for(int i = 0; i < lastUp->getCoefficients().size(); ++i) {
vec v = lastUp->getCoefficients().at(i);
newCoeffs.push_back(v);
}
cout << "====================" << endl;
// for each degree of freedom
for(int i = 0; i < newCoeffs.size(); ++i) {
vec currentDegCoeffs = newCoeffs.at(i);
// go through all samples and weight rollouts
for(int j = 0; j < sampleHistory.size(); ++j) {
currentDegCoeffs += sampleHistory.at(j).first / totalReward * (sampleHistory.at(j).second->getCoefficients().at(i) - lastUpCoeffs.at(i));
}
newCoeffs[i] = currentDegCoeffs;
}
KUKADU_SHARED_PTR<Trajectory> newUp = lastUp->copy();
newUp->setCoefficients(newCoeffs);
return newUp;
}
std::vector<KUKADU_SHARED_PTR<Trajectory> > PoWER::computeRolloutParamters() {
KUKADU_SHARED_PTR<Trajectory> lastUp = getLastUpdate();
vector<vec> dmpCoeffs = lastUp->getCoefficients();
vector<KUKADU_SHARED_PTR<Trajectory> > nextCoeffs;
for(int k = 0; k < updatesPerRollout; ++k) {
vec currCoeff;
for(int i = 0; i < dmpCoeffs.size(); ++i) {
currCoeff = dmpCoeffs.at(i);
for(int j = 0; j < currCoeff.n_elem; ++j) {
kukadu_normal_distribution normal = normals.at(j);
double eps = normal(generator);
currCoeff(j) += eps;
}
dmpCoeffs[i] = currCoeff;
}
KUKADU_SHARED_PTR<Trajectory> nextUp = lastUp->copy();
nextUp->setCoefficients(dmpCoeffs);
nextCoeffs.push_back(nextUp);
}
return nextCoeffs;
}
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();
}
