summaryrefslogtreecommitdiff
path: root/LocomotorPrimitivesController.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'LocomotorPrimitivesController.cpp')
-rw-r--r--LocomotorPrimitivesController.cpp68
1 files changed, 66 insertions, 2 deletions
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