diff options
| -rw-r--r-- | LocomotorPrimitives.cpp | 4 | ||||
| -rw-r--r-- | LocomotorPrimitivesController.cpp | 32 | ||||
| -rw-r--r-- | LocomotorPrimitivesController.h | 8 |
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; |