copied the previous work directory used for this tutorial
This commit is contained in:
parent
48562724d7
commit
69e13d6650
|
|
@ -0,0 +1,72 @@
|
|||
## Author: adril <adril@LAPTOP-EJ1AIJHT>
|
||||
## Created: 2022-12-06
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
%function buildPRM (rangeQ1Q2, nbPoints, L1, L2, MapFilename)
|
||||
%
|
||||
% Task:
|
||||
%
|
||||
% Inputs:
|
||||
% - rangeQ1Q2 : range of values (in degrees) acceptable for joints Q1 and Q2
|
||||
% - nbPoints : number of points required
|
||||
% - L1, L2 : lengths of the links (in m)
|
||||
% - MapFilename : the name of the file to be saved for the map
|
||||
%
|
||||
% Outputs:
|
||||
% - None
|
||||
%
|
||||
% Adrien Lasserre (adrien.lasserre@ecam.fr) & Gwenn Durpoix-Espinasson (g.durpoix-espinasson@ecam.fr)
|
||||
% 06/12/2022
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
function buildPRM (rangeQ1Q2, nbPoints, L1, L2, MapFilename)
|
||||
Points=zeros(1, nbPoints);
|
||||
MatrixOfLinks=zeros(nbPoints, nbPoints);
|
||||
alpha=[0;0];
|
||||
d=[0;0];
|
||||
a=[L1;L2];
|
||||
jointNumber=[1;2];
|
||||
|
||||
for i=1:nbPoints
|
||||
|
||||
[q1;q2]=[rand()*(rangeQ1Q2(1,2)-rangeQ1Q2(1,1))+rangeQ1Q2(1,1);rand()*(rangeQ1Q2(2,2)-rangeQ1Q2(2,1))+rangeQ1Q2(2,1)];
|
||||
|
||||
theta=[q1;q2];
|
||||
OutOfRange=0;
|
||||
|
||||
bTee=dh2ForwardKinematics(theta, d, a, alpha, jointNumber);
|
||||
jTee=bTee(1:2, 4);
|
||||
|
||||
if (jTee(2,1)>=L1)
|
||||
OutOfRange=1;
|
||||
else if (jTee(2,1)<=-L1)
|
||||
OutOfRange=1;
|
||||
else if (abs(jTee(1,1)) <= L2 && abs(jTee(2,1)) <=L2)
|
||||
OutOfRange=1;
|
||||
endif
|
||||
|
||||
if (OutOfRange==0)
|
||||
Points(i)=jTee;
|
||||
MatrixOfLinks(i, i)=1;
|
||||
for j=1:i
|
||||
intersect=0;
|
||||
if
|
||||
|
||||
intersect=1;
|
||||
|
||||
else
|
||||
|
||||
MatrixOfLinks(i, j)=1;
|
||||
MatrixOfLinks(j,i)=1;
|
||||
|
||||
endif
|
||||
|
||||
|
||||
|
||||
endfor
|
||||
endif
|
||||
|
||||
endfor
|
||||
|
||||
|
||||
endfunction
|
||||
|
|
@ -0,0 +1,62 @@
|
|||
function rotationMatrix = create3DRotationMatrix(roll, pitch , yaw, order)
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% function rotationMatrix = create3DRotationMatrix(roll, pitch , yaw)
|
||||
% Task: Compute the 3D rotation matrix from the values of roll, pitch, yaw angles
|
||||
%
|
||||
% Inputs:
|
||||
% - roll: the value of the roll angle in degrees
|
||||
% - pitch: the value of the pitch angle in degrees
|
||||
% - yaw: the value of the yaw angle in degrees
|
||||
% - order: if equal to 1, ZYX; if equal to 0, XYZ
|
||||
%
|
||||
% Output:
|
||||
% - rotMatrix: the rotation matrix corresponding to the roll, pitch, yaw angles
|
||||
%
|
||||
%
|
||||
% author: Guillaume Gibert, guillaume.gibert@ecam.fr
|
||||
% date: 25/01/2021
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
% convert the input angles from degrees to radians
|
||||
rollAngleInRadians = roll / 180.0 * pi;
|
||||
pitchAngleInRadians = pitch / 180.0 * pi;
|
||||
yawAngleInRadians = yaw / 180.0 * pi;
|
||||
|
||||
% ZYX or XYZ direction
|
||||
switch order
|
||||
case 0
|
||||
thetaX = yawAngleInRadians;
|
||||
thetaY = pitchAngleInRadians;
|
||||
thetaZ = rollAngleInRadians;
|
||||
case 1
|
||||
thetaX = rollAngleInRadians;
|
||||
thetaY = pitchAngleInRadians;
|
||||
thetaZ = yawAngleInRadians;
|
||||
otherwise
|
||||
disp('[ERROR](create3DRotationMatrix)-> order value is neither 0 or 1!')
|
||||
end
|
||||
|
||||
Rz = [cos(thetaZ) -sin(thetaZ) 0;
|
||||
sin(thetaZ) cos(thetaZ) 0;
|
||||
0 0 1];
|
||||
|
||||
Ry = [cos(thetaY) 0 sin(thetaY);
|
||||
0 1 0;
|
||||
-sin(thetaY) 0 cos(thetaY)];
|
||||
|
||||
Rx = [1 0 0;
|
||||
0 cos(thetaX) -sin(thetaX);
|
||||
0 sin(thetaX) cos(thetaX)];
|
||||
|
||||
|
||||
% ZYX or XYZ direction
|
||||
switch order
|
||||
case 0
|
||||
rotationMatrix = Rx * Ry * Rz
|
||||
case 1
|
||||
rotationMatrix = Rz * Ry * Rx;
|
||||
otherwise
|
||||
disp('[ERROR](create3DRotationMatrix)-> order value is neither 0 or 1!')
|
||||
end
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,39 @@
|
|||
function transformationMatrix = create3DTransformationMatrix(roll, pitch, yaw, order, tX, tY, tZ)
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% function transformationMatrix = create3DTransformationMatrix(roll, pitch, yaw, order, tX, tY, tZ)
|
||||
% Task: Create the 3D transformation matrix corresponding to roll, ptich, yaw angles and a 3D translation
|
||||
%
|
||||
% Inputs:
|
||||
% - roll: the value of the roll angle in degrees
|
||||
% - pitch: the value of the pitch angle in degrees
|
||||
% - yaw: the value of the yaw angle in degrees
|
||||
% - order: the order of rotation if 1 ZYX, if 0 XYZ
|
||||
% - tX = the value of the translation along x in mm
|
||||
% - tY = the value of the translation along y in mm
|
||||
% - tZ = the value of the translation along z in mm
|
||||
%
|
||||
% Output:
|
||||
% - transformationMatrix: the transformation matrix corresponding to roll, ptich, yaw angles and a 3D translation
|
||||
%
|
||||
%
|
||||
% author: Guillaume Gibert, guillaume.gibert@ecam.fr
|
||||
% date: 25/01/2021
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
% determine the rotation matrix (3 x 3)
|
||||
rotationMatrix = create3DRotationMatrix(roll, pitch, yaw, order);
|
||||
|
||||
% create the translation part (3 x 1)
|
||||
translationMatrix = [tX; tY; tZ];
|
||||
|
||||
% create the homogeneous coordinate part
|
||||
homogeneousCoord = [0 0 0 1];
|
||||
|
||||
% create the transformation matrix which shape is
|
||||
% ( R | t )
|
||||
% --- --
|
||||
% ( 0 | 1)
|
||||
% with R: the rotation matrix (3x3)
|
||||
% and t: the translation matrix (3x1)
|
||||
transformationMatrix = [ rotationMatrix translationMatrix; homogeneousCoord];
|
||||
|
||||
|
|
@ -0,0 +1,34 @@
|
|||
function [nbNodes, visibilityGraph] = createVisibilityGraph(connectionMatrix, points2D)
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
%function [nbNodes, visibilityGraph] = createVisibilityGraph(connectionMatrix, points2D)
|
||||
%
|
||||
% Task: Create a visibility graph from a connection matrix and a set of 2D points
|
||||
%
|
||||
% Inputs:
|
||||
% -connectionMatrix: matrix of connection if cell is equal to 1 there is an edge between the corresponding points, cell is 0 otherwise
|
||||
% -points2D: coordinates of the vertices of the graph
|
||||
%
|
||||
% Outputs:
|
||||
% -nbNodes: the number of nodes of this graph
|
||||
% -visibilityGraph: 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)
|
||||
%
|
||||
% Guillaume Gibert (guillaume.gibert@ecam.fr)
|
||||
% 19/03/2021
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
nbNodes = size(points2D,1)-2;
|
||||
visibilityGraph = NaN(nbNodes+2, nbNodes+2);
|
||||
|
||||
for l_row=1:size(connectionMatrix,1)
|
||||
for l_col=1:size(connectionMatrix,2)
|
||||
if (connectionMatrix(l_row, l_col) == 1)
|
||||
% computes the distance between the 2 points
|
||||
distance = sqrt( (points2D(l_row,1)-points2D(l_col,1))^2 + (points2D(l_row,2)-points2D(l_col,2))^2);
|
||||
visibilityGraph(l_row, l_col) =distance;
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,46 @@
|
|||
function jTee = dh2ForwardKinematics(theta, d, a, alpha, jointNumber)
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% function wTee = dh2ForwardKinematics(theta, d, a, alpha, jointNumber)
|
||||
% Task: Determine the 3D transformation matrix corresponding to a set of Denavit-Hartenberg parameters
|
||||
%
|
||||
% Inputs:
|
||||
% - theta: an array of theta parameters (rotation around z in degrees)
|
||||
% - d: an array of d parameters (translation along z in mm)
|
||||
% - a: an array of a parameters (translation along x in mm)
|
||||
% - alpha: an array of alpha parameters (rotation around x in degrees)
|
||||
% - jointNumber: joint number you want to start with (>=1 && <=size(theta,1))
|
||||
%
|
||||
% Output:
|
||||
% -jTee: the transformation matrix from the {World} reference frame to the {end-effector} reference frame
|
||||
%
|
||||
%
|
||||
% author: Guillaume Gibert, guillaume.gibert@ecam.fr
|
||||
% date: 29/01/2021
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
% checks if the arrays have the same size
|
||||
if (size(theta, 1) != size(d,1) || size(theta,1) != size(a, 1) || size(theta,1) != size(alpha, 1))
|
||||
disp('[ERROR](dh2ForwardKinematics)-> sizes of input arrays do not match!')
|
||||
return;
|
||||
end
|
||||
|
||||
% creates the output matrix as an identity one
|
||||
jTee = eye(4);
|
||||
|
||||
% checks if jointNumber is in the good range [1 size(theta,1)]
|
||||
if (jointNumber < 1 || jointNumber > size(theta, 1))
|
||||
disp('[ERROR](dh2ForwardKinematics)-> jointNumber is out of range!')
|
||||
return;
|
||||
end
|
||||
|
||||
% loops over all the joints and create the transformation matrix as follow:
|
||||
% for joint i: Trot(theta(i), z) Ttrans(d(i), z) Ttrans (a(i), x) Trot(alpha(i), x)
|
||||
for l_joint=jointNumber:size(theta, 1)
|
||||
% determine the transformation matrices for theta, d, a and alpha values of each joint
|
||||
thetaTransformMatrix = create3DTransformationMatrix(0, 0, theta(l_joint), 1, 0, 0, 0); % Rz
|
||||
dTransformMatrix = create3DTransformationMatrix(0, 0, 0, 1, 0, 0, d(l_joint)); % Tz
|
||||
aTransformMatrix = create3DTransformationMatrix(0, 0, 0, 1, a(l_joint), 0, 0); % Tx
|
||||
alphaTransformMatrix = create3DTransformationMatrix(alpha(l_joint), 0, 0, 1, 0, 0, 0); % Rx
|
||||
|
||||
jTee = jTee * thetaTransformMatrix * dTransformMatrix * aTransformMatrix *alphaTransformMatrix;
|
||||
end
|
||||
|
|
@ -0,0 +1,70 @@
|
|||
function [distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNodes, visibilityGraph)
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
%function [distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNodes, visibilityGraph)
|
||||
%
|
||||
% Task: Perform the Dijkstra algorithm on a given visibility graph
|
||||
%
|
||||
% Inputs:
|
||||
% -nbNodes: number of nodes of the graph excluding the starting and goal points
|
||||
% -visibilityGraph: 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)
|
||||
%
|
||||
% Outputs:
|
||||
% - distanceToNode: distance between the current node and its parent
|
||||
% - parentOfNode: index of the parent node for each node
|
||||
% - nodeTrajectory: best trajectory
|
||||
%
|
||||
% Guillaume Gibert (guillaume.gibert@ecam.fr)
|
||||
% 17/03/2021
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
constantLargeDitance=10000;
|
||||
|
||||
visitedNodes = zeros(1, nbNodes+2);
|
||||
distanceToNode = constantLargeDitance*ones(1, nbNodes+2);
|
||||
distanceToNode(1) = 0;
|
||||
parentOfNode = zeros(1, nbNodes+2);
|
||||
|
||||
fprintf('##Starting Dijkstra''s algorithm...\n')
|
||||
|
||||
while (sum(visitedNodes(:)==0))
|
||||
thresholdDistance = constantLargeDitance+1;
|
||||
for l_node=1:nbNodes+2
|
||||
%l_node
|
||||
if (visitedNodes(l_node)==0 && distanceToNode(l_node) < thresholdDistance)
|
||||
minIndex = l_node;
|
||||
thresholdDistance = distanceToNode(l_node);
|
||||
end
|
||||
end
|
||||
|
||||
fprintf('-->Visiting N%d\n', minIndex-1)
|
||||
|
||||
visitedNodes(minIndex) = 1;
|
||||
for l_node=1:nbNodes+2
|
||||
%l_node
|
||||
if (l_node~=minIndex && ~isnan(visibilityGraph(minIndex, l_node)))
|
||||
distance = distanceToNode(minIndex) + visibilityGraph(minIndex,l_node);
|
||||
if (distance < distanceToNode(l_node))
|
||||
distanceToNode(l_node) = distance;
|
||||
parentOfNode(l_node) = minIndex;
|
||||
end
|
||||
end
|
||||
end
|
||||
end
|
||||
fprintf('##Dijkstra''s algorithm is done!\n')
|
||||
fprintf('##Results\n')
|
||||
fprintf('Minimal distance to target: %d\n', distanceToNode(nbNodes+2))
|
||||
nodeIndex = nbNodes+2;
|
||||
nodeTrajectory = [];
|
||||
while(nodeIndex~=1)
|
||||
nodeIndex = parentOfNode(nodeIndex);
|
||||
nodeTrajectory = [nodeTrajectory nodeIndex];
|
||||
end
|
||||
fprintf('S-->');
|
||||
for l_node=2:length(nodeTrajectory)
|
||||
fprintf('N%d-->', nodeTrajectory(length(nodeTrajectory)-(l_node-1))-1);
|
||||
end
|
||||
fprintf('G\n');
|
||||
fprintf('########\n');
|
||||
|
||||
|
|
@ -0,0 +1,30 @@
|
|||
function h = drawCircle(x,y,r)
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% function h = drawCircle(x,y,r)
|
||||
% Task: Draw a circle providing its center and radius
|
||||
%
|
||||
% Inputs:
|
||||
% - x: the x-coordinate of the circle center (in m)
|
||||
% - y: the y-coordinate of the circle center (in m)
|
||||
% - r: the radius of the circle center (in m)
|
||||
%
|
||||
% Outputs:
|
||||
% - h: a reference to the plot figure
|
||||
%
|
||||
%
|
||||
% author: Guillaume Gibert, guillaume.gibert@ecam.fr
|
||||
% date: 14/09/2021
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
% holds the previous drawing
|
||||
hold on;
|
||||
|
||||
% generates samples in the range [0, 2pi]
|
||||
th = 0:pi/50:2*pi;
|
||||
|
||||
% computes (x,y) samples along the circle perimeter
|
||||
xunit = r * cos(th) + x;
|
||||
yunit = r * sin(th) + y;
|
||||
|
||||
% plots the samples
|
||||
h = plot(xunit, yunit, 'r');
|
||||
|
|
@ -0,0 +1,22 @@
|
|||
function invRotationMatrix = inverse3DRotationMatrix(rotationMatrix)
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% function invRotationMatrix = inverse3DRotationMatrix(rotationMatrix)
|
||||
% Task: Inverse a 3D rotation matrix
|
||||
%
|
||||
% Inputs:
|
||||
% - rotationMatrix: the rotation matrix to inverse
|
||||
%
|
||||
% Output:
|
||||
% -invRotationMatrix: the inverse of the rotation matrix
|
||||
%
|
||||
%
|
||||
% author: Guillaume Gibert, guillaume.gibert@ecam.fr
|
||||
% date: 25/01/2021
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
% checks if the input rotation matrix has the right size
|
||||
if (size(rotationMatrix, 1) != 3 || size(rotationMatrix, 2) != 3)
|
||||
fprintf('[ERROR] (inverseRotationMatrix) -> the size of the input rotation matrix is not 3x3!\n');
|
||||
end
|
||||
|
||||
invRotationMatrix = rotationMatrix';
|
||||
|
|
@ -0,0 +1,42 @@
|
|||
function invTransformationMatrix = inverse3DTransformationMatrix(transformMatrix)
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% function invTransformMatrix = inverse3DTransformationMatrix(transformMatrix)
|
||||
% Task: Inverse a 3D transformation matrix
|
||||
%
|
||||
% Inputs:
|
||||
% - transformMatrix: the transform matrix to inverse
|
||||
%
|
||||
% Output:
|
||||
% - invTransformationMatrix: the inverse of the transformation matrix
|
||||
%
|
||||
%
|
||||
% author: Guillaume Gibert, guillaume.gibert@ecam.fr
|
||||
% date: 25/01/2021
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
% checks if the input transform matrix has the right size
|
||||
if (size(transformMatrix, 1) != 4 || size(transformMatrix, 2) != 4)
|
||||
fprintf('[ERROR] (inverseTransformationMatrix) -> the size of the input transform matrix is not 4x4!\n');
|
||||
end
|
||||
|
||||
% retrieves the rotation matrix
|
||||
rotationMatrix = transformMatrix(1:3, 1:3);
|
||||
|
||||
%retrieves the translation matrix
|
||||
translationMatrix = transformMatrix(1:3, 4);
|
||||
|
||||
% inverses the rotation matrix
|
||||
invRotationMatrix = inverse3DRotationMatrix(rotationMatrix);
|
||||
|
||||
% inverses the translation matrix
|
||||
invTranslationMatrix = -invRotationMatrix * translationMatrix;
|
||||
|
||||
% create the inverse of the transform matrix
|
||||
% ( R^-1 | -R^-1t )
|
||||
% --- ----- ----- --
|
||||
% ( 0 | 1)
|
||||
% with R: the rotation matrix (3x3)
|
||||
% and t: the translation matrix (3x1)
|
||||
invTransformationMatrix = [invRotationMatrix invTranslationMatrix; 0 0 0 1];
|
||||
|
||||
|
||||
Loading…
Reference in New Issue