LocomotorPrimitives: Switch to using Logger class for logging

5 files changed, 141 insertions, 37 deletions

diff --git a/LocomotorPrimitives.cpp b/LocomotorPrimitives.cpp
index 3bb8d97..e5ff7f6 100644
--- a/LocomotorPrimitives.cpp
+++ b/LocomotorPrimitives.cpp
@@ -11,23 +11,25 @@
 
 #include "LocomotorPrimitivesManager.h"
 #include "LocomotorPrimitivesController.h"
+#include "Logger.h"
 
 #define FIXED_IN_SPACE	1
 #define NO_SIM		0
 
-clock_t ts_start;
-
 static const std::string MODEL_NAME = "LocomotorPrimitives";
+static const double INTEGRATOR_ACCURACY = 1.0e-3;
+
+static Logger &logger = Logger::getInstance();
 
 static void constructModel(OpenSim::Model &model, OpenSim::Storage actData)
 {
-	std::cout << "Constructing " << MODEL_NAME << " model" << std::endl;
+	logger.log("Constructing model %s\n", MODEL_NAME.c_str());
 
 	// Define controller for the model
 	LocomotorPrimitivesController *control = new LocomotorPrimitivesController(actData, 0.01);
 	control->setActuators(model.updActuators());
 	if (control->checkControls() != 0) {
-		std::cerr << "Control data for some muscles in the model is missing. ENTER to continue, ^-C to quit." << std::endl;
+		logger.err("Control data for some muscles in the model is missing. Press ENTER to continue, ^-C to quit.");
 		std::cin.get();
 	}
 	model.addController(control);
@@ -35,12 +37,12 @@ static void constructModel(OpenSim::Model &model, OpenSim::Storage actData)
 	// Set default activation and fiber length on all muscles of the model
 	const OpenSim::Set<OpenSim::Actuator> &actSet = model.getActuators();
 	int sz = actSet.getSize();
-	std::cout << " + Defining initial muscle states for " << sz << " actuators:" << std::endl;
+	logger.log("+ Defining initial muscle states for %d actuators:\n", sz);
 
 	// Set default activation and fiber length of all muscles
 	for (int i = 0; i < sz; i++) {
 		OpenSim::ActivationFiberLengthMuscle *m = dynamic_cast<OpenSim::ActivationFiberLengthMuscle *>(&actSet.get(i));
-		std::cout << "   " << m->getName() << std::endl;
+		logger.log("  => %s\n", m->getName().c_str());
 //		m->setDefaultActivation(0.5);
 //		m->setDefaultFiberLength(0.1);
 	}
@@ -50,45 +52,49 @@ static void constructModel(OpenSim::Model &model, OpenSim::Storage actData)
 	OpenSim::ForceReporter *reporter = new OpenSim::ForceReporter(&model);
 	model.addAnalysis(reporter);
 
-	std::cout << "Finished constructing model" << std::endl;
+	logger.log("Finished constructing model\n");
 }
 
 void simulateModel(OpenSim::Model &model, const double initialTime, const double finalTime)
 {
 	if (NO_SIM) {
-		std::cout << "Skipping simulation as per NO_SIM=" << NO_SIM << std::endl;
+		logger.log("Skipping simulation as per NO_SIM=%d\n", NO_SIM);
 		return;
 	}
 
-	std::cout << "Simulating model " << MODEL_NAME << std::endl;
-	std::cout << " + Initializing system" << std::endl;
+	logger.log("Simulating model %s\n", MODEL_NAME.c_str());
+
+	logger.log("+ Initializing system\n");
 	SimTK::State &si = model.initSystem();
 
-	std::cout << " + Creating integrator" << std::endl;
+//	std::cout << "=== MODEL SUMMARY ===" << std::endl;
+//	model.printDetailedInfo(si, std::cout);
+//	std::cout << "=== END MODEL SUMMARY ===" << std::endl << std::endl;
+
+	logger.log("+ Creating integrator (RungeKuttaMerson) with accuracy %e\n", INTEGRATOR_ACCURACY);
 	// Create the integrator and manager for the simulation
 	SimTK::RungeKuttaMersonIntegrator integrator(model.getMultibodySystem());
-	integrator.setAccuracy(1.0e-3);
-	std::cout << " + Creating simulation manager" << std::endl;
+	integrator.setAccuracy(INTEGRATOR_ACCURACY);
 
+	logger.log("+ Creating simulation manager\n");
 	LocomotorPrimitivesManager manager(model, integrator);
-
 	// Integrate from initial time to final time
 	manager.setInitialTime(initialTime);
 	manager.setFinalTime(finalTime);
-
-	std::cout << "=== MODEL SUMMARY ===" << std::endl;
-	model.printDetailedInfo(si, std::cout);
-	std::cout << "=== END MODEL SUMMARY ===" << std::endl << std::endl;
-	std::cout << " + Integrating from " << std::fixed << initialTime << " to " << std::fixed << finalTime << std::endl;
+	logger.log("+ Integrating from time %f to %f\n", initialTime, finalTime);
 	manager.integrate(si);
 
 	// Save the model states from forward integration
+	logger.log("+ Writing states to %s_states.sto\n", MODEL_NAME.c_str());
 	OpenSim::Storage statesDegrees(manager.getStateStorage());
 	statesDegrees.print(MODEL_NAME + "_states.sto");
 
 	// Save the forces
+	logger.log("+ Writing forces to %s_forces.mot\n", MODEL_NAME.c_str());
 	OpenSim::Analysis &reporter = model.getAnalysisSet()[0];
 	((OpenSim::ForceReporter &)reporter).getForceStorage().print(MODEL_NAME + "_forces.mot");
+
+	logger.log("Finished simulating model\n");
 }
 
 static void usage()
