diff options
| -rw-r--r-- | LocomotorPrimitives.cpp | 62 | ||||
| -rw-r--r-- | LocomotorPrimitivesController.cpp | 14 | ||||
| -rw-r--r-- | LocomotorPrimitivesController.h | 2 | ||||
| -rw-r--r-- | Logger.cpp | 58 | ||||
| -rw-r--r-- | Logger.h | 42 |
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_ */ |