Basic infrastructure to read muscle activation profiles from CSV

6 files changed, 165 insertions, 5 deletions

diff --git a/LocomotorPrimitives.cpp b/LocomotorPrimitives.cpp
index 341fa74..ea5f247 100644
--- a/LocomotorPrimitives.cpp
+++ b/LocomotorPrimitives.cpp
@@ -14,7 +14,6 @@
 #define NO_SIM			0
 
 static const std::string MODEL_NAME = "LocomotorPrimitives";
-
 // Define the initial and final simulation time
 static const double initialTime = 0.0;
 static const double finalTime = 2.0;
@@ -25,6 +24,7 @@ static void constructModel(OpenSim::Model &model)
 
 	// Define controller for the model
 	LocomotorPrimitivesController *control = new LocomotorPrimitivesController(0.01);
+	control->loadCsvData("es_r", "../../adults_es_r.csv");
 	control->setActuators(model.updActuators());
 	model.addController(control);
 
@@ -37,8 +37,8 @@ static void constructModel(OpenSim::Model &model)
 	for (int i = 0; i < sz; i++) {
 		OpenSim::ActivationFiberLengthMuscle *m = dynamic_cast<OpenSim::ActivationFiberLengthMuscle *>(&actSet.get(i));
 		std::cout << "   " << m->getName() << std::endl;
-		m->setDefaultActivation(0.01);
-		m->setDefaultFiberLength(0.1);
+//		m->setDefaultActivation(0.5);
+//		m->setDefaultFiberLength(0.1);
 	}
 
 	// Create the force reporter for obtaining the forces applied to the model
diff --git a/LocomotorPrimitivesController.cpp b/LocomotorPrimitivesController.cpp
index 4b7bc1b..381e21f 100644
--- a/LocomotorPrimitivesController.cpp
+++ b/LocomotorPrimitivesController.cpp
@@ -1,18 +1,75 @@
 #include <OpenSim/OpenSim.h>
 
 #include "LocomotorPrimitivesController.h"
+#include "MuscleEMGProfile.h"
 
 #define VERBOSE 1
 
 static const double TIME_DAMP = 0.1;
 
+int LocomotorPrimitivesController::loadCsvData(const std::string &muscleName, const std::string &file)
+{
+	std::cout << ">> loading CSV data from " << file << std::endl;
+	std::ifstream data(file);
+	std::string line;
+
+	std::cout << data.tellg() << std::endl;
+
+	if (!data.is_open()) {
+		std::cerr << "Error loading CSV data from " << file << std::endl;
+		return -1;
+	}
+
+	MuscleEMGProfile p(muscleName, 0);
+	
+	while (std::getline(data, line)) {
+		std::stringstream s(line);
+		std::string cell;
+
+		int i = 0;
+		double x;
+		while (std::getline(s, cell, ',')) {
+			if (!isdigit(cell[0]))
+				continue;
+			
+			std::cout << "read cell(" << (i == 0 ? "x" : "y") << ") -> " << cell << std::endl;
+			double val = (double) atof(cell.c_str());
+
+			if (i == 1)
+				p.addData(x, val);
+			else
+				x = val;
+
+			i = !i;
+		}
+	}
+
+	data.close();
+
+	_act.push_back(p);
+	std::cout << ">>> loaded EMG profile for " << p.getName() << ", " << p.getIdx() << "/" << p.getCapacity() << std::endl;
+	
+	return 0;
+}
+
 void LocomotorPrimitivesController::computeControls(const SimTK::State &s, SimTK::Vector &controls) const
 {
 	double t = s.getTime();
 	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;
+
+		std::cout << "    " << p.getName() << "(" << p.getIdx() << "/" << p.getCapacity() << ")" << std::endl;
+	}
+
 	//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"));
+	const OpenSim::Muscle *ercspn_l = dynamic_cast<const OpenSim::Muscle *>(&getActuatorSet().get("ercspn_l"));
+	const OpenSim::Muscle *extobl_r = dynamic_cast<const OpenSim::Muscle *>(&getActuatorSet().get("extobl_r"));
+	const OpenSim::Muscle *extobl_l = dynamic_cast<const OpenSim::Muscle *>(&getActuatorSet().get("extobl_l"));
 
 	double v = rectfem->getLengtheningSpeed(s);
 	double act = v * _alpha;
