LocomotorPrimitives: Tinkering to make simulation faster

No success yet... :-(

3 files changed, 35 insertions, 19 deletions

diff --git a/LocomotorPrimitives.cpp b/LocomotorPrimitives.cpp
index cfad6f6..6b6f5f5 100644
--- a/LocomotorPrimitives.cpp
+++ b/LocomotorPrimitives.cpp
@@ -13,17 +13,15 @@
 #define FIXED_IN_SPACE	1
 #define NO_SIM		0
 
+clock_t ts_start;
+
 static const std::string MODEL_NAME = "LocomotorPrimitives";
-// Define the initial and final simulation time
-static const double initialTime = 0.0;
-static const double finalTime = 2.0;
 
-static void constructModel(OpenSim::Model &model, std::string actDataFile)
+static void constructModel(OpenSim::Model &model, OpenSim::Storage actData)
 {
 	std::cout << "Constructing " << MODEL_NAME << " model" << std::endl;
 
 	// Define controller for the model
-	OpenSim::Storage actData(actDataFile);
 	LocomotorPrimitivesController *control = new LocomotorPrimitivesController(actData, 0.01);
 	control->setActuators(model.updActuators());
 	model.addController(control);
@@ -49,7 +47,7 @@ static void constructModel(OpenSim::Model &model, std::string actDataFile)
 	std::cout << "Finished constructing model" << std::endl;
 }
 
-void simulateModel(OpenSim::Model &model)
+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;
@@ -63,7 +61,7 @@ void simulateModel(OpenSim::Model &model)
 	std::cout << " + Creating integrator" << std::endl;
 	// Create the integrator and manager for the simulation
 	SimTK::RungeKuttaMersonIntegrator integrator(model.getMultibodySystem());
-	integrator.setAccuracy(1.0e-4);
+	integrator.setAccuracy(1.0e-3);
 	std::cout << " + Creating simulation manager" << std::endl;
 
 	LocomotorPrimitivesManager manager(model, integrator);
@@ -71,6 +69,10 @@ void simulateModel(OpenSim::Model &model)
 	// 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;
 	manager.integrate(si);
 
@@ -93,7 +95,7 @@ static void usage()
 
 int main(int argc, char **argv)
 {
-	clock_t ts_start = clock();
+	ts_start = clock();
 
 	/* set default values (to work on windows) */
 	std::string modelFile = FIXED_IN_SPACE ? "../../locomotor-primitives-fixed.osim" : "../../locomotor-primitives.osim";
@@ -117,16 +119,27 @@ int main(int argc, char **argv)
 		}
 	}
 
-	std::cout << "Starting simulation " + MODEL_NAME << " with (" << modelFile << ", " << dataFile << ")" << std::endl;
+	OpenSim::Storage actData(dataFile);
+	const double initialTime = actData.getFirstTime();
+	const double finalTime = actData.getLastTime();
+
+	if (initialTime < 0 || finalTime < 0 || finalTime <= initialTime) {
+		std::cerr << "invalid time range in data file: " << initialTime << " - " << finalTime << std::endl;
+		exit(-1);
+	}
+
+	std::cout << "Starting simulation " + MODEL_NAME << " with (" << modelFile << ", " << dataFile << ") from time "
+		  << initialTime << " to " << finalTime << std::endl;
 
 	try {
 		// Create an OpenSim model and set its name
 		OpenSim::Model osimModel(modelFile);
-		osimModel.setName(MODEL_NAME);
-		constructModel(osimModel, dataFile);
 
+		osimModel.setName(MODEL_NAME);
 		osimModel.setUseVisualizer(false);
-		simulateModel(osimModel);
+
+		constructModel(osimModel, actData);
+		simulateModel(osimModel, initialTime, finalTime);
 	} catch (OpenSim::Exception ex) {
 		std::cout << ex.getMessage() << std::endl;
 		std::cin.get();
diff --git a/LocomotorPrimitivesController.cpp b/LocomotorPrimitivesController.cpp
index 14fa764..0c13336 100644
--- a/LocomotorPrimitivesController.cpp
+++ b/LocomotorPrimitivesController.cpp
@@ -4,6 +4,9 @@
 
 #define VERBOSE 1
 
+// XXX
+extern clock_t ts_start;
+
 static const double TIME_DAMP = 0.1;
 static const unsigned int N_PRINT = 1000;
 
@@ -17,8 +20,10 @@ void LocomotorPrimitivesController::computeControls(const SimTK::State &s, SimTK
 	int act_set_siz = getActuatorSet().getSize();
 	int nc = _act.getSmallestNumberOfStates();
 
-	// Get the first nc states at a specified time (_NOT_ the state at nc as
-	// with getData()).
+	/*
+	 * Get the first nc states at a specified time (_NOT_ the state at nc as
+	 * with getData()).
+	 */
 	_act.getDataAtTime(t, nc, _muscle_act);
 
 	for (int i = 0; i < act_set_siz; i++) {
@@ -28,7 +33,7 @@ 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 << t << " (" << idx << ") actuation data for '" << muscle->getName() << "' found: " << _muscle_act[idx] << std::endl;
+				std::cout << "[" << (1.e3 * clock() - ts_start) / CLOCKS_PER_SEC << "ms] " << t << " (" << idx << ") actuation data for '" << muscle->getName() << "' found: " << _muscle_act[idx] << std::endl;
 
 			if (_muscle_act[idx] > 1.0) {
 				std::cerr << "Error: muscle actuation larger than 1.0 (" << _muscle_act[idx] << "), truncating" << std::endl;
@@ -41,8 +46,6 @@ void LocomotorPrimitivesController::computeControls(const SimTK::State &s, SimTK
 			std::cerr << "Error: no actuation data for muscle '" << muscle->getName() << "' at time " << t << " found" << std::endl;
 			continue;
 		}
-
-		//std::cout << "    " << p.getName() << "(" << p.getIdx() << "/" << p.getCapacity() << ")" << std::endl;
 	}
 
 	n++;
diff --git a/LocomotorPrimitivesManager.cpp b/LocomotorPrimitivesManager.cpp
index 71df313..d1925db 100644
--- a/LocomotorPrimitivesManager.cpp
+++ b/LocomotorPrimitivesManager.cpp
@@ -4,6 +4,6 @@
 
 bool LocomotorPrimitivesManager::doIntegration(SimTK::State &s, int step, double dtFirst)
 {
-	// TODO possiblly additional calculations
+	// TODO possibly additional calculations
 	return OpenSim::Manager::doIntegration(s, step, dtFirst);
-}
\ No newline at end of file
+}