clear all;
close all;
addpath(genpath(pwd))

l1=0.148;l2=0.069;k=3;
B=k*[-l1     0       l1     0;
     0      -l1     0       l1;
     l2    l2    l2    l2]

B_inv=pinv(B)
[k,m] = size(B);
% m=4;
umin=ones(m,1)*(-20)*pi/180;
umax=ones(m,1)*20*pi/180;

    M=50;
    x=zeros(4,(M+1)^2);
    u=zeros(4,1);
    [X,Y,Z] = sphere(M);
    x1=zeros(4,(M+1)^2);
    u1=zeros(4,1);
    x2=zeros(4,(M+1)^2);
    u2=zeros(4,1);

    N=(M+1)^2;



for i=1:N% (N+1)^2  for  sphere %length(M_des(1:1000,1))%%length(X)
    v=(0.5*[X(i);Y(i);Z(i)]);
    [u2,~] = test_LP_lib(B,v, umin, umax);
    x2(:,i)=Constrain(u2,umin,umax);
end
U2=B*x2;



dt=0.01;
t=0:dt:dt*(N-1);

tt=1:1:(N-1);

    figure,
    plot3(U2(1,:),U2(2,:),U2(3,:),'g*');