summaryrefslogtreecommitdiff
path: root/LocomotorPrimitivesController.cpp
diff options
context:
space:
mode:
authorTobias Klauser <tklauser@distanz.ch>2012-11-22 19:22:42 +0100
committerTobias Klauser <tklauser@distanz.ch>2012-11-22 19:22:42 +0100
commite9f4f691e554000f9decf84da56568d6150badb2 (patch)
tree70e3a719bf6c970205cfc0b37d0f46cd0fc84036 /LocomotorPrimitivesController.cpp
Initial import (based on Leg6DoF9Muscles model)
Diffstat (limited to 'LocomotorPrimitivesController.cpp')
-rw-r--r--LocomotorPrimitivesController.cpp37
1 files changed, 37 insertions, 0 deletions
diff --git a/LocomotorPrimitivesController.cpp b/LocomotorPrimitivesController.cpp
new file mode 100644
index 0000000..4b7bc1b
--- /dev/null
+++ b/LocomotorPrimitivesController.cpp
@@ -0,0 +1,37 @@
+#include <OpenSim/OpenSim.h>
+
+#include "LocomotorPrimitivesController.h"
+
+#define VERBOSE 1
+
+static const double TIME_DAMP = 0.1;
+
+void LocomotorPrimitivesController::computeControls(const SimTK::State &s, SimTK::Vector &controls) const
+{
+ double t = s.getTime();
+ static double last_twitch = t;
+
+ //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"));
+
+ 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);
+
+ rectfem->addInControls(ctrl_rectfem, controls);
+} \ No newline at end of file