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
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
|
#include <OpenSim/OpenSim.h>
#include "LocomotorPrimitivesController.h"
static const double TIME_DAMP = 0.1;
static const unsigned int N_PRINT = 1000;
static Logger &logger = Logger::getInstance();
int LocomotorPrimitivesController::checkControls()
{
const int act_set_siz = getActuatorSet().getSize();
int ret = 0;
/* Iterate over all controls in the model and check whether they're
* available in the loaded storage */
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) {
logger.err("no actuation data for muscle '%s' found\n", muscle->getName().c_str());
ret--;
}
}
return ret;
}
void LocomotorPrimitivesController::computeControls(const SimTK::State &s, SimTK::Vector &controls) const
{
double t = s.getTime();
static double last_twitch = t;
static unsigned int n = 0;
/* Extract muscle activation for each of the muscles */
const int act_set_siz = getActuatorSet().getSize();
const 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());
double mact;
if (indices.getSize() != 0) {
int idx = indices.get(0) - 1;
if (n % N_PRINT == 0)
logger.log("%f (%d) actuation data for muscle '%s' found: %f\n",
t, idx, muscle->getName().c_str(), _muscle_act[idx]);
if (_muscle_act[idx] > 1.0) {
logger.err("muscle actuation larger than 1.0 (%f), truncating to 1.0\n", _muscle_act[idx]);
_muscle_act[idx] = 1.0;
}
mact = _muscle_act[idx];
} else {
// no actuation data for muscle at that time, so set it to 0.0
mact = 0.0;
}
SimTK::Vector ctrl(1, mact);
muscle->addInControls(ctrl, controls);
}
n++;
}
|
