From 749425e5ed53b17ff3158f9381762aec1e8f0e75 Mon Sep 17 00:00:00 2001 From: GIBERT Guillaume Date: Mon, 29 Nov 2021 22:28:48 +0100 Subject: [PATCH] + Add functions to manage 3D transformations --- create3DRotationMatrix.m | 62 +++++++++++++++++++++++++++++++++ create3DTransformationMatrix.m | 39 +++++++++++++++++++++ dh2ForwardKinematics.m | 46 ++++++++++++++++++++++++ inverse3DRotationMatrix.m | 22 ++++++++++++ inverse3DTransformationMatrix.m | 42 ++++++++++++++++++++++ 5 files changed, 211 insertions(+) create mode 100644 create3DRotationMatrix.m create mode 100644 create3DTransformationMatrix.m create mode 100644 dh2ForwardKinematics.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/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/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]; + +