summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--LocomotorPrimitivesController.cpp59
-rw-r--r--LocomotorPrimitivesController.h13
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;
};