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
|
#include <OpenSim/OpenSim.h>
#include "LocomotorPrimitivesController.h"
#include "MuscleEMGProfile.h"
#define VERBOSE 1
static const double TIME_DAMP = 0.1;
#if 0
int LocomotorPrimitivesController::loadCsvData(const std::string &muscleName, const std::string &file)
{
std::cout << ">> loading CSV data from " << file << std::endl;
std::ifstream data(file);
std::string line;
std::cout << data.tellg() << std::endl;
if (!data.is_open()) {
std::cerr << "Error loading CSV data from " << file << std::endl;
return -1;
}
MuscleEMGProfile p(muscleName, 0);
while (std::getline(data, line)) {
std::stringstream s(line);
std::string cell;
int i = 0;
double x;
while (std::getline(s, cell, ',')) {
if (!isdigit(cell[0]))
continue;
std::cout << "read cell(" << (i == 0 ? "x" : "y") << ") -> " << cell << std::endl;
double val = (double) atof(cell.c_str());
if (i == 1)
p.addData(x, val);
else
x = val;
i = !i;
}
}
data.close();
//_act.push_back(p);
std::cout << ">>> loaded EMG profile for " << p.getName() << ", " << p.getIdx() << "/" << p.getCapacity() << std::endl;
return 0;
}
#endif
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();
for (int i = 0; i < act_set_siz; i++) {
const OpenSim::Muscle *m = dynamic_cast<const OpenSim::Muscle *>(&getActuatorSet().get(i));
const OpenSim::Array<int> indices = _act.getColumnIndicesForIdentifier(m->getName());
if (indices.getSize() != 0) {
double muscle_act = 0.0;
_act.getDataAtTime(t, indices.get(0), &muscle_act);
std::cout << indices.get(0) << " actuation data for '" << m->getName() << "' found: " << muscle_act << std::endl;
}
//std::cout << " " << p.getName() << "(" << p.getIdx() << "/" << p.getCapacity() << ")" << std::endl;
}
#if 0
//const OpenSim::Muscle *rectfem = dynamic_cast<const OpenSim::Muscle *>(&getActuatorSet().get("bifemlh_r"));
const OpenSim::Muscle *rectfem = dynamic_cast<const OpenSim::Muscle *>(&getActuatorSet().get("rect_fem_r"));
const OpenSim::Muscle *ercspn_r = dynamic_cast<const OpenSim::Muscle *>(&getActuatorSet().get("ercspn_r"));
const OpenSim::Muscle *ercspn_l = dynamic_cast<const OpenSim::Muscle *>(&getActuatorSet().get("ercspn_l"));
const OpenSim::Muscle *extobl_r = dynamic_cast<const OpenSim::Muscle *>(&getActuatorSet().get("extobl_r"));
const OpenSim::Muscle *extobl_l = dynamic_cast<const OpenSim::Muscle *>(&getActuatorSet().get("extobl_l"));
double v = rectfem->getLengtheningSpeed(s);
double act = v * _alpha;
if (act < 0.0)
act = 0.0;
if (act > 1.0)
act = 1.0;
if (t - last_twitch < TIME_DAMP)
act = 0.0;
if (act > 0.0)
last_twitch = t;
//if (VERBOSE && act > 0.0)
// std::cout << "(" << std::fixed << t << ") " << "v=" << std::fixed << v << ", act=" << std::fixed << act << std::endl;
SimTK::Vector ctrl_rectfem(1, act);
SimTK::Vector ctrl_extobl(1, 0.79);
SimTK::Vector ctrl_ercsp(1, 0.375);
rectfem->addInControls(ctrl_rectfem, controls);
ercspn_r->addInControls(ctrl_ercsp, controls);
ercspn_l->addInControls(ctrl_ercsp, controls);
extobl_r->addInControls(ctrl_extobl, controls);
extobl_l->addInControls(ctrl_extobl, controls);
#endif
}
|
