LocomotorPrimitivesController.cpp


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
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);
}