Read muscle activation from .sto

4 files changed, 58 insertions, 8 deletions

diff --git a/LocomotorPrimitives.cpp b/LocomotorPrimitives.cpp
index af1983a..0de339f 100644
--- a/LocomotorPrimitives.cpp
+++ b/LocomotorPrimitives.cpp
@@ -23,8 +23,8 @@ static void constructModel(OpenSim::Model &model)
 	std::cout << "Constructing " << MODEL_NAME << " model" << std::endl;
 
 	// Define controller for the model
-	LocomotorPrimitivesController *control = new LocomotorPrimitivesController(0.01);
-	control->loadCsvData("es_r", "../../data/adults_es_r.csv");
+	OpenSim::Storage actData("../../data/adults.sto");
+	LocomotorPrimitivesController *control = new LocomotorPrimitivesController(actData, 0.01);
 	control->setActuators(model.updActuators());
 	model.addController(control);
 
diff --git a/LocomotorPrimitivesController.cpp b/LocomotorPrimitivesController.cpp
index 84b6187..e4308bc 100644
--- a/LocomotorPrimitivesController.cpp
+++ b/LocomotorPrimitivesController.cpp
@@ -46,7 +46,7 @@ int LocomotorPrimitivesController::loadCsvData(const std::string &muscleName, co
 
 	data.close();
 
-	_act.push_back(p);
+	//_act.push_back(p);
 	std::cout << ">>> loaded EMG profile for " << p.getName() << ", " << p.getIdx() << "/" << p.getCapacity() << std::endl;
 	
 	return 0;
@@ -58,12 +58,22 @@ void LocomotorPrimitivesController::computeControls(const SimTK::State &s, SimTK
 	static double last_twitch = t;
 
 	/* Extract muscle activation for each of the muscles */
-	for (std::vector<MuscleEMGProfile>::const_iterator it = _act.begin(); it != _act.end(); ++it) {
-		MuscleEMGProfile p = *it;
+	int act_set_siz = getActuatorSet().getSize();
+	for (int i = 0; i < act_set_siz; i++) {
+		const OpenSim::Muscle *m = dynamic_cast<const OpenSim::Muscle *>(&getActuatorSet().get(i));
+		const OpenSim::Array<int> indices = _act.getColumnIndicesForIdentifier(m->getName());
+
+		if (indices.getSize() != 0) {
+			double muscle_act = 0.0;
+
+			_act.getDataAtTime(t, indices.get(0), &muscle_act);
+			std::cout << indices.get(0) << "    actuation data for '" << m->getName() << "' found: " << muscle_act << std::endl;
+		}
 
 		//std::cout << "    " << p.getName() << "(" << p.getIdx() << "/" << p.getCapacity() << ")" << std::endl;
 	}
 
+#if 0
 	//const OpenSim::Muscle *rectfem = dynamic_cast<const OpenSim::Muscle *>(&getActuatorSet().get("bifemlh_r"));
 	const OpenSim::Muscle *rectfem = dynamic_cast<const OpenSim::Muscle *>(&getActuatorSet().get("rect_fem_r"));
 	const OpenSim::Muscle *ercspn_r = dynamic_cast<const OpenSim::Muscle *>(&getActuatorSet().get("ercspn_r"));
@@ -98,4 +108,5 @@ void LocomotorPrimitivesController::computeControls(const SimTK::State &s, SimTK
 	ercspn_l->addInControls(ctrl_ercsp, controls);
 	extobl_r->addInControls(ctrl_extobl, controls);
 	extobl_l->addInControls(ctrl_extobl, controls);
+#endif
 }
\ No newline at end of file
diff --git a/LocomotorPrimitivesController.h b/LocomotorPrimitivesController.h
index 0760459..6c52fda 100644
--- a/LocomotorPrimitivesController.h
+++ b/LocomotorPrimitivesController.h
@@ -1,7 +1,6 @@
 #ifndef LOCOMOTORPRIMITIVES_CONTROLLER_H_
 #define LOCOMOTORPRIMITIVES_CONTROLLER_H_
 
-#include <map>
 #include <OpenSim/OpenSim.h>
 #include "MuscleEMGProfile.h"
 
@@ -9,14 +8,14 @@ class LocomotorPrimitivesController : public OpenSim::Controller {
 OpenSim_DECLARE_CONCRETE_OBJECT(LocomotorPrimitivesController, OpenSim::Controller);
 
 public:
-	LocomotorPrimitivesController(double alpha) : OpenSim::Controller(), _alpha(alpha) { }
+	LocomotorPrimitivesController(OpenSim::Storage act, double alpha) : OpenSim::Controller(), _act(act), _alpha(alpha) { }
 
 	int loadCsvData(const std::string &muscleName, const std::string &file);
 	void computeControls(const SimTK::State &s, SimTK::Vector &controls) const;
 private:
 	double _alpha;
 
-	std::vector<MuscleEMGProfile> _act;
+	OpenSim::Storage _act;
 };
 
 #endif /* LOCOMOTORPRIMITIVES_CONTROLLER_H_ */
diff --git a/data/adults.sto b/data/adults.sto
new file mode 100644
index 0000000..66e2002
--- /dev/null
+++ b/data/adults.sto
@@ -0,0 +1,40 @@
+control
+version=1
+nRows=35
+nColumns=2
+endheader
+time	ercspn_r
+0.0	6.09756
+2.54237	7.31707
+4.23729	9.7561
+7.62712	9.7561
+11.0169	7.31707
+12.7119	7.31707
+14.4068	6.09756
+17.7966	6.09756
+19.4915	4.87805
+21.1864	4.87805
+22.8814	3.65854
+29.661	3.65854
+31.3559	2.43902
+35.5932	2.43902
+43.2203	2.43902
+47.4576	6.09756
+50.8475	10.9756
+55.0847	17.0732
+58.4746	17.0732
+60.1695	14.6341
+61.8644	13.4146
+65.2542	9.7561
+68.6441	8.53659
+72.0339	7.31707
+73.7288	6.09756
+77.1186	4.87805
+79.661	4.87805
+81.3559	3.65854
+85.5932	3.65854
+89.8305	3.65854
+91.5254	2.43902
+94.9153	3.65854
+98.3051	4.87805
+100	4.87805