clear -all
conRobot = [2 1 1; 3 2 1; 3 1 1;3 4 1; 2 4 1];
corRobot = [0; 0; 1; 0; 2; 2; 1; 1; ]; % xy xy xy
d = 3;
figure(1)
[R,K] = rigidityMatrixFnc(conRobot);
vref = 5;
B = [1;1; 0;0; 0;0; 0; 0;];
kp = 2;
ki = 10;

sInit = [corRobot; 0;0;0;0;0;];
tspan = linspace(1,10);
dydt = @(t, y) systm_robot(t, y,d, R,K, kp, ki, B, vref);

[t,y] = ode45(dydt, tspan, sInit);
figure(2)
plot(y'(1,:),y'(2,:),y'(3,:),y'(4,:),y'(5,:),y'(6,:),y'(7,:),y'(8,:))