diff options
Diffstat (limited to 'LocomotorPrimitivesController.cpp')
-rw-r--r-- | LocomotorPrimitivesController.cpp | 37 |
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 |