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
|
/*
* 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));
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;
}
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++;
}
|
