LocomotorPrimitives: Check availability of control data before running

3 files changed, 37 insertions, 7 deletions

diff --git a/LocomotorPrimitives.cpp b/LocomotorPrimitives.cpp
index 6b6f5f5..cfb484e 100644
--- a/LocomotorPrimitives.cpp
+++ b/LocomotorPrimitives.cpp
@@ -24,6 +24,10 @@ static void constructModel(OpenSim::Model &model, OpenSim::Storage actData)
 	// Define controller for the model
 	LocomotorPrimitivesController *control = new LocomotorPrimitivesController(actData, 0.01);
 	control->setActuators(model.updActuators());
+	if (control->checkControls() != 0) {
+		std::cerr << "Control data for some muscles in the model is missing. ENTER to continue, ^-C to quit." << std::endl;
+		std::cin.get();
+	}
 	model.addController(control);
 
 	// Set default activation and fiber length on all muscles of the model
diff --git a/LocomotorPrimitivesController.cpp b/LocomotorPrimitivesController.cpp
index 0c13336..2708162 100644
--- a/LocomotorPrimitivesController.cpp
+++ b/LocomotorPrimitivesController.cpp
@@ -10,6 +10,26 @@ extern clock_t ts_start;
 static const double TIME_DAMP = 0.1;
 static const unsigned int N_PRINT = 1000;
 
+int LocomotorPrimitivesController::checkControls()
+{
+	const int act_set_siz = getActuatorSet().getSize();
+	int ret = 0;
+
+	/* Iterate over all controls in the model and check whether they're
+	 * available in the loaded storage */
+	for (int i = 0; i < act_set_siz; i++) {
+		const OpenSim::Muscle *muscle = dynamic_cast<const OpenSim::Muscle *>(&getActuatorSet().get(i));
+		const OpenSim::Array<int> indices = _act.getColumnIndicesForIdentifier(muscle->getName());
+
+		if (indices.getSize() == 0) {
+			std::cerr << "Error: no actuation data for muscle '" << muscle->getName() << "' found" << std::endl;
+			ret--;
+		}
+	}
+
+	return ret;
+}
+
 void LocomotorPrimitivesController::computeControls(const SimTK::State &s, SimTK::Vector &controls) const
 {
 	double t = s.getTime();
@@ -17,13 +37,11 @@ void LocomotorPrimitivesController::computeControls(const SimTK::State &s, SimTK
 	static unsigned int n = 0;
 
 	/* Extract muscle activation for each of the muscles */
-	int act_set_siz = getActuatorSet().getSize();
-	int nc = _act.getSmallestNumberOfStates();
+	const int act_set_siz = getActuatorSet().getSize();
+	const 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++) {
@@ -43,7 +61,7 @@ void LocomotorPrimitivesController::computeControls(const SimTK::State &s, SimTK
 			SimTK::Vector ctrl(1, _muscle_act[idx]);
 			muscle->addInControls(ctrl, controls);
 		} else {
-			std::cerr << "Error: no actuation data for muscle '" << muscle->getName() << "' at time " << t << " found" << std::endl;
+			// std::cerr << "Error: no actuation data for muscle '" << muscle->getName() << "' at time " << t << " found" << std::endl;
 			continue;
 		}
 	}
diff --git a/LocomotorPrimitivesController.h b/LocomotorPrimitivesController.h
index 702a3a5..c33745e 100644
--- a/LocomotorPrimitivesController.h
+++ b/LocomotorPrimitivesController.h
@@ -18,9 +18,17 @@ public:
 		delete[] _muscle_act;
 	}
 
+	/**
+	 * Check availability of control data for all controls in the model.
+	 *
+	 * @return    0 if control data is available for all controls, negative
+	 *            number of missing control data vectors on error
+	 */
+	int checkControls();
 	void computeControls(const SimTK::State &s, SimTK::Vector &controls) const;
 private:
 	double _alpha;
+	/* used to store actuation data in compute controls */
 	double *_muscle_act;
 
 	OpenSim::Storage _act;