commit
8c65f17168
|
|
@ -0,0 +1,78 @@
|
|||
function intersect = IsIntersecting (L1, L2, closestPoint, newPoint)
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% function path = buildRRT(L1, L2, start, finish)
|
||||
% Task: Determine the 3D transformation matrix corresponding to a set of Denavit-Hartenberg parameters
|
||||
%
|
||||
% Inputs:
|
||||
% - L1: first length
|
||||
% - L2: second length
|
||||
% - closestPoint: start point x y
|
||||
% - newPoint: end point x y
|
||||
%
|
||||
% Output:
|
||||
% -path: Vector of points
|
||||
%
|
||||
% author: Marais Lucas
|
||||
% date: 22/11/2023
|
||||
x3 = [-L2 -L2 L2 L2];
|
||||
y3 = [-L2 L2 L2 -L2];
|
||||
x1 = [-L1-L2 -L1-L2 L1+L2 L1+L2];
|
||||
y1 = [-L1-L2 L1+L2 L1+L2 -L1-L2];
|
||||
x2 = [-L1-L2 -L1-L2 L1+L2 L1+L2];
|
||||
y2 = [-L1 L1 L1 -L1];
|
||||
% checks if the path is crossed by an obstacle
|
||||
crossesObstacle = false;
|
||||
for i = 1:length(x1)
|
||||
edge1 = [x1(i), y1(i), x1(mod(i, 4) + 1), y1(mod(i, 4) + 1)];
|
||||
edge2 = [x2(i), y2(i), x2(mod(i, 4) + 1), y2(mod(i, 4) + 1)];
|
||||
edge3 = [x3(i), y3(i), x3(mod(i, 4) + 1), y3(mod(i, 4) + 1)];
|
||||
|
||||
% Check if the line intersects with any obstacle edge
|
||||
if doIntersect(closestPoint, newPoint, edge1(1:2), edge1(3:4)) || ...
|
||||
doIntersect(closestPoint, newPoint, edge2(1:2), edge2(3:4)) || ...
|
||||
doIntersect(closestPoint, newPoint, edge3(1:2), edge3(3:4))
|
||||
crossesObstacle = true;
|
||||
break;
|
||||
end
|
||||
end
|
||||
|
||||
% Return the result
|
||||
intersect = crossesObstacle;
|
||||
endfunction
|
||||
|
||||
function intersects = doIntersect(p1, q1, p2, q2)
|
||||
% Function to check if two line segments (p1, q1) and (p2, q2) intersect
|
||||
|
||||
if (p1 == q1) || (p2 == q2)
|
||||
intersects = false; % Degenerate cases, no intersection
|
||||
return;
|
||||
end
|
||||
|
||||
% Check if the line segments are not collinear
|
||||
if orientation(p1, q1, p2) ~= orientation(p1, q1, q2) && ...
|
||||
orientation(p2, q2, p1) ~= orientation(p2, q2, q1)
|
||||
intersects = true;
|
||||
return;
|
||||
end
|
||||
|
||||
intersects = false; % No intersection
|
||||
end
|
||||
|
||||
|
||||
function o = orientation(p, q, r)
|
||||
% Function to find the orientation of triplet (p, q, r)
|
||||
% Returns:
|
||||
% 0 -> Collinear points
|
||||
% 1 -> Clockwise points
|
||||
% 2 -> Counterclockwise points
|
||||
|
||||
val = (q(2) - p(2)) * (r(1) - q(1)) - (q(1) - p(1)) * (r(2) - q(2));
|
||||
|
||||
if val == 0
|
||||
o = 0; % Collinear
|
||||
elseif val > 0
|
||||
o = 1; % Clockwise
|
||||
else
|
||||
o = 2; % Counterclockwise
|
||||
end
|
||||
end
|
||||
|
|
@ -0,0 +1,159 @@
|
|||
function path = buildRRT(L1, L2, pt1, pt2)
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% function path = buildRRT(L1, L2, start, finish)
|
||||
% Task: Determine the 3D transformation matrix corresponding to a set of Denavit-Hartenberg parameters
|
||||
%
|
||||
% Inputs:
|
||||
% - L1: first length
|
||||
% - L2: second length
|
||||
% - x1: start point x
|
||||
% - y1: first point y
|
||||
% - x2: end point x
|
||||
% - y2: end point y
|
||||
%
|
||||
% Output:
|
||||
% -path: Vector of points
|
||||
%
|
||||
% author: Marais Lucas
|
||||
% date: 22/11/2023
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
|
||||
x3 = [-L2 -L2 L2 L2];
|
||||
y3 = [-L2 L2 L2 -L2];
|
||||
x1 = [-L1-L2 -L1-L2 L1+L2 L1+L2];
|
||||
y1 = [-L1-L2 L1+L2 L1+L2 -L1-L2];
|
||||
x2 = [-L1-L2 -L1-L2 L1+L2 L1+L2];
|
||||
y2 = [-L1 L1 L1 -L1];
|
||||
xy_valid = []
|
||||
xy_valid(end+1,:) = pt1
|
||||
q1q2_valid = [];
|
||||
validLinks = [];
|
||||
|
||||
distanceBetweenPoints = 0.1;
|
||||
|
||||
fill(x1, y1, 'r');
|
||||
|
||||
hold on;
|
||||
|
||||
done = 1;
|
||||
if ~(IsIntersecting (L1, L2, pt1, pt2))
|
||||
xy_valid(end+1,:) = pt2;
|
||||
validLinks(end+1,:) = [1 2];
|
||||
done = 0
|
||||
endif
|
||||
|
||||
t = linspace(0, 2*pi, 100)';
|
||||
r=L1+L2;
|
||||
circsx = r.*cos(t) + 0;
|
||||
circsy = r.*sin(t) + 0;
|
||||
hold on;
|
||||
fill(x2, y2, 'w');
|
||||
hold on;
|
||||
plot(circsx, circsy, 'b');
|
||||
|
||||
hold on;
|
||||
fill(x3, y3, 'r');
|
||||
hold on;
|
||||
|
||||
axis equal;
|
||||
|
||||
while(done == 1)
|
||||
% samples randomly the joint space
|
||||
q1 = rand()*360.0;
|
||||
q2 = rand()*360.0;
|
||||
|
||||
% creates 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);
|
||||
|
||||
% determines the position of the end-effector
|
||||
position_ee = wTee(1:2,end);
|
||||
%determine the closest point
|
||||
min = 12345678901234567890;
|
||||
closestPoint = [];
|
||||
closestPointIdx = 0;
|
||||
for i=1:size(xy_valid,1)
|
||||
dist = (position_ee(1)-xy_valid(i,1))^2+ (position_ee(2)-xy_valid(i,2))^2;
|
||||
if (dist < min)
|
||||
min = dist;
|
||||
closestPoint = xy_valid(i, :);
|
||||
closestPointIdx = i;
|
||||
endif
|
||||
endfor
|
||||
min = 12345678901234567890;
|
||||
|
||||
%place the point at a given length
|
||||
vectorForce = [position_ee(1)-closestPoint(1,1) position_ee(2)-closestPoint(1,2)];
|
||||
% Calculate the Euclidean norm (length) of the vector
|
||||
vectorNorm = norm(vectorForce);
|
||||
% Normalize the vector
|
||||
vectorForce = vectorForce / vectorNorm;
|
||||
newPoint = closestPoint+vectorForce*distanceBetweenPoints;
|
||||
|
||||
plot(newPoint(1), newPoint(2), 'b');
|
||||
% checks if the end-effector is not hitting any obstacle
|
||||
eeHittingObstacle = 0;
|
||||
if (newPoint(2) >= L1)
|
||||
eeHittingObstacle = 1;
|
||||
end
|
||||
if (newPoint(2) <= -L1)
|
||||
eeHittingObstacle = 1;
|
||||
end
|
||||
if (newPoint(1) >= -L2 && newPoint(1) <= L2 && newPoint(2) >= -L2 && newPoint(2) <= L2)
|
||||
eeHittingObstacle = 1;
|
||||
end
|
||||
|
||||
% If the there is something wrong don't do
|
||||
if ~(IsIntersecting (L1, L2, closestPoint, newPoint) || eeHittingObstacle == 1)
|
||||
validLinks(end+1,:) = [closestPointIdx length(xy_valid)+1];
|
||||
xy_valid(end+1,:) = newPoint;
|
||||
q1q2_valid(end+1,:) = theta;
|
||||
endif
|
||||
%no more obstacles
|
||||
if ~(IsIntersecting (L1, L2, newPoint, pt2) || eeHittingObstacle == 1)
|
||||
done = 0
|
||||
xy_valid(end+1,:) = pt2;
|
||||
validLinks(end+1,:) = [closestPointIdx length(xy_valid)];
|
||||
endif
|
||||
|
||||
end
|
||||
|
||||
visibilityGraph = zeros(length(xy_valid));
|
||||
|
||||
% Add edges to visibility graph based on valid links
|
||||
for i = 1:length(xy_valid)
|
||||
for j = i+1:length(xy_valid)
|
||||
if ~IsIntersecting(L1, L2, xy_valid(i, :), xy_valid(j, :))
|
||||
% If the line segment between points i and j does not intersect with obstacles
|
||||
visibilityGraph(i, j) = norm(xy_valid(i, :) - xy_valid(j, :));
|
||||
visibilityGraph(j, i) = visibilityGraph(i, j); % Assuming undirected graph
|
||||
else
|
||||
visibilityGraph(i, j) = NaN;% No links
|
||||
visibilityGraph(j, i) = visibilityGraph(i, j); % Assuming undirected graph
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
[distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(length(xy_valid)-2, visibilityGraph);
|
||||
nodeTrajectory = [1 nodeTrajectory];
|
||||
nodeTrajectory(end) = length(xy_valid)
|
||||
for i=1:length(nodeTrajectory)-1
|
||||
x = [xy_valid(nodeTrajectory(i),1) xy_valid(nodeTrajectory(i+1),1)]
|
||||
y = [xy_valid(nodeTrajectory(i),2) xy_valid(nodeTrajectory(i+1),2)]
|
||||
plot(x, y)
|
||||
endfor
|
||||
|
||||
path = nodeTrajectory;
|
||||
end
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -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,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
|
||||
|
||||
|
||||
|
|
@ -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,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');
|
||||
|
||||
|
|
@ -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');
|
||||
|
|
@ -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