clipboard.txt

mode = 1;

if mode == 1

    tree_point = [0.1, 0.0; 0.3, -0.05; -0.15, 0.08]';

    c_a = norm(xf-tree_point(:, 1)) * norm(xf-tree_point(:, 2)) * norm(xf-tree_point(:, 3));

    c_b = [];
    
elseif mode == 2
    
    tree_point = [d,        0.0, my_pi,        0.0;
              d - 0.05, 0.0, my_pi - 0.05, 0.0;
              d - 0.1, 0.08, my_pi + 0.03, -0.01 ]';
    
    mul = 1.0;
    for i = 1:size(tree_point, 2)
        mul = mul * norm(xf-tree_point(:, i));
    end
    
    c_a = mul;
    c_b = [];
else
    c_a = [];
    c_b = [];
end



mode = 1;

if mode == 1
    
    x = x(1, :);
    v = x(2, :);
    k = 0.5;
    m = 1.0;

    dz = [v; -k/m * x + 1/m * u];

elseif mode == 2
    
    m1 = 1.0;
    m2 = 0.1;
    g = 9.81;
    l = 1.0;
    d = 1.0;
    
    dz = [ x(2) ;
         ( l*m2*sin(x(3))*x(4)^2 + u(1) + m2*g*cos(x(3))*sin(x(3)) ) / ( m1 + m2*(1 - (cos(x(3)))^2) );
           x(4) ;
          -( l*m2*cos(x(3))*sin(x(3))*x(4)^2 + u(1)*cos(x(3)) + (m1+m2)*g*sin(x(3)) ) / ( l*m1 + l*m2*(1 - (cos(x(3)))^2) ) ];  
else
    dz = [];
end



mode = 1;

if mode == 1
    R = 1.0;

    % Want slopes near zero, so minimize slope squared:
    f = 1.0 + u.^2*R;
    
elseif mode == 2
    R = 10.0;
    
    f = 1.0 + u.^2*R;
    
else
    f = 1.0 + u.^2;
end