From e953e9ac75341b928ae89861ce38f7bba0d8b911 Mon Sep 17 00:00:00 2001 From: Tobias Klauser Date: Tue, 29 Jan 2013 14:52:31 +0100 Subject: LocomotorPrimitives: Implement signal handling This way we can still write result files if the simulation is interrupted. --- LocomotorPrimitives.cpp | 77 ++++++++++++++++++++++++++++++++++++------ LocomotorPrimitivesManager.cpp | 12 +++++++ LocomotorPrimitivesManager.h | 3 +- 3 files changed, 80 insertions(+), 12 deletions(-) diff --git a/LocomotorPrimitives.cpp b/LocomotorPrimitives.cpp index 0a0ebcc..de031f2 100644 --- a/LocomotorPrimitives.cpp +++ b/LocomotorPrimitives.cpp @@ -7,6 +7,8 @@ * Copyright (C) 2013 Tobias Klauser */ +#include + #include #include @@ -21,6 +23,19 @@ static const std::string MODEL_NAME = "LocomotorPrimitives"; static const double INTEGRATOR_ACCURACY = 1.0e-3; static Logger &logger = Logger::getInstance(); +static LocomotorPrimitivesManager *manager = NULL; + +static void signal_handler(int signr) +{ + switch (signr) { + case SIGINT: + logger.log("Got signal SIGINT. Quitting...\n"); + if (manager) + manager->halt(); + default: + break; + } +} static void constructModel(OpenSim::Model &model, OpenSim::Storage actData) { @@ -44,15 +59,28 @@ static void constructModel(OpenSim::Model &model, OpenSim::Storage actData) for (int i = 0; i < sz; i++) { OpenSim::ActivationFiberLengthMuscle *m = dynamic_cast(&actSet.get(i)); logger.log(" => %s\n", m->getName().c_str()); -// m->setDefaultActivation(0.5); -// m->setDefaultFiberLength(0.1); + m->setDefaultActivation(0.01); + m->setDefaultFiberLength(0.1); } +// logger.log("+ Adding Actuation Analysis\n"); +// OpenSim::Actuation *actuation = new OpenSim::Actuation(&model); +// actuation->setStepInterval(1); +// model.addAnalysis(actuation); + // Create the force reporter for obtaining the forces applied to the model // during a forward simulation + logger.log("+ Adding Force Reporter\n"); OpenSim::ForceReporter *reporter = new OpenSim::ForceReporter(&model); model.addAnalysis(reporter); + // Muscle analysis for recording and computting basic quantities + // (length, shortening velocity, tendon length, ...) for muscles during + // a simulation +// logger.log("+ Adding Muscle Analysis\n"); +// OpenSim::MuscleAnalysis *analysis = new OpenSim::MuscleAnalysis(&model); +// model.addAnalysis(analysis); + logger.log("Finished constructing model\n"); } @@ -80,7 +108,8 @@ void simulateModel(OpenSim::Model &model, const double initial_time, // model.printDetailedInfo(si, std::cout); // std::cout << "=== END MODEL SUMMARY ===" << std::endl << std::endl; - logger.log("+ Creating integrator (RungeKuttaMerson) with accuracy %e and step size %f (0.0 means dynamic)\n", INTEGRATOR_ACCURACY, fixed_step_size); + logger.log("+ Creating integrator (RungeKuttaMerson) with accuracy %e and step size %f (0.0 means dynamic)\n", + INTEGRATOR_ACCURACY, fixed_step_size); // Create the integrator and manager for the simulation SimTK::RungeKuttaMersonIntegrator integrator(model.getMultibodySystem()); if (fixed_step_size > 0) @@ -88,22 +117,38 @@ void simulateModel(OpenSim::Model &model, const double initial_time, integrator.setAccuracy(INTEGRATOR_ACCURACY); logger.log("+ Creating simulation manager\n"); - LocomotorPrimitivesManager manager(model, integrator); + manager = new LocomotorPrimitivesManager(model, integrator); // Integrate from initial time to final time - manager.setInitialTime(initial_time); - manager.setFinalTime(final_time); + manager->setInitialTime(initial_time); + manager->setFinalTime(final_time); logger.log("+ Integrating from time %f to %f\n", initial_time, final_time); - manager.integrate(si); + try { + manager->integrate(si); + } catch (...) { + logger.log("Got exception during integration\n"); + } // 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()); + OpenSim::Storage statesDegrees(manager->getStateStorage()); statesDegrees.print(MODEL_NAME + "_states.sto"); + logger.log("+ Writing states (degrees) to %s_states_degrees.mot\n", MODEL_NAME.c_str()); + model.updSimbodyEngine().convertRadiansToDegrees(statesDegrees); + statesDegrees.setWriteSIMMHeader(true); + statesDegrees.print(MODEL_NAME + "_states_degrees.mot"); + // 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"); + const OpenSim::Analysis &a = model.getAnalysisSet().get("ForceReporter"); + ((OpenSim::ForceReporter &) a).getForceStorage().print(MODEL_NAME + "_forces.mot"); + + // Save the actuation +// logger.log("+ Writing actuation to %s_actuation.sto\n", MODEL_NAME.c_str()); + // TODO + +// logger.log("+ Writing muscle analysis to %s_muscles\n", MODEL_NAME.c_str()); + // TODO logger.log("Finished simulating model\n"); } @@ -127,6 +172,9 @@ int main(int argc, char **argv) std::string data_file = "../../data/adults.sto"; std::string output_dir = "."; + sigset_t block_mask; + struct sigaction saction; + for (int i = 1; (i < argc) && (argv[i][0] == '-'); i++) { switch (argv[i][1]) { case 'd': @@ -160,7 +208,14 @@ int main(int argc, char **argv) exit(-1); } - logger.log("Starting simulation %s with model file %s, actuation data file %s\n, fixed step size: %f", MODEL_NAME.c_str(), model_file.c_str(), data_file.c_str(), fixed_step_size); + /* Register signal handler */ + sigfillset(&block_mask); + saction.sa_handler = signal_handler; + saction.sa_mask = block_mask; + saction.sa_flags = SA_RESTART; + sigaction(SIGINT, &saction, NULL); + + logger.log("Starting simulation %s with model file %s, actuation data file %s\n, fixed step size: %f\n", MODEL_NAME.c_str(), model_file.c_str(), data_file.c_str(), fixed_step_size); try { // Create an OpenSim model and set its name diff --git a/LocomotorPrimitivesManager.cpp b/LocomotorPrimitivesManager.cpp index d1925db..eb267e2 100644 --- a/LocomotorPrimitivesManager.cpp +++ b/LocomotorPrimitivesManager.cpp @@ -5,5 +5,17 @@ bool LocomotorPrimitivesManager::doIntegration(SimTK::State &s, int step, double dtFirst) { // TODO possibly additional calculations + std::cout << "==> doIntegration" << std::endl; return OpenSim::Manager::doIntegration(s, step, dtFirst); } + +bool LocomotorPrimitivesManager::integrate(SimTK::State &s, double dtFirst) +{ + int step = 0; + + std::cout << "==> integrate" << std::endl; + + s.setTime(getInitialTime()); + + return doIntegration(s, step, dtFirst); +} diff --git a/LocomotorPrimitivesManager.h b/LocomotorPrimitivesManager.h index 2bd6e3d..f58dd17 100644 --- a/LocomotorPrimitivesManager.h +++ b/LocomotorPrimitivesManager.h @@ -8,7 +8,8 @@ class LocomotorPrimitivesManager : public OpenSim::Manager public: LocomotorPrimitivesManager(OpenSim::Model &model, SimTK::Integrator &integrator) : OpenSim::Manager(model, integrator) { } + bool integrate(SimTK::State &s, double dtFirst=1.0e-6); bool doIntegration(SimTK::State &s, int step, double dtFirst); }; -#endif /* LOCOMOTORPRIMITIVES_MANAGER_H_ */ \ No newline at end of file +#endif /* LOCOMOTORPRIMITIVES_MANAGER_H_ */ -- cgit v1.2.3-54-g00ecf