bvp4c - how does residuum works? double inverted pendulum

I am working on double pendulum on a cart swingup and I am looking for parameters for trajectory of a cart that leads to swingup - pendulums going from down-down to up-up. In several article bvp4c is often reffered to be a good way to solve the problem. This is my code:

function solveBVP3
run ../parametry.m
global tf_su m1 m2 l1 l2 L1 b1 b2 I1 I2 g sol

%final time of swingup
tf_su=1.6;

% just testing values
options = bvpset('RelTol',1e-5,'NMax',1000);

%initial guess for trajectory and parameters
solinit=bvpinit(linspace(0,tf_su,100),@odeInit,[0 0 0 0]);

%solution
sol=bvp4c(@myODE, @odeBC, solinit,options);

%visualisation
sol.th0=[sol.x;sol.y(1,:)]';
sol.th1=[sol.x;sol.y(2,:)]';
sol.th2=[sol.x;sol.y(3,:)]';
disp(sol.parameters);
close all
plot(sol.x,sol.y(1,:),sol.x,sol.y(2,:),sol.x,sol.y(3,:));

end

function dydt = myODE(t,y,p)
%t is time, y is state at time t, p are parameters
global tf_su  m1 m2 l1 l2 L1 b1 b2 I1 I2 g

% y(1) ... cart
% y(2) ... arm1
% y(3) ... arm2
% y(4) ... dcart
% y(5) ... darm
% y(6) ... darm2

% 2nd derivative "u" of needed trajectory is acc of cart = system input
% and is set by parameters p
u=pi^2/tf_su^2*(p(2)+p(4))*cos(pi*t/tf_su);
for i = 2:5
    u=u-(i*pi/tf_su)^2*p(i-1)*cos(i*pi*t/tf_su);
end

% movement equations
dydt = [y(4) y(5) y(6) u 0 0];

A = I1 + l1^2*m1 + L1^2*m2;
B = l2*L1*m2*cos(y(2)-y(3));
C = (l1*m1+L1*m2)*g*sin(y(2)) - l2*L1*m2*sin(y(2)-y(3))*y(6)^2 - b1*y(5) - ...
    b2*(y(5)-y(6)) + (l1*m1+L1*m2)*cos(y(2))*u;
D = I2 + l2^2*m2;
E = l2*g*m2*sin(y(3)) + l2*L1*m2*sin(y(2)-y(3))*y(5)^2 + b2*(y(5)-y(6)) + ...
    l2*m2*cos(y(3))*u;

dydt(5) = (A*E-B*C) / (A*D-B^2);
dydt(6) = (C*D-B*E) / (A*D-B^2);
end

function res = odeBC(ya, yb, p)
% Says that at time 0, pendulums are down, and at time tf_su, they are
% zero. Velocities are zero both at start and at the end. No need to specify
% position and velocity of the cart at tf_su as the trajectory function
% grants them zero.
res = [ya(1)
    ya(2)+pi
    ya(3)+pi
    ya(4)
    ya(5)
    ya(6)
    yb(2)
    yb(3)
    yb(5)
    yb(6)
    ];
end

function v = odeInit(t)
%initial guess of trajectory
global tf_su
    v=[0;
        -pi+pi*t/tf_su;
        -pi+pi*t/tf_su;
        0;
        pi/tf_su;
        pi/tf_su;
        ]';
end

When I run the program, I either get solution with high residuum, like 30 instead of 1e-05 (and with totally nonphysical behavior), or I get an error:

Error using bvp4c (line 251)
Unable to solve the collocation equations -- a
singular Jacobian encountered.

Error in solveBVP_mini (line 15)
sol=bvp4c(@myODE, @odeBC, solinit,options);

I am pretty sure the equations of movement are correct. I think the problem might be my understanding of residuum - is it the boundary conditions related vector that is kept around zero when optimalizing the parameters?