get 3D angles of vectors

1 view (last 30 days)
Carlos Fambuena
Carlos Fambuena on 8 Oct 2019
Commented: darova on 8 Oct 2019
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
  3 Comments
darova
darova on 8 Oct 2019
Can you make a simple drawing where you show what angles do you need?
121Untitled.png
Carlos Fambuena
Carlos Fambuena on 8 Oct 2019
Hi Dorva, the angles that I want to calculate are those in figure 1.
angles.png

Sign in to comment.

Accepted Answer

darova
darova on 8 Oct 2019
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);
11Capture.PNG
  2 Comments
Carlos Fambuena
Carlos Fambuena on 8 Oct 2019
Thank you very much! This function works very well.
darova
darova on 8 Oct 2019
You can accept the answer so i will be paid

Sign in to comment.

More Answers (0)

Categories

Find more on Sparse Matrices in Help Center and File Exchange

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!