From e9f4f691e554000f9decf84da56568d6150badb2 Mon Sep 17 00:00:00 2001 From: Tobias Klauser Date: Thu, 22 Nov 2012 19:22:42 +0100 Subject: Initial import (based on Leg6DoF9Muscles model) --- LocomotorPrimitivesController.cpp | 37 +++++++++++++++++++++++++++++++++++++ 1 file changed, 37 insertions(+) create mode 100644 LocomotorPrimitivesController.cpp (limited to 'LocomotorPrimitivesController.cpp') 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 + +#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(&getActuatorSet().get("bifemlh_r")); + const OpenSim::Muscle *rectfem = dynamic_cast(&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 -- cgit v1.2.3-54-g00ecf