Add control to logs.

src/log.cpp

@@ -30,6 +30,7 @@ void DataToLog::init(ProfileLog &aProfileLog)
   gyrometer.resize(3*aProfileLog.length);
   force_sensors.resize(aProfileLog.nbForceSensors*6*aProfileLog.length);
   temperatures.resize(aProfileLog.nbDofs*aProfileLog.length);
+  controls.resize(aProfileLog.nbDofs*aProfileLog.length);
   timestamp.resize(aProfileLog.length);
   duration.resize(aProfileLog.length);
 
@@ -79,6 +80,8 @@ void Log::record(DataToLog &aDataToLog)
 	StoredData_.motor_currents[JointID+lref_]= aDataToLog.motor_currents[JointID];
       if (aDataToLog.temperatures.size()>JointID)
 	StoredData_.temperatures[JointID+lref_]= aDataToLog.temperatures[JointID];
+      if (aDataToLog.controls.size()>JointID)
+	StoredData_.controls[JointID+lref_]= aDataToLog.controls[JointID];
     }
   for(unsigned int axis=0;axis<3;axis++)
     {
@@ -157,6 +160,9 @@ void Log::save(std::string &fileName)
   suffix = "-temperatures.log";
   saveVector(fileName,suffix,StoredData_.temperatures, profileLog_.nbDofs);
 
+  suffix = "-controls.log";
+  saveVector(fileName,suffix,StoredData_.controls, profileLog_.nbDofs);
+
   suffix = "-duration.log";
   saveVector(fileName,suffix,StoredData_.duration, 1);
 

src/log.hh

@@ -41,6 +41,8 @@ namespace rc_sot_system {
     std::vector<double> motor_currents;
     // Measured temperatures
     std::vector<double> temperatures;
+    // Control
+    std::vector<double> controls;
 
     // Timestamp
     std::vector<double> timestamp;

src/roscontrol-sot-controller.cpp

@@ -1018,7 +1018,9 @@ namespace sot_controller
 	    i<command_.size();++i)
 	  {
 	    joints_[joints_name_[i]].joint.setCommand(command_[i]);
-	  }
+            DataOneIter_.controls[i] = command_[i];
+          }
+
       }
     else
       ROS_INFO_STREAM("no control.");