From 1ed2797955836050c1a84121c1fbd053f96b646f Mon Sep 17 00:00:00 2001
From: Tobias Klauser <tklauser@distanz.ch>
Date: Wed, 16 Jan 2013 17:00:30 +0100
Subject: LocomotorPrimitivesController: Fix to read correct data from storage

---
 LocomotorPrimitivesController.cpp | 59 +++++++++++----------------------------
 LocomotorPrimitivesController.h   | 13 +++++++--
 2 files changed, 27 insertions(+), 45 deletions(-)

diff --git a/LocomotorPrimitivesController.cpp b/LocomotorPrimitivesController.cpp
index d22ee71..69f7c72 100644
--- a/LocomotorPrimitivesController.cpp
+++ b/LocomotorPrimitivesController.cpp
@@ -13,54 +13,27 @@ void LocomotorPrimitivesController::computeControls(const SimTK::State &s, SimTK
 
 	/* Extract muscle activation for each of the muscles */
 	int act_set_siz = getActuatorSet().getSize();
+	int nc = _act.getSmallestNumberOfStates();
+
+	// Get the first nc states at a specified time (_NOT_ the state at nc as
+	// with getData()).
+	_act.getDataAtTime(t, nc, _muscle_act);
+
 	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());
+		const OpenSim::Muscle *muscle = dynamic_cast<const OpenSim::Muscle *>(&getActuatorSet().get(i));
+		const OpenSim::Array<int> indices = _act.getColumnIndicesForIdentifier(muscle->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;
+			int idx = indices.get(0) - 1;
+			std::cout << t << " (" << idx << ") actuation data for '" << muscle->getName() << "' found: " << _muscle_act[idx] << std::endl;
+
+			SimTK::Vector ctrl(1, _muscle_act[idx]);
+			muscle->addInControls(ctrl, controls);
+		} else {
+			std::cerr << "Error: no actuation data for muscle '" << muscle->getName() << "' at time " << t << " found" << std::endl;
+			continue;
 		}
 
 		//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"));
-	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;
-
-	if (act < 0.0)
-		act = 0.0;
-	if (act > 1.0)
-		act = 1.0;
-
-	if (t - last_twitch < TIME_DAMP)
-		act = 0.0;
-
-	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;
-
-	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);
-#endif
 }
diff --git a/LocomotorPrimitivesController.h b/LocomotorPrimitivesController.h
index c0f75be..702a3a5 100644
--- a/LocomotorPrimitivesController.h
+++ b/LocomotorPrimitivesController.h
@@ -7,12 +7,21 @@ class LocomotorPrimitivesController : public OpenSim::Controller {
 OpenSim_DECLARE_CONCRETE_OBJECT(LocomotorPrimitivesController, OpenSim::Controller);
 
 public:
-	LocomotorPrimitivesController(OpenSim::Storage act, double alpha) : OpenSim::Controller(), _act(act), _alpha(alpha) { }
+	LocomotorPrimitivesController(OpenSim::Storage act, double alpha)
+		: OpenSim::Controller(), _act(act), _alpha(alpha)
+	{
+		_muscle_act = new double[act.getSmallestNumberOfStates()];
+	}
+
+	~LocomotorPrimitivesController()
+	{
+		delete[] _muscle_act;
+	}
 
-	int loadCsvData(const std::string &muscleName, const std::string &file);
 	void computeControls(const SimTK::State &s, SimTK::Vector &controls) const;
 private:
 	double _alpha;
+	double *_muscle_act;
 
 	OpenSim::Storage _act;
 };
-- 
cgit v1.2.3-54-g00ecf