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

finished with original function instead of fast computing one

This commit is contained in:
Thomas OLIVE 2021-12-20 00:32:07 +01:00
parent 4c9e2ee655
commit 118adb38c4
3 changed files with 10 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
DrawObstacles.m
View File

@ -27,11 +27,15 @@ function DrawObstacles()
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)|| (y<=-L1) || (x>=-L2 && x<=L2 && y>=-L2 && y<=L2)) % keeping them if they are inside the obstacles
[q1 q2 q1_ q2_] = MyIK(L1, L2, x, y); % computing their corresponding q1 q2 coordinates [nbSol, qi] = solveIK2LinkPlanarRobot(L1, L2, x, y);% computing their corresponding q1 q2 coordinates
% saving the computed coordinates for j=1:nbSol
q1_plot = [q1_plot q1 q1_]; % saving the computed coordinates
q1 = qi(j,1);
q2 = qi(j,2);
q1_plot = [q1_plot q1];
q2_plot = [q2_plot q2 q2_]; q2_plot = [q2_plot q2];
endfor
x_plot = [x_plot x]; x_plot = [x_plot x];
y_plot = [y_plot y]; y_plot = [y_plot y];

2
MyFK.m
View File

@ -5,7 +5,7 @@ function [x, y] = MyFK(L1, L2, Q1, Q2)
% Task: returns the x y coordinates in the cartesian space % Task: returns the x y coordinates in the cartesian space
% from a given point in the q1 q2 joint space % from a given point in the q1 q2 joint space
% using geometry rules since we know that we have two links only of length L1 and L2 % using geometry rules since we know that we have two links only of length L1 and L2
% % computation way faster than the method with matrices
% %
% Inputs: % Inputs:
%-L1: length of the first link of the 2d planar 2 link robot arm %-L1: length of the first link of the 2d planar 2 link robot arm

32
MyIK.m
View File

@ -1,32 +0,0 @@
function [q1 q2 q1_ q2_] = MyIK(L1, L2, x, y)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%function [q1 q2 q1_ q2_] = MyIK(L1, L2, x, y)
%
% Task: returns the two possible solutions of q1 q2 coordinate in the joint space
% from a given point in the x y cartesian space
% using geometry rules since we know that we have two links only of length L1 and L2
%
%
% Inputs:
%-L1: length of the first link of the 2d planar 2 link robot arm
%-L2: length of the second link of the 2d planar 2 link robot arm
%-x: x coordinate of the given point
%-y: x coordinate of the given point
%
% Outputs:
%-q1: q1 coordinate of the given point (first solution)
%-q2: q2 coordinate of the given point (first solution)
%-q1_: q1 coordinate of the given point (second solution)
%-q2_: q2 coordinate of the given point (second solution)
%
%
% Thomas OLIVE (thomas.olive@ecam.fr)
% 18/12/2021
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
q1 = acosd(x/sqrt(x^2+y^2)) - acosd( (-L2^2+x^2+y^2+L1^2) / (2*L1*sqrt(x^2+y^2)));
q2 = 180 - acosd( (L1^2+L2^2-(x^2+y^2)) / (2*L1*L2));
q1_ = q1 + 2*acosd( (-L2^2+x^2+y^2+L1^2) / (2*L1*sqrt(x^2+y^2)));
q2_ = -q2;
endfunction