get 3D angles of vectors

Hi,

I am trying to calculate the angles and link lengths to fit some manipulators to a 3D curve. I have done this so far, but apparently the calculation of the ngles is not accurate enough and I have a considerable position error when I reconstruct the manipulators using the angles and link lengths obtained from this function. Any idea of what could be going wrong or how to improve this? Thanks you beforehand.

function [ang,ind1,L] = fitManipulators(cit,pc,ns,rL)
%This function fits manipulators of three double joints each to a curve. That is,
%calculates the length of the links and the angles of the joints so the
%manipulators follow the curve.
%INPUTS:
    %cit: Matrix of 3D points N by 3 in the curve.
    %pc: Vector with the indexes where the segment start and end.
    %ns: Number of segments in which the curve is going to be splited. This
    %   number concides also with the number of manipulators.
    %rL: relative length of those segments w.r.t the total length of the 3D
    %    the segment they belong to.
%OUTPUTS:
    %ang: Angles of the joints of the manipulators
    %ind1: Indexes of the curve where there is a joint
    %L: ns by 3 matrix with the lengths of the links of each manipulator.
    
       ang = zeros(ns*6,1);
       angz_1 = 0;
       angx_1 = 0;
       ind1 = zeros(1,ns*3);
       L = zeros(ns,3);
       f = 1;
  
       for i =1:ns

           Ls = pc(i+1)-pc(i); %Length of the segment
           ind = pc(i);        %Index 
           k = 1;
           
           for j =1:2:6
              ind1(f) = ind+round(Ls*rL(k));  %Getting the next index
              vj = cit(ind,:);                %Getting the 3D coordinates
              vj1 = cit(ind1(f),:);
              vd = vj1 - vj;
              L(i,k) = norm(vd);

              angx = atan2(vd(3),norm([vd(1) vd(2)]));
              angz = atan2(vd(1),norm([vd(2) vd(3)]));
              angz = angz-angz_1;
              angx = angx-angx_1;
              ang(6*(i-1)+j:6*(i-1)+j+1) = [angz;angx];
              angz_1 = angz;
              angx_1 = angx;
              ind = ind1(f);
              k = k+1;
              f =f+1;
              
           end

       end
end

[Carlos Fambuena]

I forgot to add the image that shows how poor this fit is :*). The black line with pink and green points is the shape i am trying to impose in my manipulators. The blue and red links are the manipulator.

[darova]

Can you make a simple drawing where you show what angles do you need?

[Carlos Fambuena]

Hi Dorva, the angles that I want to calculate are those in figure 1.

Answer 1

[darova]

If cit is your 3D data

vj = cit(ind,:);                %Getting the 3D coordinates

use cart2sph

v = diff(cit);                % vectors in 3D
[az,el,~] = cart2sph(v(:,1),v(:,2),v(:,3));
    % angles between vectors
az_diff = diff(az);
el_diff = diff(el);

[Carlos Fambuena]

Thank you very much! This function works very well.

[darova]

You can accept the answer so i will be paid