81 lines
3.0 KiB
Matlab
81 lines
3.0 KiB
Matlab
function dist = checkingLine(GapValue, L1, L2, n, j, points)
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%function dist = checkingLine(GapValue, L1, L2, n, j, points)
|
|
%
|
|
% Task: Computes the distance between two points in the q1 q2 joint space
|
|
% excepts when they can not be linked because it would collide a known obstacle in the X Y cartesian space
|
|
% The 'colliding' (or not) is verified by sampling the line in q1 q2 joint space and computing the X Y corresponding parameters
|
|
% for each of the sampled points on the line, as we know the obstacles position in the X Y cartesian space only
|
|
%
|
|
% Inputs:
|
|
%-GapValue: the distance between the q1 values of each sampled points along the line
|
|
%-L1: length of the first link of the 2d planar 2 link robot arm
|
|
%-L2: length of the second link of the 2d planar 2 link robot arm
|
|
%-n: index of the first given point in the 'points' matrix
|
|
%-j: index of the second given point in the 'points' matrix
|
|
%-points: a matrix saving the q1 q2 and x y coordinates of each point,
|
|
% the 1st column describing the 1st (start) point
|
|
%
|
|
% Outputs:
|
|
% -nbNodes: number of nodes of the graph excluding the starting and goal points
|
|
% -obstacle: a matrix containing the distance between connected nodes
|
|
% (NaN refers to not connected nodes)
|
|
% The matrix has a size of (nbNodes+2)x(nbNodes+2)
|
|
%-points: a matrix saving the q1 q2 and x y coordinates of each point,
|
|
% the 1st column describing the 1st (start) point
|
|
%
|
|
%
|
|
%
|
|
%
|
|
% Thomas OLIVE (thomas.olive@ecam.fr)
|
|
% 18/12/2021
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
q1 = points(1,j);
|
|
q2 = points(2,j);
|
|
q1_ = points(1,n);
|
|
q2_ = points(2,n);
|
|
|
|
|
|
dist = 0; % 0 value, if not modified, will represent that we didn't note about an invalid link between the n and j points
|
|
if n==j
|
|
dist = NaN;
|
|
endif
|
|
if q1 != q1_
|
|
|
|
%defining Q2 as a function of Q1
|
|
A = (q2_- q2)/(q1_ - q1);
|
|
B = q2 - A * q1;
|
|
Q2 = @(Q1) A*Q1+B;
|
|
|
|
%getting the sampling direction
|
|
if q1 > q1_
|
|
gap = -GapValue;
|
|
else
|
|
gap = GapValue;
|
|
endif
|
|
|
|
|
|
%getting the sample points
|
|
for g=q1:gap:q1_
|
|
Q1test = g;
|
|
Q2test = Q2(g);
|
|
|
|
%Is each sampled point from the line colliding with an obstacle in X Y cartesian space
|
|
|
|
%[Xtest, Ytest]=MyFK(L1,L2,Q1test,Q2test); remove the % on that line and add it on the next ones for faster computation
|
|
jTee=dh2ForwardKinematics([Q1test; Q2test], [0; 0], [L1; L2], [0; 0], 1);
|
|
Xtest = jTee(1,4);
|
|
Ytest = jTee(2,4);
|
|
% if a sampled point is colliding and we haven't already noted it, we do
|
|
if dist !=NaN && (Ytest >= L1 || Ytest <= -L1 || (Xtest>=-L2 && Xtest<=L2 && Ytest>=-L2 && Ytest<=L2)) %verifie les obstacles
|
|
dist = NaN;
|
|
endif
|
|
|
|
endfor
|
|
if dist ==0 && n!=j % if we didn't note nothing about the link between n and j, we compute the distance
|
|
dist = sqrt( (points(1,j)-points(1,n))^2 + (points(2,j)-points(2,n))^2);
|
|
endif
|
|
endif
|
|
|
|
endfunction |