thomas.olive
/
MotionPlanning
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

added some comments

This commit is contained in:
Thomas OLIVE 2021-12-21 18:01:58 +01:00
parent 0b5c6e2633
commit 42ab8a308b
4 changed files with 11 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
DrawObstacles.m
View File

@ -16,7 +16,7 @@ function DrawObstacles()
% 18/12/2021 % 18/12/2021
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[S, G, randVmin, randVmax, L1, L2] = setParams(); [S, G, randVmin, randVmax, L1, L2, threshold] = setParams();
qi = []; qi = [];
q1_plot = []; q1_plot = [];
q2_plot = []; q2_plot = [];
@ -26,14 +26,15 @@ function DrawObstacles()
for i=1:10000 for i=1:10000
x = -(L1+L2) + rand()*2*(L1+L2); %creating random x and y values x = -(L1+L2) + rand()*2*(L1+L2); %creating random x and y values
y = -(L1+L2) + rand()*2*(L1+L2); y = -(L1+L2) + rand()*2*(L1+L2);
if((y >= L1)|| (y<=-L1) || (x>=-L2 && x<=L2 && y>=-L2 && y<=L2)) % keeping them if they are inside the obstacles if(y >= L1 - threshold || y <= -L1 + threshold || (x>=-L2 - threshold && x<=L2 - threshold && y>=-L2 - threshold && y<=L2 + threshold))% keeping them if they are inside the obstacles
[nbSol, qi] = solveIK2LinkPlanarRobot(L1, L2, x, y);% computing their corresponding q1 q2 coordinates [nbSol, qi] = solveIK2LinkPlanarRobot(L1, L2, x, y);% computing their corresponding q1 q2 coordinates
for j=1:nbSol for j=1:nbSol
% saving the computed coordinates % saving the computed coordinates
q1 = qi(j,1); q1 = qi(j,1);
q2 = qi(j,2); q2 = qi(j,2);
if q2 > 180 if q2 > 180
q2 = q2-360; q2 = q2-360; %the IK function is returning q2 values between 0 and 360
endif endif
q1_plot = [q1_plot q1]; q1_plot = [q1_plot q1];

10
buildPRM.m
View File

@ -34,12 +34,12 @@ function [nbNodes, obstacle, points] = buildPRM()
threshold = 0.000001; threshold = 0.000001;
[nbSol, qi] = solveIK2LinkPlanarRobot(2, 1, 2, 0); [nbSol, qi] = solveIK2LinkPlanarRobot(2, 1, 2, 0);
Sq1 = qi(1,1) Sq1 = qi(1,1);
Sq2 = qi(1,2) Sq2 = qi(1,2);
[nbSol, qi] = solveIK2LinkPlanarRobot(2, 1, -2, 0); [nbSol, qi] = solveIK2LinkPlanarRobot(2, 1, -2, 0);
Gq1 = qi(1,1) Gq1 = qi(1,1);
Gq2 = qi(1,2) Gq2 = qi(1,2);
points = [Sq1 Sq2 S(1) S(2)]'; points = [Sq1 Sq2 S(1) S(2)]';
obstacle = []; obstacle = [];
@ -59,7 +59,7 @@ function [nbNodes, obstacle, points] = buildPRM()
x= jTee(1,4); x= jTee(1,4);
y= jTee(2,4); y= jTee(2,4);
%Checking if the new random point (end effector) is part of a known obstacle in X Y cartesian space %Checking if the new random point (end effector) is part of a known obstacle in X Y cartesian space
if not((y >= L1 - threshold || y <= -L1 + threshold || (x>=-L2 - threshold && x<=L2 - threshold && y>=-L2 - threshold && y<=L2 + threshold)) if not((y >= L1 - threshold || y <= -L1 + threshold || (x>=-L2 - threshold && x<=L2 - threshold && y>=-L2 - threshold && y<=L2 + threshold)))
n=columns(points); n=columns(points);

2
planPathPRM.m
View File

@ -13,7 +13,7 @@ function planPathPRM()
% Thomas OLIVE (thomas.olive@ecam.fr) % Thomas OLIVE (thomas.olive@ecam.fr)
% 18/12/2021 % 18/12/2021
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[S, G, randVmin, randVmax, L1, L2, threshold] = setParams() [S, G, randVmin, randVmax, L1, L2, threshold] = setParams();
[nbNode, visGraph, points] = buildPRM(); [nbNode, visGraph, points] = buildPRM();
[distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNode, visGraph); [distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNode, visGraph);