summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--LocomotorPrimitivesController.cpp204
-rw-r--r--LocomotorPrimitivesController.h1
2 files changed, 130 insertions, 75 deletions
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<const OpenSim::Muscle *>(&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);
diff --git a/LocomotorPrimitivesController.h b/LocomotorPrimitivesController.h
index 68bf1af..a2d1f30 100644
--- a/LocomotorPrimitivesController.h
+++ b/LocomotorPrimitivesController.h
@@ -35,6 +35,7 @@ private:
OpenSim::Storage _act;
+ int getAndInitActuationData(const double t) const;
double getMuscleActivation(const std::string &muscle_name) const;
};
generated by cgit v1.2.3-54-g00ecf (git 2.39.0) at 2024-04-20 06:07:51 +0000