working on optimizer

p's and q's set to iterized values, Fperturb applied and ground
%         reaction forces added, progress clock,simulation stores the
results
%         added totalenergy as cost function