diff --git a/buildPRM.m b/buildPRM.m new file mode 100644 index 0000000..55d2fdc --- /dev/null +++ b/buildPRM.m @@ -0,0 +1,72 @@ +## Author: adril +## 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 diff --git a/create3DRotationMatrix.m b/create3DRotationMatrix.m new file mode 100644 index 0000000..0695497 --- /dev/null +++ b/create3DRotationMatrix.m @@ -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 + + diff --git a/create3DTransformationMatrix.m b/create3DTransformationMatrix.m new file mode 100644 index 0000000..ffad708 --- /dev/null +++ b/create3DTransformationMatrix.m @@ -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]; + diff --git a/createVisibilityGraph.m b/createVisibilityGraph.m new file mode 100644 index 0000000..2d71c50 --- /dev/null +++ b/createVisibilityGraph.m @@ -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 + + diff --git a/dh2ForwardKinematics.m b/dh2ForwardKinematics.m new file mode 100644 index 0000000..195a16d --- /dev/null +++ b/dh2ForwardKinematics.m @@ -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 diff --git a/dijkstra.m b/dijkstra.m new file mode 100644 index 0000000..03a919e --- /dev/null +++ b/dijkstra.m @@ -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'); + diff --git a/drawCircle.m b/drawCircle.m new file mode 100644 index 0000000..1b45209 --- /dev/null +++ b/drawCircle.m @@ -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'); diff --git a/inverse3DRotationMatrix.m b/inverse3DRotationMatrix.m new file mode 100644 index 0000000..b996807 --- /dev/null +++ b/inverse3DRotationMatrix.m @@ -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'; \ No newline at end of file diff --git a/inverse3DTransformationMatrix.m b/inverse3DTransformationMatrix.m new file mode 100644 index 0000000..5351ec6 --- /dev/null +++ b/inverse3DTransformationMatrix.m @@ -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]; + +