@@ -101,7 +107,7 @@ static void usage()
 
 int main(int argc, char **argv)
 {
-	ts_start = clock();
+	clock_t ts_start = clock();
 
 	/* set default values (to work on windows) */
 	std::string modelFile = FIXED_IN_SPACE ? "../../locomotor-primitives-fixed.osim" : "../../locomotor-primitives.osim";
@@ -130,12 +136,11 @@ int main(int argc, char **argv)
 	const double finalTime = actData.getLastTime();
 
 	if (initialTime < 0 || finalTime < 0 || finalTime <= initialTime) {
-		std::cerr << "invalid time range in data file: " << initialTime << " - " << finalTime << std::endl;
+		logger.err("invalid time range in data file: %f - %f\n", initialTime, finalTime);
 		exit(-1);
 	}
 
-	std::cout << "Starting simulation " + MODEL_NAME << " with (" << modelFile << ", " << dataFile << ") from time "
-		  << initialTime << " to " << finalTime << std::endl;
+	logger.log("Starting simulation %s with model file %s, actuation data file %s\n", MODEL_NAME.c_str(), modelFile.c_str(), dataFile.c_str());
 
 	try {
 		// Create an OpenSim model and set its name
@@ -147,22 +152,21 @@ int main(int argc, char **argv)
 		constructModel(osimModel, actData);
 		simulateModel(osimModel, initialTime, finalTime);
 	} catch (OpenSim::Exception ex) {
-		std::cout << ex.getMessage() << std::endl;
+		logger.err("Exception: %s. Press ENTER to quit", ex.getMessage());
 		std::cin.get();
 		return 1;
 	} catch (std::exception ex) {
-		std::cout << ex.what() << std::endl;
+		logger.err("Exception: %s. Press ENTER to quit", ex.what());
 		std::cin.get();
 		return 1;
 	} catch (...) {
-		std::cout << "UNRECOGNIZED EXCEPTION" << std::endl;
+		logger.err("Unrecognized Exception. Press ENTER to quit");
 		std::cin.get();
 		return 1;
 	}
 
-	std::cout << "main() routine time = " << 1.e3*(clock()-ts_start)/CLOCKS_PER_SEC << "ms" << std::endl;
-
-	std::cout << "Simulation " + MODEL_NAME << " completed successfully." << std::endl;
+	logger.log("Total runtime = %fms\n", 1.e3 * (clock() - ts_start) / CLOCKS_PER_SEC);
+	logger.log("Simulation %s completed successfully. Press ENTER to quit", MODEL_NAME.c_str());
 	std::cin.get();
 	return 0;
 }
diff --git a/LocomotorPrimitivesController.cpp b/LocomotorPrimitivesController.cpp
index 27ac205..0bdaf81 100644
--- a/LocomotorPrimitivesController.cpp
+++ b/LocomotorPrimitivesController.cpp
@@ -2,14 +2,11 @@
 
 #include "LocomotorPrimitivesController.h"
 
-#define VERBOSE 1
-
-// XXX
-extern clock_t ts_start;
-
 static const double TIME_DAMP = 0.1;
 static const unsigned int N_PRINT = 1000;
 
+static Logger &logger = Logger::getInstance();
+
 int LocomotorPrimitivesController::checkControls()
 {
 	const int act_set_siz = getActuatorSet().getSize();
@@ -22,7 +19,7 @@ int LocomotorPrimitivesController::checkControls()
 		const OpenSim::Array<int> indices = _act.getColumnIndicesForIdentifier(muscle->getName());
 
 		if (indices.getSize() == 0) {
-			std::cerr << "Error: no actuation data for muscle '" << muscle->getName() << "' found" << std::endl;
+			logger.err("no actuation data for muscle '%s' found\n", muscle->getName().c_str());
 			ret--;
 		}
 	}
