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
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
|
/*
* 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;
enum {
BIFEMLH,
BIFEMSH,
SEMIMEM,
SEMITEN,
GLUT_MAX1,
GLUT_MAX2,
GLUT_MAX3,
TFL,
};
/* TODO: Make the whole scaling business a bit more sophisticated, e.g. by
* looking up a mapping name -> enum value */
static const double SCALE_MACT[] = {
[BIFEMLH] = 1.0,
[BIFEMSH] = 1.0,
[SEMIMEM] = 1.0,
[SEMITEN] = 1.0,
[GLUT_MAX1] = 1.0,
[GLUT_MAX2] = 1.0,
[GLUT_MAX3] = 1.0,
[TFL] = 1.0,
};
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));
std::string muscle_name = muscle->getName();
/* If it is one of the 'special' combined muscles, check the
* combination instead. See comment in computeControls() for
* details. */
if (muscle_name.compare("bifemlh_r") == 0) {
muscle_name = "hs_r";
} else if (muscle_name.compare("bifemlh_l") == 0) {
muscle_name = "hs_l";
} else if (muscle_name.compare("bifemsh_r") == 0) {
muscle_name = "hs_r";
} else if (muscle_name.compare("bifemsh_l") == 0) {
muscle_name = "hs_l";
} else if (muscle_name.compare("semimem_r") == 0) {
muscle_name = "hs_r";
} else if (muscle_name.compare("semimem_l") == 0) {
muscle_name = "hs_l";
} else if (muscle_name.compare("semiten_r") == 0) {
muscle_name = "hs_r";
} else if (muscle_name.compare("semiten_l") == 0) {
muscle_name = "hs_l";
} else if (muscle_name.compare("glut_max1_r") == 0) {
muscle_name = "gm_r";
} else if (muscle_name.compare("glut_max1_l") == 0) {
muscle_name = "gm_l";
} else if (muscle_name.compare("glut_max2_r") == 0) {
muscle_name = "gm_r";
} else if (muscle_name.compare("glut_max2_l") == 0) {
muscle_name = "gm_l";
} else if (muscle_name.compare("glut_max3_r") == 0) {
muscle_name = "gm_r";
} else if (muscle_name.compare("glut_max3_l") == 0) {
muscle_name = "gm_l";
}
const OpenSim::Array<int> indices = _act.getColumnIndicesForIdentifier(muscle_name);
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;
}
/* Initialize the muscle actuation data array for all muscles, at a given
* timestep */
int LocomotorPrimitivesController::getAndInitActuationData(const double t) const
{
/* 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);
return act_set_siz;
}
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;
const int act_set_siz = getAndInitActuationData(t);
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;
/* 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;
mact *= SCALE_MACT[BIFEMLH];
} else if (muscle_name.compare("bifemlh_l") == 0) {
mact = getMuscleActivation("hs_l");
mact /= 4.0;
mact *= SCALE_MACT[BIFEMLH];
} else if (muscle_name.compare("bifemsh_r") == 0) {
mact = getMuscleActivation("hs_r");
mact /= 4.0;
mact *= SCALE_MACT[BIFEMSH];
} else if (muscle_name.compare("bifemsh_l") == 0) {
mact = getMuscleActivation("hs_l");
mact /= 4.0;
mact *= SCALE_MACT[BIFEMSH];
} else if (muscle_name.compare("semimem_r") == 0) {
mact = getMuscleActivation("hs_r");
mact /= 4.0;
mact *= SCALE_MACT[SEMIMEM];
} else if (muscle_name.compare("semimem_l") == 0) {
mact = getMuscleActivation("hs_l");
mact /= 4.0;
mact *= SCALE_MACT[SEMIMEM];
} else if (muscle_name.compare("semiten_r") == 0) {
mact = getMuscleActivation("hs_r");
mact /= 4.0;
mact *= SCALE_MACT[SEMITEN];
} else if (muscle_name.compare("semiten_l") == 0) {
mact = getMuscleActivation("hs_l");
mact /= 4.0;
mact *= SCALE_MACT[SEMITEN];
} else if (muscle_name.compare("glut_max1_r") == 0) {
mact = getMuscleActivation("gm_r");
mact /= 3.0;
mact *= SCALE_MACT[GLUT_MAX1];
} else if (muscle_name.compare("glut_max1_l") == 0) {
mact = getMuscleActivation("gm_l");
mact /= 3.0;
mact *= SCALE_MACT[GLUT_MAX1];
} else if (muscle_name.compare("glut_max2_r") == 0) {
mact = getMuscleActivation("gm_r");
mact /= 3.0;
mact *= SCALE_MACT[GLUT_MAX2];
} else if (muscle_name.compare("glut_max2_l") == 0) {
mact = getMuscleActivation("gm_l");
mact /= 3.0;
mact *= SCALE_MACT[GLUT_MAX2];
} else if (muscle_name.compare("glut_max3_r") == 0) {
mact = getMuscleActivation("gm_r");
mact /= 3.0;
mact *= SCALE_MACT[GLUT_MAX3];
} else if (muscle_name.compare("glut_max3_l") == 0) {
mact = getMuscleActivation("gm_l");
mact /= 3.0;
mact *= SCALE_MACT[GLUT_MAX3];
} else {
/* Otherwise just get the actuaction data for
* the plain name, it must be there! */
mact = getMuscleActivation(muscle_name);
/* Allow to scale some muscles afterwards */
if (muscle_name.compare("tfl_l") == 0)
mact *= SCALE_MACT[TFL];
if (muscle_name.compare("tfl_r") == 0)
mact *= SCALE_MACT[TFL];
}
if (mact > 1.0) {
logger.warn("Actuation data for '%s' larger than 1.0, truncating.\n", muscle_name.c_str());
mact = 1.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++;
}
|
