1function fitness = walkcostfunction_new(simt, simx, simrec, nq, simcons)
2
3
4
5[~, Tm_start] = Tmatrices(simx(:,1), zeros(nq, 1), simcons.p_in);
6[~, Tm_end] = Tmatrices(simx(:,end), zeros(nq, 1), simcons.p_in);
7
8
9sid1 = length([simx(:,1); nq]) + 1;
10activations = simx(sid1 + mdl.findcontrol('reflex').sid(1:14),:);
11fitness.act = 1 - sum(sum(activations))/numel(activations);
12
13
14reflex = simrec(mdl.findcontrol('reflex').recid(end-5:end),:);
15ligaments = simrec(mdl.findcontrol('ligaments').recid,:);
16fitness.tor = max(10, (80 - (sum(sum(abs(reflex + ligaments)))/numel(reflex))));
17
18
19fitness.maxtor = 1/sum(max(abs(reflex),[],2))/6;
20
21
22fitness.d = Tm_end(1) - Tm_start(1);
23
24
25v_des = 1.2;
26v_avg = fitness.d/simt(end);
27fitness.v = max(0.1, v_des * 2 - abs(v_avg - v_des));
28
29
30steps = [];
31for i = 2:length(simt)
32 if any(simrec(mdl.controllers{1}.recid([2 5]),i)) && ~any(simrec(mdl.controllers{1}.recid([2 5]),i-1))
33 steps = [steps i];
34 end
35end
36
37if length(steps > 10)
38 fitness.stepdev = max(0.1, 1-std(diff(steps/1000)));
39else
40 fitness.stepdev = 0.1;
41end
42
43end
44