@@ -28,10 +85,17 @@ void LocomotorPrimitivesController::computeControls(const SimTK::State &s, SimTK
 	if (act > 0.0)
 		last_twitch = t;
 	
-	if (VERBOSE && act > 0.0)
-		std::cout << "(" << std::fixed << t << ") " << "v=" << std::fixed << v << ", act=" << std::fixed << act << std::endl;
+	//if (VERBOSE && act > 0.0)
+	//	std::cout << "(" << std::fixed << t << ") " << "v=" << std::fixed << v << ", act=" << std::fixed << act << std::endl;
 
 	SimTK::Vector ctrl_rectfem(1, act);
+	SimTK::Vector ctrl_extobl(1, 0.79);
+	SimTK::Vector ctrl_ercsp(1, 0.375);
 
 	rectfem->addInControls(ctrl_rectfem, controls);
+
+	ercspn_r->addInControls(ctrl_ercsp, controls);
+	ercspn_l->addInControls(ctrl_ercsp, controls);
+	extobl_r->addInControls(ctrl_extobl, controls);
+	extobl_l->addInControls(ctrl_extobl, controls);
 }
\ No newline at end of file
diff --git a/LocomotorPrimitivesController.h b/LocomotorPrimitivesController.h
index 7d0c08a..0760459 100644
--- a/LocomotorPrimitivesController.h
+++ b/LocomotorPrimitivesController.h
@@ -1,7 +1,9 @@
 #ifndef LOCOMOTORPRIMITIVES_CONTROLLER_H_
 #define LOCOMOTORPRIMITIVES_CONTROLLER_H_
 
+#include <map>
 #include <OpenSim/OpenSim.h>
+#include "MuscleEMGProfile.h"
 
 class LocomotorPrimitivesController : public OpenSim::Controller {
 OpenSim_DECLARE_CONCRETE_OBJECT(LocomotorPrimitivesController, OpenSim::Controller);
@@ -9,9 +11,12 @@ OpenSim_DECLARE_CONCRETE_OBJECT(LocomotorPrimitivesController, OpenSim::Controll
 public:
 	LocomotorPrimitivesController(double alpha) : OpenSim::Controller(), _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;
 };
 
 #endif /* LOCOMOTORPRIMITIVES_CONTROLLER_H_ */
diff --git a/MuscleEMGProfile.cpp b/MuscleEMGProfile.cpp
new file mode 100644
index 0000000..b1b4977
--- /dev/null
+++ b/MuscleEMGProfile.cpp
@@ -0,0 +1,7 @@
+#include "MuscleEMGProfile.h"
+
+void MuscleEMGProfile::addData(double x, double y)
+{
+	_x.push_back(x);
+	_y.push_back(y);
+}
\ No newline at end of file
diff --git a/MuscleEMGProfile.h b/MuscleEMGProfile.h
new file mode 100644
index 0000000..2d31f27
--- /dev/null
+++ b/MuscleEMGProfile.h
@@ -0,0 +1,49 @@
+#ifndef MUSCLE_EMG_PROFILE_H_
+#define MUSCLE_EMG_PROFILE_H_
+
+#include <ostream>
+#include <string>
+#include <vector>
+
+class MuscleEMGProfile {
+public:
+	MuscleEMGProfile(const std::string &name, unsigned capacity)
+			: _name(const_cast<std::string &>(name))
+	{
+		if (capacity == 0)
+			capacity = 100;
+		_x.reserve(capacity);
+		_y.reserve(capacity);
+	}
+
+	MuscleEMGProfile& operator=(const MuscleEMGProfile &m)
+	{
+		_name = m._name;
+		_x = m._x;
+		_y = m._y;
+
+		return *this;
+	}
+
+	unsigned getIdx() { return _x.size(); }
+	unsigned getCapacity() { return _x.max_size(); }
+	const std::string &getName() { return _name; }
+
+	void addData(double x, double y);
+
+private:
+	std::string _name;
+	std::vector<double> _x;
+	std::vector<double> _y;
+
+//	friend std::ostream& operator<<(std::ostream &s, const MuscleEMGProfile &m);
+};
+
+/*
+std::ostream& operator<<(std::ostream &s, const MuscleEMGProfile &m)
+{
+	return s << "MuscleEMGProfile(" << m._name << ", " << m._idx << "/" << m._capacity << ")" << std::endl;
+}
+*/
+
+#endif /* MUSCLE_EMG_PROFILE_H_ */
\ No newline at end of file
diff --git a/adults_es_r.csv b/adults_es_r.csv
new file mode 100644
index 0000000..2481326
--- /dev/null
+++ b/adults_es_r.csv
@@ -0,0 +1,35 @@
+x,y
+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