S = sysHarOscOCP([]);
T = S.T;
N = 10;
s = 2;
h = T/N;
hs = h/s;

options = optimset('TolFun',  1e-12, ...
                   'TolX',    1e-12, ...
                   'TolCon',  1e-12, ...
                   'MaxIter', 1000, ...
                   'Display', 'off', ...
                   'Algorithm', 'sqp');
options.s = s;
options.BoundData = 'momentum';

t = 0:hs:T;
qig = S.q(t,S);
uig = S.u(t,S);
qig = reshape([qig zeros(1,s-1)],[S.dimQ s N+1]);
uig = reshape([uig zeros(1,s-1)],[S.dimQ s N+1]);

[q, p, u] = varInt(S,S.q0,S.p0,S.qT,S.pT,T,N,qig,uig,options);

t  = 0:h:T;
qe = S.q(t,S);
ue = S.u(t,S);
q  = squeeze(q(1,1,:))';
u  = squeeze(u(1,1,:))';
err_q = max(abs(qe-q))
err_u = max(abs(ue-u))
