Commit e4cbbcef authored by Olivier Stasse's avatar Olivier Stasse

Improve log.

parent ad6ec2ae
......@@ -18,22 +18,23 @@ DataToLog::DataToLog()
{
}
void DataToLog::init(unsigned int nbDofs,long int length)
void DataToLog::init(ProfileLog &aProfileLog)
{
motor_angle.resize(nbDofs*length);
joint_angle.resize(nbDofs*length);
velocities.resize(nbDofs*length);
torques.resize(nbDofs*length);
motor_currents.resize(nbDofs*length);
orientation.resize(4*length);
accelerometer.resize(3*length);
gyrometer.resize(3*length);
force_sensors.resize(4*6*length);
temperatures.resize(nbDofs*length);
timestamp.resize(length);
duration.resize(length);
for(unsigned int i=0;i<nbDofs*length;i++)
profileLog_ = aProfileLog;
motor_angle.resize(aProfileLog.nbDofs*aProfileLog.length);
joint_angle.resize(aProfileLog.nbDofs*aProfileLog.length);
velocities.resize(aProfileLog.nbDofs*aProfileLog.length);
torques.resize(aProfileLog.nbDofs*aProfileLog.length);
motor_currents.resize(aProfileLog.nbDofs*aProfileLog.length);
orientation.resize(4*aProfileLog.length);
accelerometer.resize(3*aProfileLog.length);
gyrometer.resize(3*aProfileLog.length);
force_sensors.resize(aProfileLog.nbForceSensors*6*aProfileLog.length);
temperatures.resize(aProfileLog.nbDofs*aProfileLog.length);
timestamp.resize(aProfileLog.length);
duration.resize(aProfileLog.length);
for(unsigned int i=0;i<aProfileLog.nbDofs*aProfileLog.length;i++)
{ motor_angle[i] = joint_angle[i] =
velocities[i] = 0.0; }
......@@ -46,13 +47,12 @@ Log::Log():
{
}
void Log::init(unsigned int nbDofs, unsigned int length)
void Log::init(ProfileLog &aProfileLog)
{
profileLog_ = aProfileLog;
lref_ =0;
lrefts_=0;
nbDofs_=nbDofs;
length_=length;
StoredData_.init(nbDofs,length);
StoredData_.init(aProfileLog);
struct timeval current;
gettimeofday(&current,0);
......@@ -62,11 +62,11 @@ void Log::init(unsigned int nbDofs, unsigned int length)
void Log::record(DataToLog &aDataToLog)
{
if ( (aDataToLog.motor_angle.size()!=nbDofs_) ||
(aDataToLog.velocities.size()!=nbDofs_))
if ( (aDataToLog.motor_angle.size()!=aDataToLog.nbDofs()) ||
(aDataToLog.velocities.size()!=aDataToLog.nbDofs()))
return;
for(unsigned int JointID=0;JointID<nbDofs_;JointID++)
for(unsigned int JointID=0;JointID<aDataToLog.nbDofs();JointID++)
{
if (aDataToLog.motor_angle.size()>JointID)
StoredData_.motor_angle[JointID+lref_]= aDataToLog.motor_angle[JointID];
......@@ -86,11 +86,12 @@ void Log::record(DataToLog &aDataToLog)
StoredData_.accelerometer[lrefts_*3+axis] = aDataToLog.accelerometer[axis];
StoredData_.gyrometer[lrefts_*3+axis] = aDataToLog.gyrometer[axis];
}
for(unsigned int fsID=0;fsID<4;fsID++)
unsigned width_pad= 6 * profileLog_.nbForceSensors;
for(unsigned int fsID=0;fsID<profileLog_.nbForceSensors;fsID++)
{
for(unsigned int axis=0;axis<6;axis++)
{
StoredData_.force_sensors[lrefts_*24+fsID*6+axis] =
StoredData_.force_sensors[lrefts_*width_pad+fsID*6+axis] =
aDataToLog.force_sensors[fsID*6+axis];
}
}
......@@ -102,9 +103,9 @@ void Log::record(DataToLog &aDataToLog)
StoredData_.duration[lrefts_] = time_stop_it_ - time_start_it_;
lref_ += nbDofs_;
lref_ += aDataToLog.nbDofs();
lrefts_ ++;
if (lref_>=nbDofs_*length_)
if (lref_>=aDataToLog.nbDofs()*aDataToLog.length())
{
lref_=0;
lrefts_=0;
......@@ -133,15 +134,15 @@ void Log::stop_it()
void Log::save(std::string &fileName)
{
std::string suffix("-mastate.log");
saveVector(fileName,suffix,StoredData_.motor_angle, nbDofs_);
saveVector(fileName,suffix,StoredData_.motor_angle, profileLog_.nbDofs);
suffix="-jastate.log";
saveVector(fileName,suffix,StoredData_.joint_angle, nbDofs_);
saveVector(fileName,suffix,StoredData_.joint_angle, profileLog_.nbDofs);
suffix = "-vstate.log";
saveVector(fileName,suffix,StoredData_.velocities, nbDofs_);
saveVector(fileName,suffix,StoredData_.velocities, profileLog_.nbDofs);
suffix = "-torques.log";
saveVector(fileName,suffix,StoredData_.torques, nbDofs_);
saveVector(fileName,suffix,StoredData_.torques, profileLog_.nbDofs);
suffix = "-motor-currents.log";
saveVector(fileName,suffix,StoredData_.motor_currents, nbDofs_);
saveVector(fileName,suffix,StoredData_.motor_currents, profileLog_.nbDofs);
suffix = "-accelero.log";
saveVector(fileName,suffix,StoredData_.accelerometer, 3);
suffix = "-gyro.log";
......@@ -153,7 +154,7 @@ void Log::save(std::string &fileName)
saveVector(fileName,suffix,StoredData_.force_sensors, 6);
suffix = "-temperatures.log";
saveVector(fileName,suffix,StoredData_.temperatures, nbDofs_);
saveVector(fileName,suffix,StoredData_.temperatures, profileLog_.nbDofs);
suffix = "-duration.log";
saveVector(fileName,suffix,StoredData_.duration, 1);
......@@ -192,12 +193,13 @@ void Log::saveVector(std::string &fileName,std::string &suffix,
double dt;
if (aof.is_open())
{
writeHeaderToBinaryBuffer (aof, length_, size+2);
for(unsigned long int i=0;i<length_;i++)
writeHeaderToBinaryBuffer (aof, profileLog_.length, size+2);
for(unsigned long int i=0;i<profileLog_.length;i++)
{
// Compute and save dt
if (i==0)
dt = StoredData_.timestamp[i] - StoredData_.timestamp[length_-1];
dt = StoredData_.timestamp[i] - StoredData_.timestamp
[profileLog_.length-1];
else
dt = StoredData_.timestamp[i] - StoredData_.timestamp[i-1];
writeToBinaryFile (aof, StoredData_.timestamp[i], dt, avector, idx, size);
......
......@@ -12,6 +12,13 @@
namespace rc_sot_system {
struct ProfileLog
{
unsigned int nbDofs;
unsigned int nbForceSensors;
unsigned int length;
};
struct DataToLog
{
// Measured angle values at the motor side.
......@@ -40,20 +47,22 @@ namespace rc_sot_system {
// Duration
std::vector<double> duration;
ProfileLog profileLog_;
DataToLog();
void init(unsigned int nbDofs, long int length);
void init(ProfileLog &aProfileLog);
unsigned int nbDofs() { return profileLog_.nbDofs;}
unsigned int nbForceSensors() { return profileLog_.nbForceSensors;}
unsigned int length() { return profileLog_.length;}
};
class Log
{
private:
// Actuated informations logged.
unsigned int nbDofs_;
// Number of iterations to be logged.
unsigned int length_;
/// Profile Log
ProfileLog profileLog_;
// Current position in the circular buffer for angles.
long unsigned int lref_;
// Current position int the circular buffer for timestamp
......@@ -77,7 +86,7 @@ namespace rc_sot_system {
Log();
void init(unsigned int nbDofs, unsigned int length);
void init(ProfileLog &aProfileLog);
void record(DataToLog &aDataToLog);
void save(std::string &fileName);
......
......@@ -460,9 +460,10 @@ namespace sot_controller
/// Deduce from this the degree of freedom number.
nbDofs_ = joints_name_.size();
/// Initialize the size of the data to store.
DataOneIter_.init(nbDofs_,1);
DataOneIter_.init(profileLog_);
/// Initialize the data logger for 300s.
RcSotLog.init(nbDofs_,300000);
RcSotLog.init(profileLog_);
return true;
}
......
......@@ -180,7 +180,10 @@ namespace sot_controller
int verbosity_level_;
/// URDF model of the robot.
urdf::ModelInterfaceSharedPtr modelURDF_;
urdf::ModelInterfaceSharedPtr modelURDF_;
/// Profile log
rc_sot_system::ProfileLog profileLog_;
public :
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment