LocomotorPrimitives: Support fixed integrator timestep

1 files changed, 19 insertions, 11 deletions

diff --git a/LocomotorPrimitives.cpp b/LocomotorPrimitives.cpp
index e5ff7f6..868726c 100644
--- a/LocomotorPrimitives.cpp
+++ b/LocomotorPrimitives.cpp
@@ -55,7 +55,8 @@ static void constructModel(OpenSim::Model &model, OpenSim::Storage actData)
 	logger.log("Finished constructing model\n");
 }
 
-void simulateModel(OpenSim::Model &model, const double initialTime, const double finalTime)
+void simulateModel(OpenSim::Model &model, const double initial_time,
+		   const double final_time, double fixed_step_size)
 {
 	if (NO_SIM) {
 		logger.log("Skipping simulation as per NO_SIM=%d\n", NO_SIM);
@@ -71,17 +72,19 @@ void simulateModel(OpenSim::Model &model, const double initialTime, const double
 //	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);
+	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)
+		integrator.setFixedStepSize(fixed_step_size);
 	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);
-	logger.log("+ Integrating from time %f to %f\n", initialTime, finalTime);
+	manager.setInitialTime(initial_time);
+	manager.setFinalTime(final_time);
+	logger.log("+ Integrating from time %f to %f\n", initial_time, final_time);
 	manager.integrate(si);
 
 	// Save the model states from forward integration
@@ -102,12 +105,14 @@ static void usage()
 	std::cout << "usage: " << MODEL_NAME << " [OPTION...]" << std::endl << std::endl
 		  << "  -d FILE  specify .sto datafile to use for muscle activation data" << std::endl
 		  << "  -m FILE  specify .osim to use as model" << std::endl
+		  << "  -s N     set fixed step size for integrator to N (default: dynamic)" << std::endl
 		  << "  -h       show this help and exit" << std::endl;
 }
 
 int main(int argc, char **argv)
 {
 	clock_t ts_start = clock();
+	double fixed_step_size = 0.0;
 
 	/* set default values (to work on windows) */
 	std::string modelFile = FIXED_IN_SPACE ? "../../locomotor-primitives-fixed.osim" : "../../locomotor-primitives.osim";
@@ -121,6 +126,9 @@ int main(int argc, char **argv)
 		case 'm':
 			modelFile = argv[++i];
 			break;
+		case 's':
+			fixed_step_size = strtod(argv[++i], NULL);
+			break;
 		case 'h':
 			usage();
 			exit(0);
@@ -132,15 +140,15 @@ int main(int argc, char **argv)
 	}
 
 	OpenSim::Storage actData(dataFile);
-	const double initialTime = actData.getFirstTime();
-	const double finalTime = actData.getLastTime();
+	const double initial_time = actData.getFirstTime();
+	const double final_time = actData.getLastTime();
 
-	if (initialTime < 0 || finalTime < 0 || finalTime <= initialTime) {
-		logger.err("invalid time range in data file: %f - %f\n", initialTime, finalTime);
+	if (initial_time < 0 || final_time < 0 || final_time <= initial_time) {
+		logger.err("invalid time range in data file: %f - %f\n", initial_time, final_time);
 		exit(-1);
 	}
 
-	logger.log("Starting simulation %s with model file %s, actuation data file %s\n", MODEL_NAME.c_str(), modelFile.c_str(), dataFile.c_str());
+	logger.log("Starting simulation %s with model file %s, actuation data file %s\n, fixed step size: %f", MODEL_NAME.c_str(), modelFile.c_str(), dataFile.c_str(), fixed_step_size);
 
 	try {
 		// Create an OpenSim model and set its name
@@ -150,7 +158,7 @@ int main(int argc, char **argv)
 		osimModel.setUseVisualizer(false);
 
 		constructModel(osimModel, actData);
-		simulateModel(osimModel, initialTime, finalTime);
+		simulateModel(osimModel, initial_time, final_time, fixed_step_size);
 	} catch (OpenSim::Exception ex) {
 		logger.err("Exception: %s. Press ENTER to quit", ex.getMessage());
 		std::cin.get();