1function fitness = walkcostfunction_new(simt, simx, simrec, nq, simcons)
 2%WALKCOSTFUNCTION Calculates the fittness of a simulation
 3%#codegen
 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%% muscle activation
 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%% average joint torque
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%% maximal joint torque
19fitness.maxtor = 1/sum(max(abs(reflex),[],2))/6;
20
21%% distance traveled
22fitness.d = Tm_end(1) - Tm_start(1);
23
24%% average speed
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%% step length deviation
30steps = []; % finds the steps based on contacts of the foot
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