diff --git a/src/quadarm_task_priority_ctrl.cpp b/src/quadarm_task_priority_ctrl.cpp
index 0b6bec2c079fd6583df0578dffd9edde2a449be1..f9fa15659518fb1b85d06f1924cd7da2df9c7ef5 100644
--- a/src/quadarm_task_priority_ctrl.cpp
+++ b/src/quadarm_task_priority_ctrl.cpp
@@ -8,7 +8,7 @@ using namespace std;
 
 CQuadarm_Task_Priority_Ctrl::CQuadarm_Task_Priority_Ctrl()
 {
-  // time (&this->time_ini_);
+  this->datetime_ = get_datetime();
 }
  
 CQuadarm_Task_Priority_Ctrl::~CQuadarm_Task_Priority_Ctrl()
@@ -661,31 +661,50 @@ Matrix4d CQuadarm_Task_Priority_Ctrl::GetTransform(const Frame &f)
 
   return T;
 }
+
+std::string CQuadarm_Task_Priority_Ctrl::get_datetime()
+{
+  time_t rawtime;
+  struct tm * timeinfo;
+  char buffer[80];
+
+  time (&rawtime);
+  timeinfo = localtime(&rawtime);
+
+  // strftime(buffer,80,"%d%m%Y_%I%M%S",timeinfo);
+  strftime(buffer,80,"%d%m%Y_%I%M",timeinfo);
+  std::string str(buffer);
+
+  return str;
+}
 void CQuadarm_Task_Priority_Ctrl::write_to_file(const string& folder_name, const string& file_name, const MatrixXd& data, const double& ts)
 {
   // Get home directory
   int myuid;
   passwd *mypasswd;
-  string path;
+  std::string path;
   myuid = getuid();
   mypasswd = getpwuid(myuid);
   path= mypasswd->pw_dir;
-  // path+= "/Desktop/Results/";
 
   // Add Folder name
-  stringstream folder;
+  std::stringstream folder;
   folder << folder_name;
   path+= folder.str();
 
+  // Add Folder with date and time
+  // path+="/";
+  // path+=this->datetime_;
+
   // Check if path exists, and create folder if needed
   struct stat sb;
   if (stat(path.c_str(), &sb) != 0)
   {
     mkdir(path.c_str(), S_IRWXU|S_IRGRP|S_IXGRP);
-    string command = "mkdir -p ";
-    stringstream ss;
+    std::string command = "mkdir -p ";
+    std::stringstream ss;
     ss << command << path;
-    string com = ss.str();
+    std::string com = ss.str();
     int ret = system(com.c_str());
     if (WEXITSTATUS(ret)!=0)
       std::cout << "Error creating directory." << std::endl;
@@ -694,11 +713,11 @@ void CQuadarm_Task_Priority_Ctrl::write_to_file(const string& folder_name, const
   path+="/";
 
   // Create file
-  string FileName;
+  std::string FileName;
   FileName = path;
   FileName+= file_name;
-  ofstream File;
-  File.open(FileName.c_str(), ios::out | ios::app | ios::binary);
+  std::ofstream File;
+  File.open(FileName.c_str(), std::ios::out | std::ios::app | std::ios::binary);
 
   // Write time stamp
   File << ts;
diff --git a/src/quadarm_task_priority_ctrl.h b/src/quadarm_task_priority_ctrl.h
index 5ebb18c5db507fd5e49a9928417299b14fb42a67..543a7fed3ed0d4b97cf778b5abe98af5ea8fa5dc 100644
--- a/src/quadarm_task_priority_ctrl.h
+++ b/src/quadarm_task_priority_ctrl.h
@@ -31,6 +31,8 @@
 #include <boost/scoped_ptr.hpp>
 #include <boost/shared_ptr.hpp>
 
+#include <ctime>
+
 using namespace Eigen;
 using namespace KDL;
 using namespace std;
@@ -115,7 +117,6 @@ typedef struct{
 
 class CQuadarm_Task_Priority_Ctrl
 {
-
   private:
 
     uam uam_; // Unmanned Aerial Manipulator
@@ -134,6 +135,16 @@ class CQuadarm_Task_Priority_Ctrl
 
     double quad_height_; // Quadrotor ground distance.
 
+    string datetime_; // Initial date and time
+
+    /**
+    * \brief Get date and time
+    *
+    * Get date and time as string
+    *
+    */    
+    std::string get_datetime();
+
     /**
     * \brief Compute Unmanned Aerial Manipulator Kinematics
     *