diff options
| -rw-r--r-- | LocomotorPrimitivesController.cpp | 12 |
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++; } |