Code:

double reach(double p[], double *error, double *effort)
{
int i,j,k,n, nsteps, numder, numint;
double t, step, sumeffort, sumerror, ymin;
double q[NDOF], qdot[NDOF], F[NMUS], m[12], Lce[NMUS], Act[NMUS], Stim[NMUS];
double e1, e2, e3, e4, e5; /* calculated error values */
double cputime;
double W = 0.05; /* weight for cost function */
time_t clockstart;
double initact[NMUS] = {0,0,0,0,0,0}; /* initial muscle activation levels */
double return_val; /* cost function return value */
double initq[NDOF] = {0,0};
double initqdot[NDOF] = {0,0};
double initLce[NMUS];
int v, w;
clockstart = clock();
for(n = 0; n < NPAR; n++)
globp[n] = p[n];
copyPtoG(globp, Gp, Gd);
t = 0.0;
/* do the simulation, compute cost function, and write output files */
sumeffort = 0.0; /* initialize effort sum to zero */
sumerror = 0.0; /* initialize error sum to zero */
nsteps = 200; /* simulate for a total of 2.0 sec */
step = 0.010; /* time between samples */
/* simulate one step in the specified reaching task */
for(v = 0; v < NUMTASKS; v++){
for(w = 0; w < NDOF; w++){
initq[w] = initial[v][w];
single_target[w] = target[v][w];
initLce[0] = 99; /* CE starts just slack in each simulation */
initcond(&t, initq, initqdot, initact);
for (j=0; j<nsteps; j++) {
simulate(&t, step);
getq(q);
getqdot(qdot);
getmuscle(F, m);
printf("In reach(): calling user_stim(); j = %d, nsteps = %d.\n", j, nsteps);
user_stim(&t, q, qdot, Stim);
/* add this sample's results to the cost function */
e1 = q[0] - single_target[0];
e2 = q[1] - single_target[1];
e3 = q[2] - single_target[2];
e4 = q[3] - single_target[3];
e5 = q[4] - single_target[4];
sumerror = sumerror + (e1*e1) + (e2*e2) + (e3*e3) + (e4*e4) + (e5*e5);
for(k = 0; k < NMUS; k++)
sumeffort = sumeffort + (Stim[k]*Stim[k]);
} /* end for(j) loop */
} /* end for w loop */
} /* end for v loop */
/* compute cost function: RMS diff between simulation and data, normalized to SD */
*error = sqrt(sumerror/(nsteps*NUMTASKS)/NDOF)*(180/PI);
*effort = sqrt(sumeffort/(nsteps*NUMTASKS)/NMUS);
return_val = (*error + (W*(*effort)));
return sqrt(return_val);
}