LocomotorPrimitivesController: Only print state every 1000 steps

1 files changed, 11 insertions, 1 deletions

diff --git a/LocomotorPrimitivesController.cpp b/LocomotorPrimitivesController.cpp
index 69f7c72..14fa764 100644
--- a/LocomotorPrimitivesController.cpp
+++ b/LocomotorPrimitivesController.cpp
@@ -5,11 +5,13 @@
 #define VERBOSE 1
 
 static const double TIME_DAMP = 0.1;
+static const unsigned int N_PRINT = 1000;
 
 void LocomotorPrimitivesController::computeControls(const SimTK::State &s, SimTK::Vector &controls) const
 {
 	double t = s.getTime();
 	static double last_twitch = t;
+	static unsigned int n = 0;
 
 	/* Extract muscle activation for each of the muscles */
 	int act_set_siz = getActuatorSet().getSize();
@@ -25,7 +27,13 @@ void LocomotorPrimitivesController::computeControls(const SimTK::State &s, SimTK
 
 		if (indices.getSize() != 0) {
 			int idx = indices.get(0) - 1;
-			std::cout << t << " (" << idx << ") actuation data for '" << muscle->getName() << "' found: " << _muscle_act[idx] << std::endl;
+			if (n % N_PRINT == 0)
+				std::cout << 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;
+				_muscle_act[idx] = 1.0;
+			}
 
 			SimTK::Vector ctrl(1, _muscle_act[idx]);
 			muscle->addInControls(ctrl, controls);
@@ -36,4 +44,6 @@ void LocomotorPrimitivesController::computeControls(const SimTK::State &s, SimTK
 
 		//std::cout << "    " << p.getName() << "(" << p.getIdx() << "/" << p.getCapacity() << ")" << std::endl;
 	}
+
+	n++;
 }