Mdm,Mdm);

% 2. Define static penalty as distance from target setpoint
ell = cost.p; % pendulum length
Q = zeros(D1); Q([1 D0+1],[1 D0+1]) = [1 ell]'*[1 ell]; Q(D0+2,D0+2) = ell^2;

% 3. Trigonometric augmentation
if D1-D0 > 0
  % augment target
  target = [cost.target(:); gTrig(cost.target(:), 0*s, cost.angle)];
    
  % augment state
  i = 1:D0; k = D0+1:D1;
  [M(k) S(k,k) C mdm sdm Cdm mds sds Cds] = gTrig(M(i),S(i,i),cost.angle);
  
  % compute derivatives (for augmentation)
  X = reshape(1:D1*D1,[D1 D1