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
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
|
/*
* Author: Tobias Klauser <tobias.klauser@uzh.ch>
*
* Copyright (C) 2013 Tobias Klauser <tobias.klauser@uzh.ch>
*/
#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;
}
double LocomotorPrimitivesController::getMuscleActivation(const std::string &muscle_name) const
{
double mact;
const OpenSim::Array<int> indices = _act.getColumnIndicesForIdentifier(muscle_name);
if (indices.getSize() != 0) {
int idx = indices.get(0) - 1;
if (_muscle_act[idx] > 1.0) {
logger.err("%s (%d): muscle actuation larger than 1.0 (%f), truncating to 1.0\n", muscle_name.c_str(), idx, _muscle_act[idx]);
_muscle_act[idx] = 1.0;
}
mact = _muscle_act[idx];
} else {
mact = 0.0;
}
return mact;
}
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));
std::string muscle_name = muscle->getName();
double mact = getMuscleActivation(muscle_name);
if (mact == 0.0) {
/* Special case: In the paper some muscles were measured
* in combination but they're present as individual
* muscles in the model (see list below). Thus we
* linearly distribute the activation value among the
* individual muscles of the combination.
*
* TODO: Introduce scale factor for each muscle.
*
* measurement muscle model
* -----------------------------------------------------
* HS (hamstring) bifemlh bifemsh semimem semiten
* GM (gluteus maximus) glut_max1 glut_max2 glut_max3
*/
if (muscle_name.compare("bifemlh_r") == 0) {
mact = getMuscleActivation("hs_r");
mact /= 4.0;
} else if (muscle_name.compare("bifemlh_l") == 0) {
mact = getMuscleActivation("hs_l");
mact /= 4.0;
} else if (muscle_name.compare("bifemsh_r") == 0) {
mact = getMuscleActivation("hs_r");
mact /= 4.0;
} else if (muscle_name.compare("bifemsh_l") == 0) {
mact = getMuscleActivation("hs_l");
mact /= 4.0;
} else if (muscle_name.compare("semimem_r") == 0) {
mact = getMuscleActivation("hs_r");
mact /= 4.0;
} else if (muscle_name.compare("semimem_l") == 0) {
mact = getMuscleActivation("hs_l");
mact /= 4.0;
} else if (muscle_name.compare("semiten_r") == 0) {
mact = getMuscleActivation("hs_r");
mact /= 4.0;
} else if (muscle_name.compare("semiten_l") == 0) {
mact = getMuscleActivation("hs_l");
mact /= 4.0;
} else if (muscle_name.compare("glut_max1_r") == 0) {
mact = getMuscleActivation("gm_r");
mact /= 3.0;
} else if (muscle_name.compare("glut_max1_l") == 0) {
mact = getMuscleActivation("gm_l");
mact /= 3.0;
} else if (muscle_name.compare("glut_max2_r") == 0) {
mact = getMuscleActivation("gm_r");
mact /= 3.0;
} else if (muscle_name.compare("glut_max2_l") == 0) {
mact = getMuscleActivation("gm_l");
mact /= 3.0;
} else if (muscle_name.compare("glut_max3_r") == 0) {
mact = getMuscleActivation("gm_r");
mact /= 3.0;
} else if (muscle_name.compare("glut_max3_l") == 0) {
mact = getMuscleActivation("gm_l");
mact /= 3.0;
} else {
// there's really no actuation data for muscle
// at that time, so set it to 0.0
mact = 0.0;
}
if (n % N_PRINT == 0)
logger.log("timestep %f: actuation data for"
"muscle '%s': %f\n",
t, muscle_name.c_str(), mact);
}
SimTK::Vector ctrl(1, mact);
muscle->addInControls(ctrl, controls);
}
n++;
}
|
