LocomotorPrimitives: Implement signal handling

This way we can still write result files if the simulation is
interrupted.

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 <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
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_ */