diff options
Diffstat (limited to 'LocomotorPrimitives.cpp')
| -rw-r--r-- | LocomotorPrimitives.cpp | 77 |
1 files changed, 66 insertions, 11 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 <tobias.klauser@uzh.ch> */ +#include <signal.h> + #include <OpenSim/OpenSim.h> #include <OpenSim/Common/IO.h> @@ -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<OpenSim::ActivationFiberLengthMuscle *>(&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 |