diff options
| -rw-r--r-- | LocomotorPrimitivesController.cpp | 113 | ||||
| -rw-r--r-- | LocomotorPrimitivesController.h | 2 |
2 files changed, 98 insertions, 17 deletions
diff --git a/LocomotorPrimitivesController.cpp b/LocomotorPrimitivesController.cpp index 63d1c26..ccf45e5 100644 --- a/LocomotorPrimitivesController.cpp +++ b/LocomotorPrimitivesController.cpp @@ -1,3 +1,9 @@ +/* + * Author: Tobias Klauser <tobias.klauser@uzh.ch> + * + * Copyright (C) 2013 Tobias Klauser <tobias.klauser@uzh.ch> + */ + #include <OpenSim/OpenSim.h> #include "LocomotorPrimitivesController.h" @@ -27,7 +33,28 @@ int LocomotorPrimitivesController::checkControls() return ret; } -void LocomotorPrimitivesController::computeControls(const SimTK::State &s, SimTK::Vector &controls) const +double LocomotorPrimitivesController::getMuscleActivation(const std::string &muscle_name) const +{ + double mact; + const OpenSim::Array<int> indices = _act.getColumnIndicesForIdentifier(muscle_name); + + if (indices.getSize() != 0) { + int idx = indices.get(0) - 1; + if (_muscle_act[idx] > 1.0) { + logger.err("%s (%d): muscle actuation larger than 1.0 (%f), truncating to 1.0\n", muscle_name.c_str(), idx, _muscle_act[idx]); + _muscle_act[idx] = 1.0; + } + + mact = _muscle_act[idx]; + } else { + mact = 0.0; + } + + return mact; +} + +void LocomotorPrimitivesController::computeControls(const SimTK::State &s, + SimTK::Vector &controls) const { double t = s.getTime(); static double last_twitch = t; @@ -43,25 +70,77 @@ void LocomotorPrimitivesController::computeControls(const SimTK::State &s, SimTK 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()); - double mact; + std::string muscle_name = muscle->getName(); + double mact = getMuscleActivation(muscle_name); + + if (mact == 0.0) { + /* Special case: In the paper some muscles were measured + * in combination but they're present as individual + * muscles in the model (see list below). Thus we + * linearly distribute the activation value among the + * individual muscles of the combination. + * + * TODO: Introduce scale factor for each muscle. + * + * measurement muscle model + * ----------------------------------------------------- + * HS (hamstring) bifemlh bifemsh semimem semiten + * GM (gluteus maximus) glut_max1 glut_max2 glut_max3 + */ + if (muscle_name.compare("bifemlh_r") == 0) { + mact = getMuscleActivation("hs_r"); + mact /= 4.0; + } else if (muscle_name.compare("bifemlh_l") == 0) { + mact = getMuscleActivation("hs_l"); + mact /= 4.0; + } else if (muscle_name.compare("bifemsh_r") == 0) { + mact = getMuscleActivation("hs_r"); + mact /= 4.0; + } else if (muscle_name.compare("bifemsh_l") == 0) { + mact = getMuscleActivation("hs_l"); + mact /= 4.0; + } else if (muscle_name.compare("semimem_r") == 0) { + mact = getMuscleActivation("hs_r"); + mact /= 4.0; + } else if (muscle_name.compare("semimem_l") == 0) { + mact = getMuscleActivation("hs_l"); + mact /= 4.0; + } else if (muscle_name.compare("semiten_r") == 0) { + mact = getMuscleActivation("hs_r"); + mact /= 4.0; + } else if (muscle_name.compare("semiten_l") == 0) { + mact = getMuscleActivation("hs_l"); + mact /= 4.0; + } else if (muscle_name.compare("glut_max1_r") == 0) { + mact = getMuscleActivation("gm_r"); + mact /= 3.0; + } else if (muscle_name.compare("glut_max1_l") == 0) { + mact = getMuscleActivation("gm_l"); + mact /= 3.0; + } else if (muscle_name.compare("glut_max2_r") == 0) { + mact = getMuscleActivation("gm_r"); + mact /= 3.0; + } else if (muscle_name.compare("glut_max2_l") == 0) { + mact = getMuscleActivation("gm_l"); + mact /= 3.0; + } else if (muscle_name.compare("glut_max3_r") == 0) { + mact = getMuscleActivation("gm_r"); + mact /= 3.0; + } else if (muscle_name.compare("glut_max3_l") == 0) { + mact = getMuscleActivation("gm_l"); + mact /= 3.0; + } else { + // there's really no actuation data for muscle + // at that time, so set it to 0.0 + mact = 0.0; + } - if (indices.getSize() != 0) { - int idx = indices.get(0) - 1; if (n % N_PRINT == 0) - logger.log("timestep %f: actuation data for muscle '%s' (%d) found: %f\n", - t, muscle->getName().c_str(), idx, _muscle_act[idx]); + logger.log("timestep %f: actuation data for" + "muscle '%s': %f\n", + t, muscle_name.c_str(), mact); - if (_muscle_act[idx] > 1.0) { - logger.err("muscle actuation larger than 1.0 (%f), truncating to 1.0\n", _muscle_act[idx]); - _muscle_act[idx] = 1.0; - } - - mact = _muscle_act[idx]; - } else { - // no actuation data for muscle at that time, so set it to 0.0 - mact = 0.0; - } +} SimTK::Vector ctrl(1, mact); muscle->addInControls(ctrl, controls); diff --git a/LocomotorPrimitivesController.h b/LocomotorPrimitivesController.h index 578a659..68bf1af 100644 --- a/LocomotorPrimitivesController.h +++ b/LocomotorPrimitivesController.h @@ -34,6 +34,8 @@ private: double *_muscle_act; OpenSim::Storage _act; + + double getMuscleActivation(const std::string &muscle_name) const; }; #endif /* LOCOMOTORPRIMITIVES_CONTROLLER_H_ */ |