@@ -52,10 +49,11 @@ void LocomotorPrimitivesController::computeControls(const SimTK::State &s, SimTK
 		if (indices.getSize() != 0) {
 			int idx = indices.get(0) - 1;
 			if (n % N_PRINT == 0)
-				std::cout << "[" << (1.e3 * clock() - ts_start) / CLOCKS_PER_SEC << "ms] " << t << " (" << idx << ") actuation data for '" << muscle->getName() << "' found: " << _muscle_act[idx] << std::endl;
+				logger.log("%f (%d) actuation data for muscle '%s' found: %f\n",
+						t, idx, muscle->getName().c_str(), _muscle_act[idx]);
 
 			if (_muscle_act[idx] > 1.0) {
-				std::cerr << "Error: muscle actuation larger than 1.0 (" << _muscle_act[idx] << "), truncating" << std::endl;
+				logger.err("muscle actuation larger than 1.0 (%f), truncating to 1.0\n", _muscle_act[idx]);
 				_muscle_act[idx] = 1.0;
 			}
 
diff --git a/LocomotorPrimitivesController.h b/LocomotorPrimitivesController.h
index c33745e..578a659 100644
--- a/LocomotorPrimitivesController.h
+++ b/LocomotorPrimitivesController.h
@@ -3,6 +3,8 @@
 
 #include <OpenSim/OpenSim.h>
 
+#include "Logger.h"
+
 class LocomotorPrimitivesController : public OpenSim::Controller {
 OpenSim_DECLARE_CONCRETE_OBJECT(LocomotorPrimitivesController, OpenSim::Controller);
 
diff --git a/Logger.cpp b/Logger.cpp
new file mode 100644
index 0000000..b0850a6
--- /dev/null
+++ b/Logger.cpp
@@ -0,0 +1,58 @@
+/**
+ * General purpose logger class.
+ *
+ * Copyright (C) 2013 Tobias Klauser <tklauser@distanz.ch>
+ *
+ * This file is subject to the terms and conditions of the GNU General
+ * Public License, version 2.
+ */
+
+#include <errno.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/time.h>
+#include <time.h>
+
+#include "Logger.h"
+
+const char *Logger::_LOGGER_DATE_FMT = "%b %d %Y %H:%M:%S";
+
+int Logger::_log_vfprintf(FILE *f, const char *prefix, const char *fmt, va_list ap)
+{
+	struct timeval now;
+	char buf[64];
+	int ret;
+
+	if (gettimeofday(&now, NULL))
+		return -EINVAL;
+
+	strftime(buf, sizeof(buf), _LOGGER_DATE_FMT, localtime(&now.tv_sec));
+	ret = fprintf(f, "[%s.%03lu] %s%s", buf, now.tv_usec / 1000,
+			prefix ? prefix : "", prefix ? ": " : "");
+
+	return vfprintf(f, fmt, ap);
+}
+
+int Logger::log(const char *fmt, ...)
+{
+	va_list ap;
+	int ret;
+
+	va_start(ap, fmt);
+	ret = _log_vfprintf(_f_out, NULL, fmt, ap);
+	va_end(ap);
+
+	return ret;
+}
+
+int Logger::err(const char *fmt, ...)
+{
+	va_list ap;
+	int ret;
+
+	va_start(ap, fmt);
+	ret = _log_vfprintf(_f_err, "Error", fmt, ap);
+	va_end(ap);
+
+	return ret;
+}
diff --git a/Logger.h b/Logger.h
new file mode 100644
index 0000000..5ee5bd4
--- /dev/null
+++ b/Logger.h
@@ -0,0 +1,42 @@
+/**
+ * General purpose logger class.
+ *
+ * Copyright (C) 2013 Tobias Klauser <tklauser@distanz.ch>
+ *
+ * This file is subject to the terms and conditions of the GNU General
+ * Public License, version 2.
+ */
+
+#ifndef LOGGER_H_
+#define LOGGER_H_
+
+#include <stdarg.h>
+#include <stdio.h>
+
+/* TODO: Make singleton */
+class Logger {
+
+public:
+	static Logger& getInstance()
+	{
+		// instantiated on first use, guaranteed to be destroyed
+		static Logger instance;
+		return instance;
+	}
+
+	int log(const char *fmt, ...);
+	int err(const char *fmt, ...);
+private:
+	Logger() : _f_out(stdout), _f_err(stderr) { }
+	~Logger() { }
+	// Declare copy constructer to prevent the compiler from generating one
+	Logger(Logger const&);
+	void operator=(Logger const&);
+
+	FILE *_f_out, *_f_err;
+	static const char *_LOGGER_DATE_FMT;
+
+	int _log_vfprintf(FILE *f, const char *prefix, const char *fmt, va_list ap);
+};
+
+#endif /* LOGGER_H_ */