From ff1367f30db89b2f4ac20a8dcde06cb58e6175b8 Mon Sep 17 00:00:00 2001 From: Gabri6 Date: Wed, 22 Nov 2023 08:27:47 +0100 Subject: [PATCH] initial commit --- create3DRotationMatrix.m | 62 ++++++++++++++++++++++++++++ create3DTransformationMatrix.m | 39 ++++++++++++++++++ createVisibilityGraph.m | 34 ++++++++++++++++ dh2ForwardKinematics.m | 46 +++++++++++++++++++++ dijkstra.m | 71 +++++++++++++++++++++++++++++++++ drawCircle.m | 30 ++++++++++++++ inverse3DRotationMatrix.m | 22 ++++++++++ inverse3DTransformationMatrix.m | 42 +++++++++++++++++++ 8 files changed, 346 insertions(+) create mode 100644 create3DRotationMatrix.m create mode 100644 create3DTransformationMatrix.m create mode 100644 createVisibilityGraph.m create mode 100644 dh2ForwardKinematics.m create mode 100644 dijkstra.m create mode 100644 drawCircle.m create mode 100644 inverse3DRotationMatrix.m create mode 100644 inverse3DTransformationMatrix.m 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..cbfc056 --- /dev/null +++ b/dijkstra.m @@ -0,0 +1,71 @@ +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) +% The first row/col corresponds to the Starting point, the last row/col to the Goal point. +% +% 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]; + +