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