Tscfinal =  [cos(-pi/4) -sin(-pi/4) 0   1.5; 
          sin(-pi/4)  cos(-pi/4) 0  -1.5; 
                0           0     1  0.025; 
               0           0     0    1]; 
[result_traj,result_conf,result_Xerr] = Main_function(Tscfinal);