LocomotorPrimitivesController: Implement splitting of combined muscles

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_ */