From 2e820c0ab42f43dcd2ddc5d6596782783a083396 Mon Sep 17 00:00:00 2001 From: Tobias Klauser Date: Fri, 12 Apr 2013 14:01:46 +0200 Subject: LocomotorPrimitivesController: Make splitting of muscles easier --- LocomotorPrimitivesController.cpp | 204 ++++++++++++++++++++++++-------------- 1 file changed, 129 insertions(+), 75 deletions(-) (limited to 'LocomotorPrimitivesController.cpp') diff --git a/LocomotorPrimitivesController.cpp b/LocomotorPrimitivesController.cpp index 8a85e4f..45fabc3 100644 --- a/LocomotorPrimitivesController.cpp +++ b/LocomotorPrimitivesController.cpp @@ -11,6 +11,30 @@ static const double TIME_DAMP = 0.1; static const unsigned int N_PRINT = 1000; +enum { + BIFEMLH, + BIFEMSH, + SEMIMEM, + SEMITEN, + GLUT_MAX1, + GLUT_MAX2, + GLUT_MAX3, + TFL, +}; + +/* TODO: Make the whole scaling business a bit more sophisticated, e.g. by + * looking up a mapping name -> enum value */ +static const double SCALE_MACT[] = { + [BIFEMLH] = 1.0, + [BIFEMSH] = 1.0, + [SEMIMEM] = 1.0, + [SEMITEN] = 1.0, + [GLUT_MAX1] = 1.0, + [GLUT_MAX2] = 1.0, + [GLUT_MAX3] = 1.0, + [TFL] = 1.0, +}; + static Logger &logger = Logger::getInstance(); int LocomotorPrimitivesController::checkControls() @@ -87,13 +111,10 @@ double LocomotorPrimitivesController::getMuscleActivation(const std::string &mus return mact; } -void LocomotorPrimitivesController::computeControls(const SimTK::State &s, - SimTK::Vector &controls) const +/* Initialize the muscle actuation data array for all muscles, at a given + * timestep */ +int LocomotorPrimitivesController::getAndInitActuationData(const double t) const { - double t = s.getTime(); - static double last_twitch = t; - static unsigned int n = 0; - /* Extract muscle activation for each of the muscles */ const int act_set_siz = getActuatorSet().getSize(); const int nc = _act.getSmallestNumberOfStates(); @@ -102,79 +123,112 @@ void LocomotorPrimitivesController::computeControls(const SimTK::State &s, * with getData()). */ _act.getDataAtTime(t, nc, _muscle_act); + return act_set_siz; +} + +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; + const int act_set_siz = getAndInitActuationData(t); + for (int i = 0; i < act_set_siz; i++) { const OpenSim::Muscle *muscle = dynamic_cast(&getActuatorSet().get(i)); 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 (n % N_PRINT == 0) - logger.log("timestep %f: actuation data for " - "muscle '%s': %f\n", - t, muscle_name.c_str(), mact); + double mact; + + /* 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; + mact *= SCALE_MACT[BIFEMLH]; + } else if (muscle_name.compare("bifemlh_l") == 0) { + mact = getMuscleActivation("hs_l"); + mact /= 4.0; + mact *= SCALE_MACT[BIFEMLH]; + } else if (muscle_name.compare("bifemsh_r") == 0) { + mact = getMuscleActivation("hs_r"); + mact /= 4.0; + mact *= SCALE_MACT[BIFEMSH]; + } else if (muscle_name.compare("bifemsh_l") == 0) { + mact = getMuscleActivation("hs_l"); + mact /= 4.0; + mact *= SCALE_MACT[BIFEMSH]; + } else if (muscle_name.compare("semimem_r") == 0) { + mact = getMuscleActivation("hs_r"); + mact /= 4.0; + mact *= SCALE_MACT[SEMIMEM]; + } else if (muscle_name.compare("semimem_l") == 0) { + mact = getMuscleActivation("hs_l"); + mact /= 4.0; + mact *= SCALE_MACT[SEMIMEM]; + } else if (muscle_name.compare("semiten_r") == 0) { + mact = getMuscleActivation("hs_r"); + mact /= 4.0; + mact *= SCALE_MACT[SEMITEN]; + } else if (muscle_name.compare("semiten_l") == 0) { + mact = getMuscleActivation("hs_l"); + mact /= 4.0; + mact *= SCALE_MACT[SEMITEN]; + } else if (muscle_name.compare("glut_max1_r") == 0) { + mact = getMuscleActivation("gm_r"); + mact /= 3.0; + mact *= SCALE_MACT[GLUT_MAX1]; + } else if (muscle_name.compare("glut_max1_l") == 0) { + mact = getMuscleActivation("gm_l"); + mact /= 3.0; + mact *= SCALE_MACT[GLUT_MAX1]; + } else if (muscle_name.compare("glut_max2_r") == 0) { + mact = getMuscleActivation("gm_r"); + mact /= 3.0; + mact *= SCALE_MACT[GLUT_MAX2]; + } else if (muscle_name.compare("glut_max2_l") == 0) { + mact = getMuscleActivation("gm_l"); + mact /= 3.0; + mact *= SCALE_MACT[GLUT_MAX2]; + } else if (muscle_name.compare("glut_max3_r") == 0) { + mact = getMuscleActivation("gm_r"); + mact /= 3.0; + mact *= SCALE_MACT[GLUT_MAX3]; + } else if (muscle_name.compare("glut_max3_l") == 0) { + mact = getMuscleActivation("gm_l"); + mact /= 3.0; + mact *= SCALE_MACT[GLUT_MAX3]; + } else { + /* Otherwise just get the actuaction data for + * the plain name, it must be there! */ + mact = getMuscleActivation(muscle_name); + + /* Allow to scale some muscles afterwards */ + if (muscle_name.compare("tfl_l") == 0) + mact *= SCALE_MACT[TFL]; + if (muscle_name.compare("tfl_r") == 0) + mact *= SCALE_MACT[TFL]; + } -} + if (mact > 1.0) { + logger.warn("Actuation data for '%s' larger than 1.0, truncating.\n", muscle_name.c_str()); + mact = 1.0; + } + + if (n % N_PRINT == 0) + logger.log("timestep %f: actuation data for " + "muscle '%s': %f\n", + t, muscle_name.c_str(), mact); SimTK::Vector ctrl(1, mact); muscle->addInControls(ctrl, controls); -- cgit v1.2.3-54-g00ecf