blob: 69f7c727144e5466b6ddcd133020051f45ad1b88 (
plain)
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
38
39
|
#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;
/* 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 *muscle = dynamic_cast<const OpenSim::Muscle *>(&getActuatorSet().get(i));
const OpenSim::Array<int> indices = _act.getColumnIndicesForIdentifier(muscle->getName());
if (indices.getSize() != 0) {
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;
}
}
|