From ad6a598de4025321a5227edf2a676c0f3397927c Mon Sep 17 00:00:00 2001
From: Tobias Klauser <tklauser@distanz.ch>
Date: Wed, 5 Dec 2012 21:14:14 +0100
Subject: Basic infrastructure to read muscle activation profiles from CSV

---
 LocomotorPrimitivesController.cpp | 68 +++++++++++++++++++++++++++++++++++++--
 1 file changed, 66 insertions(+), 2 deletions(-)

(limited to 'LocomotorPrimitivesController.cpp')

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
-- 
cgit v1.2.3-54-g00ecf