Teacher functions
This commit is contained in:
parent
950ce34720
commit
eebbd604c1
|
|
@ -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,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,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