files added
This commit is contained in:
parent
edda2e7903
commit
890cb746d1
|
|
@ -0,0 +1,54 @@
|
||||||
|
function retval = buildPRM (L1, L2, nbVP)
|
||||||
|
%%%%%%%%%%%%%%%%%%%
|
||||||
|
%function buildPRM(L1, L2, nbV)
|
||||||
|
%ex. buildPRM(2000, 1000, 10)
|
||||||
|
%
|
||||||
|
%Inputs:
|
||||||
|
% -L1 : length of link 1(in mm)
|
||||||
|
% -L2 : length of link 2(in mm)
|
||||||
|
% -nbVP : Number of Valid Points
|
||||||
|
%
|
||||||
|
%Outputs:
|
||||||
|
%
|
||||||
|
%Author : Thomas Périn, thomas.perin@ecam.fr
|
||||||
|
%Date/ 22/11/2023
|
||||||
|
%%%%%%%%%%%%%%%%%%%
|
||||||
|
|
||||||
|
q1q2_valid = [];
|
||||||
|
|
||||||
|
while (size(q1q2_valid) < nbVP)
|
||||||
|
% Sample randomly the joint Space
|
||||||
|
q1 = rand() * 360;
|
||||||
|
q2 = rand() * 360;
|
||||||
|
|
||||||
|
% Creats the DH table
|
||||||
|
theta = [q1, q2];
|
||||||
|
d = [0 0];
|
||||||
|
a = [L1 L2];
|
||||||
|
alpha = [0 0];
|
||||||
|
|
||||||
|
% Computes the FK
|
||||||
|
wTee = dh2ForwardKinematics(theta, d, a, alpha, 1);
|
||||||
|
|
||||||
|
% Find Position of End Effector
|
||||||
|
position_ee = wTee(1:2, end);
|
||||||
|
|
||||||
|
% Check if the end-effector is not hitting an obstacle
|
||||||
|
eeHittingObst = 0;
|
||||||
|
if(postionn_ee(2) >= L1 && postionn_ee(2) <= -L1)
|
||||||
|
eeHittingObst = 1;
|
||||||
|
endif
|
||||||
|
if(postionn_ee(1) >= -L2 && position_ee(1) <= L2 && position_ee(2) >= -L2 && position_ee(2) <= L2)
|
||||||
|
eeHittingObst = 1;
|
||||||
|
endif
|
||||||
|
|
||||||
|
% If not interference, store the value
|
||||||
|
if (eeHittingObst == 0 )
|
||||||
|
q1q2_valid = [q1q2_valid, theta];
|
||||||
|
endif
|
||||||
|
|
||||||
|
counter += 1;
|
||||||
|
|
||||||
|
endwhile
|
||||||
|
|
||||||
|
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,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
|
||||||
Loading…
Reference in New Issue