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
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[S, G, randVmin, randVmax, L1, L2] = setParams();
[S, G, randVmin, randVmax, L1, L2, threshold] = setParams();
qi = [];
q1_plot = [];
q2_plot = [];
@ -26,14 +26,15 @@ function DrawObstacles()
for i=1:10000
x = -(L1+L2) + rand()*2*(L1+L2); %creating random x and y values
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
for j=1:nbSol
% saving the computed coordinates
q1 = qi(j,1);
q2 = qi(j,2);
if q2 > 180
q2 = q2-360;
q2 = q2-360; %the IK function is returning q2 values between 0 and 360
endif
q1_plot = [q1_plot q1];

10
buildPRM.m
View File

@ -34,12 +34,12 @@ function [nbNodes, obstacle, points] = buildPRM()
threshold = 0.000001;
[nbSol, qi] = solveIK2LinkPlanarRobot(2, 1, 2, 0);
Sq1 = qi(1,1)
Sq2 = qi(1,2)
Sq1 = qi(1,1);
Sq2 = qi(1,2);
[nbSol, qi] = solveIK2LinkPlanarRobot(2, 1, -2, 0);
Gq1 = qi(1,1)
Gq2 = qi(1,2)
Gq1 = qi(1,1);
Gq2 = qi(1,2);
points = [Sq1 Sq2 S(1) S(2)]';
obstacle = [];
@ -59,7 +59,7 @@ function [nbNodes, obstacle, points] = buildPRM()
x= jTee(1,4);
y= jTee(2,4);
%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);

2
planPathPRM.m
View File

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