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.
get 3D angles of vectors
1 view (last 30 days)
Show older comments
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
Accepted Answer
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);
2 Comments
More Answers (0)
See Also
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!