%Cost function for reaching with arm_dyn
%
% [l, l_x, l_xx, l_u, l_uu, l_ux] = arm_cost(x, u, t, target)
%
% t = nan signals final time


function [l, l_x, l_xx, l_u, l_uu, l_ux] = arm2Cost(x, u, t, target)

wp = 1E+4;      % terminal position cost weight
wv = 1E+3;      % terminal velocity cost weight

l1_   = 0.3;    % arm segment lengths
l2_   = 0.33;

% compute cost
if isnan(t),                        % final cost
    l = wp*sum((x(1:2)-target).^2) + wv*sum(x(3:4).^2);

else                                % running cost
    l = sum(u.^2);
end


% compute derivatives of cost
if nargout>1,
    l_x = zeros(4,1);
    l_xx = zeros(4,4);
    l_u = 2*u;
    l_uu = 2*eye(2);
    l_ux = zeros(2,4);
    
    if isnan(t),                           % final cost
        l_x(1:2) = 2*wp*(x(1:2)-target);
        l_x(3:4) = 2*wv*x(3:4);
        l_xx = 2*diag([wp wp wv wv]);
    end
end