├── 1_Robot_Manipulator_and_Coordinates ├── DH-functions │ ├── DH_draw.m │ ├── DH_orient.m │ ├── DH_para.m │ ├── DH_plot.m │ └── DH_table.m ├── DH_main.m ├── README.md ├── html.7z └── images │ └── DH_main_01.png ├── 2_2DOF_Manipulator_Inverse_Kinematics ├── INV-functions │ ├── ARM_2DOF.m │ └── INV_KIN_2DOF.m ├── README.md ├── html.7z ├── images │ ├── main_2DOF_01.png │ ├── main_2DOF_02.png │ ├── results_1.gif │ └── results_2.gif └── main_2DOF.m ├── 3_Inverse_Algebraic_Manipulator_Control ├── README.md ├── html.7z ├── images │ ├── inverse_algebraic_control_01.png │ └── inverse_algebraic_control_02.png └── inverse_algebraic_control.m ├── 4_Planar_3DOF_Manipulator_Trajectory ├── INV-functions │ ├── PLANAR_ARM_3DOF.m │ └── PLANAR_INV_KIN_3DOF.m ├── PLANAR_JACOB_3DOF.m ├── PLANAR_main_3DOF.m ├── README.md ├── html.7z └── images │ ├── PLANAR_main_3DOF_01.png │ ├── results_1.gif │ └── results_2.gif ├── 5_PUMA560_Robot_Simulation ├── PUMA-functions │ ├── PUMA_Draw.m │ ├── PUMA_InverseKinematics.m │ ├── PUMA_Plot.m │ ├── PUMA_TransMatrices.m │ └── PUMA_Transformation.m ├── PUMA_Jacob.m ├── PUMA_main.m ├── README.md ├── html.7z └── images │ ├── PUMA_main_01.png │ └── results_1.gif ├── 6_Spring_Mass_Damper_System_Control └── springMassSystem1.slx ├── 7_Dijkstra_Algorithm ├── README.md ├── dijkstra.m ├── html.7z └── images │ ├── dijkstra_01.png │ ├── dijkstra_02.png │ ├── dijkstra_03.png │ └── results_1.gif ├── 8_Astar_Algorithm ├── Astar.m ├── README.md ├── html.7z └── images │ ├── Astar_01.png │ ├── Astar_02.png │ ├── Astar_03.png │ └── results_1.gif ├── Aux1_Rotation_Matrices ├── README.md ├── html.7z ├── images │ ├── rotation_matrices_01.png │ ├── rotation_matrices_02.png │ └── rotation_matrices_03.png └── rotation_matrices.m ├── LICENSE └── README.md /1_Robot_Manipulator_and_Coordinates/DH-functions/DH_draw.m: -------------------------------------------------------------------------------- 1 | % This is used to draw lines in a 3D space using an array of coordinates 2 | % passed in as the parameter 3 | 4 | function [] = DH_draw(X, Y, Z) 5 | 6 | [~, c(1)] = size(X); 7 | [~, c(2)] = size(Y); 8 | [~, c(3)] = size(Z); 9 | 10 | if (c(1) ~= c(2) || c(1) ~= c(3)) 11 | error('Invalid arguments. Size of all the arguments should be same') 12 | return 13 | end 14 | 15 | if (c(1) < 2) 16 | error('The input coordinate arrays should have atleast 2 entries') 17 | end 18 | 19 | for i = 1:(c(1)-1) 20 | plot3([X(i), X(i+1)], [Y(i), Y(i+1)], [Z(i), Z(i+1)]); 21 | hold on 22 | end 23 | 24 | end 25 | -------------------------------------------------------------------------------- /1_Robot_Manipulator_and_Coordinates/DH-functions/DH_orient.m: -------------------------------------------------------------------------------- 1 | % This generates the Transformation Matrix using the orientation and 2 | % position information of the 3 axes. 3 | 4 | function Trans = DH_orient(A, B, G, dX, dY, dZ) 5 | 6 | Rot= [cosd(G)*cosd(B), cosd(G)*sind(B)*sind(A)-sind(A)*cosd(A), cosd(G)*sind(B)*cosd(A)+sind(G)*sind(A); 7 | sind(G)*sind(B), sind(G)*sind(B)*sind(A)+cosd(G)*cosd(A), sind(G)*sind(B)*cosd(G)-cosd(G)*sind(A); 8 | -sind(B), cosd(B)*sind(A), cosd(B)*cosd(A); 9 | 0, 0, 0]; 10 | Disp = [dX; dY; dZ; 1]; 11 | 12 | Trans = horzcat(Rot, Disp); 13 | end -------------------------------------------------------------------------------- /1_Robot_Manipulator_and_Coordinates/DH-functions/DH_para.m: -------------------------------------------------------------------------------- 1 | % This generates the Transformation Matrix using the DH parameters 2 | % The parameteres required are the values of a row in DH Table 3 | 4 | function Trans = DH_para(alpha, a, d, theta, Trans) 5 | 6 | % If only 4 parameters were passed, assume that the transformation matrix 7 | % generated has reference frame same as global reference frame (origin). 8 | if (nargin == 4) 9 | Trans = [cosd(theta), -sind(theta)*cosd(alpha), sind(theta)*sind(alpha), a*cosd(theta); 10 | sind(theta), cosd(theta)*cosd(alpha), -cosd(theta)*sind(alpha), a*sind(theta); 11 | 0, sind(alpha), cosd(alpha), d; 12 | 0, 0, 0, 1]; 13 | 14 | % If all 5 parameters were passed, the passed transfomation matrix is used 15 | % as reference frame and new transformation matrix is generated using DH 16 | % parameters. It is then multiplied with passed transformation matrix to 17 | % give new transfomation matrix with respect to global reference frame. 18 | else 19 | tempTrans =[cosd(theta), -sind(theta)*cosd(alpha), sind(theta)*sind(alpha), a*cosd(theta); 20 | sind(theta), cosd(theta)*cosd(alpha), -cosd(theta)*sind(alpha), a*sind(theta); 21 | 0, sind(alpha), cosd(alpha), d; 22 | 0, 0, 0, 1]; 23 | 24 | Trans = Trans * tempTrans; 25 | end 26 | 27 | end -------------------------------------------------------------------------------- /1_Robot_Manipulator_and_Coordinates/DH-functions/DH_plot.m: -------------------------------------------------------------------------------- 1 | % This function takes in the Transformation matrix as a parameter and draws 2 | % the coordinate system using it. It also returns the origin of the newly 3 | % created coordinate system. 4 | 5 | function [orgX, orgY, orgZ] = DH_plot(Trans) 6 | 7 | [r, c] = size(Trans); 8 | if (r ~= 4 || c ~= 4) 9 | error('Invalid Argument. Transformation Matrix not of correct dimension.'); 10 | end 11 | 12 | orgX = Trans(1,4); 13 | orgY = Trans(2,4); 14 | orgZ = Trans(3,4); 15 | 16 | X = plot3([orgX,orgX+Trans(1,1)], [orgY, orgY+Trans(2,1)], [orgZ, orgZ+Trans(3,1)]); 17 | hold on 18 | grid on 19 | Y = plot3([orgX,orgX+Trans(1,2)], [orgY, orgY+Trans(2,2)], [orgZ, orgZ+Trans(3,2)]); 20 | Z = plot3([orgX,orgX+Trans(1,3)], [orgY, orgY+Trans(2,3)], [orgZ, orgZ+Trans(3,3)]); 21 | 22 | title('Multi-DOF manipulator') 23 | xlabel('X-Coordinate Axis') 24 | ylabel('Y-Coordinate Axis') 25 | zlabel('Z-Coordinate Axis') 26 | 27 | end -------------------------------------------------------------------------------- /1_Robot_Manipulator_and_Coordinates/DH-functions/DH_table.m: -------------------------------------------------------------------------------- 1 | % This creates the Robot Manipulator Link(s) position and orientation. 2 | % The entries of the DH Table are passed as the parameter. 3 | 4 | function [] = DH_table(alpha, a, d, theta) 5 | 6 | [~, c(1)] = size(alpha); 7 | [~, c(2)] = size(a); 8 | [~, c(3)] = size(d); 9 | [~, c(4)] = size(theta); 10 | 11 | if (c(1) ~= c(2) || c(1) ~= c(3) || c(1) ~= c(4)) 12 | error('Invalid arguments. Size of all the arguments should be same') 13 | return 14 | end 15 | 16 | % Plot the Global Reference frame 17 | Trans = DH_para(0,0,0,0); 18 | % or Trans = DH_orient(0,0,0,0,0,0); could also be used 19 | [X(1),Y(1),Z(1)] = DH_plot(Trans); 20 | 21 | for i = 1:c(1) 22 | Trans = DH_para(alpha(i), a(i), d(i), theta(i), Trans); 23 | [X(i+1),Y(i+1),Z(i+1)] = DH_plot(Trans); 24 | end 25 | 26 | DH_draw(X, Y, Z); 27 | 28 | end 29 | 30 | -------------------------------------------------------------------------------- /1_Robot_Manipulator_and_Coordinates/DH_main.m: -------------------------------------------------------------------------------- 1 | %% 2 | % Author: Yash Bansod 3 | % 4 | % GitHub: 5 | % 6 | % This is the main program. 7 | % Just call DH_main in command line or run this file to run the program. 8 | 9 | %% Clear the environment and the command line 10 | clear; 11 | clc; 12 | close all; 13 | 14 | %% Add the directory containing relevant functions to the path variables 15 | addpath('./DH-functions/') 16 | 17 | %% Define the DH table parameters 18 | a = [5, 5, 0]; % Link Lengths (Along X axis) 19 | alpha = [0, 0, 0]; % Link Twist (Across X axis) 20 | d = [0, 0, 2]; % Link Offset (Along Z axis) 21 | theta = [15, 30, 0]; % Link Rotation (Across Z axis) 22 | 23 | %% Call the function that creates and plots the Manipulator 24 | DH_table(alpha, a, d, theta); -------------------------------------------------------------------------------- /1_Robot_Manipulator_and_Coordinates/README.md: -------------------------------------------------------------------------------- 1 | Author: Yash Bansod 2 | GitHub: https://github.com/YashBansod 3 | 4 | This is the main program. Just call DH_main in command line or run this file to run the program. 5 | 6 | ## Clear the environment and the command line 7 | 8 | ```matlab 9 | clear; 10 | clc; 11 | close all; 12 | ``` 13 | 14 | ## Add the directory containing relevant functions to the path variables 15 | 16 | ```matlab 17 | addpath('./DH-functions/') 18 | ``` 19 | 20 | ## Define the DH table parameters 21 | 22 | ```matlab 23 | a = [5, 5, 0]; % Link Lengths (Along X axis) 24 | alpha = [0, 0, 0]; % Link Twist (Across X axis) 25 | d = [0, 0, 2]; % Link Offset (Along Z axis) 26 | theta = [15, 30, 0]; % Link Rotation (Across Z axis) 27 | ``` 28 | 29 | ## Call the function that creates and plots the Manipulator 30 | 31 | ```matlab 32 | DH_table(alpha, a, d, theta); 33 | ``` 34 | 35 | ## Results 36 | 37 |
Multi-DOF manipulator
38 | 39 | -------------------------------------------------------------------------------- /1_Robot_Manipulator_and_Coordinates/html.7z: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YashBansod/Robotics-Planning-Dynamics-and-Control/ee8984dd5f090b803c87ac9fdf4f9be625787b2b/1_Robot_Manipulator_and_Coordinates/html.7z -------------------------------------------------------------------------------- /1_Robot_Manipulator_and_Coordinates/images/DH_main_01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YashBansod/Robotics-Planning-Dynamics-and-Control/ee8984dd5f090b803c87ac9fdf4f9be625787b2b/1_Robot_Manipulator_and_Coordinates/images/DH_main_01.png -------------------------------------------------------------------------------- /2_2DOF_Manipulator_Inverse_Kinematics/INV-functions/ARM_2DOF.m: -------------------------------------------------------------------------------- 1 | function [Jacobian, Joint] = ARM_2DOF(L1, L2, theta1, theta2, draw, orgX, orgY) 2 | %% Function Configuration 3 | if (nargin == 5) 4 | orgX = 0; 5 | orgY = 0; 6 | end 7 | 8 | %% Modelling of the Manipulator 9 | X(1)= orgX+ L1*cosd(theta1); 10 | Y(1)= orgY+ L1*sind(theta1); 11 | X(2)= X(1)+ L2*cosd(theta1 + theta2); 12 | Y(2)= Y(1)+ L2*sind(theta1 + theta2); 13 | 14 | % Storing the Joint Locations of the Manipulator 15 | Joint = [orgX, orgY; X(1), Y(1); X(2), Y(2)]; 16 | 17 | % Storing the Jacobian matrix for the current Manipulator configuration 18 | Jacobian = [-L1*sind(theta1) - L2*sind(theta1+theta2), -L2*sind(theta1+theta2); 19 | L1*cosd(theta1)+ L2*cosd(theta1+theta2), L2*cosd(theta1+theta2)]; 20 | 21 | % If it was mentioned in the parameter 'draw' then draw the manipulator. 22 | if draw == true 23 | plot([orgX,X(1)],[orgY,Y(1)]); 24 | hold on 25 | plot([X(1),X(2)],[Y(1),Y(2)]); 26 | axis([-(L1 + L2), (L1 + L2), -(L1 + L2), (L1 + L2)]); 27 | hold off; 28 | title('2 DOF Planar Manipulator'); 29 | legend('1st Link of Arm', '2nd Link of Arm'); 30 | drawnow; 31 | end 32 | 33 | end 34 | 35 | -------------------------------------------------------------------------------- /2_2DOF_Manipulator_Inverse_Kinematics/INV-functions/INV_KIN_2DOF.m: -------------------------------------------------------------------------------- 1 | function [expPoint, Joint] = INV_KIN_2DOF(L1, L2, expX, expY, theta1, theta2, orgX, orgY) 2 | %% Function Configuration 3 | 4 | % If only Lengths (L1, L2) of the links was passed into the funtion, then 5 | % use the following code sequence. 6 | if (nargin == 2) 7 | theta1 = 0; 8 | theta2 = 0; 9 | orgX = 0; 10 | orgY = 0; 11 | [Jacobian, Joint] = ARM_2DOF(L1, L2, theta1, theta2, true, orgX, orgY); 12 | [expX, expY] = ginput; 13 | 14 | % If the Lengths (L1, L2) and the Final desired position of manipulator 15 | % (expX, expY) was passed then use this code sequence. 16 | elseif (nargin == 4) 17 | theta1 = 0; 18 | theta2 = 0; 19 | orgX = 0; 20 | orgY = 0; 21 | 22 | % If All parameters were passed except the origin of the manipulator (orgX, 23 | % orgY) then use the following code sequence. 24 | elseif (nargin == 6) 25 | orgX = 0; 26 | orgY = 0; 27 | end 28 | 29 | expX = expX(end); 30 | expY = expY(end); 31 | fprintf("The selected point for end effector is (%0.2f, %0.2f)\n", [expX, expY]) 32 | if hypot(orgX - expX, orgY - expY) > L1 + L2 33 | error("Point out of reach"); 34 | return 35 | end 36 | 37 | %% Variable Initialization 38 | 39 | % Control Variables for the PI controller 40 | Kp = 0.4; 41 | Ki = 0.05; 42 | 43 | % Variables to store error related data 44 | sumErrorVec = 0; 45 | errorThreshold = 0.1; 46 | 47 | % Matrix containing Joint angle Information and Desired Point Location 48 | Theta = [theta1; theta2]; 49 | expPoint = [expX; expY]; 50 | 51 | %% Inverse Kinematics Initialization 52 | 53 | % Draw the original position of the manipulator 54 | [Jacobian, Joint] = ARM_2DOF(L1, L2, theta1, theta2, true, orgX, orgY); 55 | [m, n] = size(Joint); 56 | currPoint = [Joint(m,1); Joint(m,2)]; 57 | 58 | % Get the error vector representing difference between current and desired 59 | % position of the manipulator 60 | errorVec = (expPoint - currPoint); 61 | sumErrorVec = errorVec + sumErrorVec; 62 | 63 | % Apply the control equation to the error vector 64 | controlErrorVec = errorVec * Kp + sumErrorVec * Ki; 65 | deltaTheta = transpose(Jacobian) * controlErrorVec; 66 | 67 | % Apply the control changes to the Joint Angles 68 | Theta(1,1) = Theta(1,1) + deltaTheta(1,1); 69 | Theta(2,1) = Theta(2,1) + deltaTheta(2,1); 70 | 71 | % Find the euclidean distance between the desired and current position of 72 | % the manipulator. 73 | dist = sqrt((expPoint(1,1) - currPoint(1,1))^2 + ... 74 | (expPoint(2,1)-currPoint(2,1))^2); 75 | 76 | %% Iterative Inverse Kinematics for correcting the manipulator position. 77 | 78 | % Correct the manipulator position to reduce the distance. 79 | while (dist > errorThreshold) 80 | [Jacobian, Joint] = ARM_2DOF(L1, L2, Theta(1,1), Theta(2,1), ... 81 | true, orgX, orgY); 82 | currPoint = [Joint(m,1); Joint(m,2)]; 83 | errorVec = (expPoint - currPoint); 84 | sumErrorVec = errorVec + sumErrorVec; 85 | controlErrorVec = errorVec * Kp + sumErrorVec * Ki; 86 | deltaTheta = transpose(Jacobian) * controlErrorVec; 87 | Theta(1,1) = Theta(1,1) + deltaTheta(1,1); 88 | Theta(2,1) = Theta(2,1) + deltaTheta(2,1); 89 | dist = sqrt((expPoint(1,1) - currPoint(1,1))^2 + ... 90 | (expPoint(2,1)-currPoint(2,1))^2); 91 | end 92 | 93 | end 94 | -------------------------------------------------------------------------------- /2_2DOF_Manipulator_Inverse_Kinematics/README.md: -------------------------------------------------------------------------------- 1 | Author: Yash Bansod 2 | GitHub: https://github.com/YashBansod 3 | 4 | This is the main program. 5 | 6 | ## Clear the environment and the command line 7 | 8 | ```matlab 9 | clear; 10 | clc; 11 | close all; 12 | ``` 13 | 14 | ## Add the directory containing relevant functions to the path variables 15 | 16 | ```matlab 17 | addpath('./INV-functions/') 18 | ``` 19 | 20 | ## Define the input parameters and simulate 21 | 22 | ```matlab 23 | % Set the length of the links of the manipulator robot. 24 | L1 = 5; 25 | L2 = 5; 26 | 27 | % This function will take desired end-effector position from user via mouse 28 | % pointer. Select a point using the mouse and then press Enter. 29 | % [expPoint, Joint] = INV_KIN_2DOF(L1,L2); 30 | 31 | % Alternatively, user can choose to pass the desired end-effector position 32 | % directly as absolute coordinates too. 33 | expX = 5; 34 | expY = 7; 35 | [expPoint, Joint] = INV_KIN_2DOF(L1, L2, expX, expY); 36 | 37 | fprintf("The final end effector position is (%0.2f, %0.2f)\n", expPoint); 38 | [m, n] = size(Joint); 39 | currPoint = [Joint(m,1); Joint(m,2)]; 40 | dist = sqrt((expPoint(1,1) - currPoint(1,1))^2 + ... 41 | (expPoint(2,1)-currPoint(2,1))^2 ); 42 | 43 | msgbox('Operation Complete') 44 | ``` 45 | 46 | ## Results 47 |
48 | 49 | 50 | 51 | 52 | 53 |
54 |
55 | 56 | 57 | 58 | The selected point for end effector is (5.00, 7.00) 59 | The final end effector position is (5.00, 7.00) 60 | 61 |
62 | 63 | 64 | 65 | 66 | 67 |
68 |
69 | 70 | -------------------------------------------------------------------------------- /2_2DOF_Manipulator_Inverse_Kinematics/html.7z: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YashBansod/Robotics-Planning-Dynamics-and-Control/ee8984dd5f090b803c87ac9fdf4f9be625787b2b/2_2DOF_Manipulator_Inverse_Kinematics/html.7z -------------------------------------------------------------------------------- /2_2DOF_Manipulator_Inverse_Kinematics/images/main_2DOF_01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YashBansod/Robotics-Planning-Dynamics-and-Control/ee8984dd5f090b803c87ac9fdf4f9be625787b2b/2_2DOF_Manipulator_Inverse_Kinematics/images/main_2DOF_01.png -------------------------------------------------------------------------------- /2_2DOF_Manipulator_Inverse_Kinematics/images/main_2DOF_02.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YashBansod/Robotics-Planning-Dynamics-and-Control/ee8984dd5f090b803c87ac9fdf4f9be625787b2b/2_2DOF_Manipulator_Inverse_Kinematics/images/main_2DOF_02.png -------------------------------------------------------------------------------- /2_2DOF_Manipulator_Inverse_Kinematics/images/results_1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YashBansod/Robotics-Planning-Dynamics-and-Control/ee8984dd5f090b803c87ac9fdf4f9be625787b2b/2_2DOF_Manipulator_Inverse_Kinematics/images/results_1.gif -------------------------------------------------------------------------------- /2_2DOF_Manipulator_Inverse_Kinematics/images/results_2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YashBansod/Robotics-Planning-Dynamics-and-Control/ee8984dd5f090b803c87ac9fdf4f9be625787b2b/2_2DOF_Manipulator_Inverse_Kinematics/images/results_2.gif -------------------------------------------------------------------------------- /2_2DOF_Manipulator_Inverse_Kinematics/main_2DOF.m: -------------------------------------------------------------------------------- 1 | %% 2 | % Author: Yash Bansod 3 | % 4 | % GitHub: 5 | % 6 | % This is the main program. 7 | 8 | %% Clear the environment and the command line 9 | clear; 10 | clc; 11 | close all; 12 | 13 | %% Add the directory containing relevant functions to the path variables 14 | addpath('./INV-functions/') 15 | 16 | %% Define the input parameters and simulate 17 | % Set the length of the links of the manipulator robot. 18 | L1 = 5; 19 | L2 = 5; 20 | 21 | % This function will take desired end-effector position from user via mouse 22 | % pointer. Select a point using the mouse and then press Enter. 23 | % [expPoint, Joint] = INV_KIN_2DOF(L1,L2); 24 | 25 | % Alternatively, user can choose to pass the desired end-effector position 26 | % directly as absolute coordinates too. 27 | expX = 5; 28 | expY = 7; 29 | [expPoint, Joint] = INV_KIN_2DOF(L1, L2, expX, expY); 30 | 31 | fprintf("The final end effector position is (%0.2f, %0.2f)\n", expPoint); 32 | [m, n] = size(Joint); 33 | currPoint = [Joint(m,1); Joint(m,2)]; 34 | dist = sqrt((expPoint(1,1) - currPoint(1,1))^2 + ... 35 | (expPoint(2,1)-currPoint(2,1))^2 ); 36 | 37 | msgbox('Operation Complete') 38 | -------------------------------------------------------------------------------- /3_Inverse_Algebraic_Manipulator_Control/README.md: -------------------------------------------------------------------------------- 1 | Author: Yash Bansod 2 | GitHub: https://github.com/YashBansod 3 | 4 | This is the main program. 5 | 6 | ## Clear the environment and the command line 7 | 8 | ```matlab 9 | clear; 10 | clc; 11 | close all; 12 | ``` 13 | 14 | ## Define the input parameters and simulate 15 | 16 | ```matlab 17 | % Set the length of the links of the manipulator robot. 18 | L1 = 10; 19 | L2 = 10; 20 | 21 | % Define the initial end-effector position (This should be reachable) 22 | X = 20; 23 | Y = 0; 24 | ``` 25 | 26 | ## Compute the inverse algebraic solution for the initial position 27 | 28 | ```matlab 29 | C2 = (X^2 + Y^2 - L1^2 -L2^2)/(2 * L1 * L2); 30 | S2 = sqrt(1-C2^2); 31 | 32 | theta2 = atan2(S2, C2); 33 | 34 | K1 = L1 + L2* cos(theta2); 35 | K2 = L2 * sin(theta2); 36 | theta1 = atan2(Y, X) - atan2(K2, K1); 37 | 38 | X1 = L1*cos(theta1); 39 | Y1 = L1*sin(theta1); 40 | 41 | X2 = L1*cos(theta1) + L2*cos(theta1 + theta2); 42 | Y2 = L1*sin(theta1) + L2*sin(theta1 + theta2); 43 | 44 | plot([0, X1], [0,Y1]); 45 | hold on; 46 | grid on; 47 | plot([X1,X2], [Y1, Y2]); 48 | title('2 DOF Planar Manipulator'); 49 | legend('1st Link of Arm - Initial', '2nd Link of Arm - Initial'); 50 | axis([-30 30 -30 30]); 51 | 52 | % Take desired end-effector position from user via mouse pointer. 53 | % [X,Y] = ginput; 54 | % X = X(end); 55 | % Y = Y(end); 56 | 57 | % Alternatively, user can choose the desired end-effector position 58 | % directly as absolute coordinates too. 59 | X = 5; 60 | Y = 7; 61 | 62 | fprintf("The selected point for end effector is (%0.2f, %0.2f)\n", [X, Y]) 63 | if hypot(X, Y) > L1 + L2 64 | error("Point out of reach"); 65 | end 66 | ``` 67 | 68 | The selected point for end effector is (5.00, 7.00) 69 | 70 |
inverse_algebraic_control_01
71 | 72 | ## Compute the inverse algebraic solution for the final position 73 | 74 | ```matlab 75 | C2 = (X^2 + Y^2 - L1^2 -L2^2)/(2 * L1 * L2); 76 | S2 = sqrt(1-C2^2); 77 | 78 | theta2 = atan2(S2, C2); 79 | 80 | K1 = L1 + L2* cos(theta2); 81 | K2 = L2 * sin(theta2); 82 | theta1 = atan2(Y, X) - atan2(K2, K1); 83 | 84 | X1 = L1*cos(theta1); 85 | Y1 = L1*sin(theta1); 86 | 87 | X2 = L1*cos(theta1) + L2*cos(theta1 + theta2); 88 | Y2 = L1*sin(theta1) + L2*sin(theta1 + theta2); 89 | 90 | plot([0, X1], [0,Y1]); 91 | plot([X1,X2], [Y1, Y2]); 92 | legend('1st Link of Arm - Initial', '2nd Link of Arm - Initial', ... 93 | '1st Link of Arm - Final', '2nd Link of Arm - Final'); 94 | hold off; 95 | 96 | fprintf("The final end effector position is (%0.2f, %0.2f)\n", [X2,Y2]); 97 | ``` 98 | 99 | The final end effector position is (5.00, 7.00) 100 | 101 |
inverse_algebraic_control_02
-------------------------------------------------------------------------------- /3_Inverse_Algebraic_Manipulator_Control/html.7z: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YashBansod/Robotics-Planning-Dynamics-and-Control/ee8984dd5f090b803c87ac9fdf4f9be625787b2b/3_Inverse_Algebraic_Manipulator_Control/html.7z -------------------------------------------------------------------------------- /3_Inverse_Algebraic_Manipulator_Control/images/inverse_algebraic_control_01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YashBansod/Robotics-Planning-Dynamics-and-Control/ee8984dd5f090b803c87ac9fdf4f9be625787b2b/3_Inverse_Algebraic_Manipulator_Control/images/inverse_algebraic_control_01.png -------------------------------------------------------------------------------- /3_Inverse_Algebraic_Manipulator_Control/images/inverse_algebraic_control_02.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YashBansod/Robotics-Planning-Dynamics-and-Control/ee8984dd5f090b803c87ac9fdf4f9be625787b2b/3_Inverse_Algebraic_Manipulator_Control/images/inverse_algebraic_control_02.png -------------------------------------------------------------------------------- /3_Inverse_Algebraic_Manipulator_Control/inverse_algebraic_control.m: -------------------------------------------------------------------------------- 1 | %% 2 | % Author: Yash Bansod 3 | % 4 | % GitHub: 5 | % 6 | % This is the main program. 7 | 8 | %% Clear the environment and the command line 9 | clear; 10 | clc; 11 | close all; 12 | 13 | 14 | %% Define the input parameters and simulate 15 | 16 | % Set the length of the links of the manipulator robot. 17 | L1 = 10; 18 | L2 = 10; 19 | 20 | % Define the initial end-effector position (This should be reachable) 21 | X = 20; 22 | Y = 0; 23 | 24 | 25 | %% Compute the inverse algebraic solution for the initial position 26 | 27 | C2 = (X^2 + Y^2 - L1^2 -L2^2)/(2 * L1 * L2); 28 | S2 = sqrt(1-C2^2); 29 | 30 | theta2 = atan2(S2, C2); 31 | 32 | K1 = L1 + L2* cos(theta2); 33 | K2 = L2 * sin(theta2); 34 | theta1 = atan2(Y, X) - atan2(K2, K1); 35 | 36 | X1 = L1*cos(theta1); 37 | Y1 = L1*sin(theta1); 38 | 39 | X2 = L1*cos(theta1) + L2*cos(theta1 + theta2); 40 | Y2 = L1*sin(theta1) + L2*sin(theta1 + theta2); 41 | 42 | plot([0, X1], [0,Y1]); 43 | hold on; 44 | grid on; 45 | plot([X1,X2], [Y1, Y2]); 46 | title('2 DOF Planar Manipulator'); 47 | legend('1st Link of Arm - Initial', '2nd Link of Arm - Initial'); 48 | axis([-30 30 -30 30]); 49 | 50 | % Take desired end-effector position from user via mouse pointer. 51 | % [X,Y] = ginput; 52 | % X = X(end); 53 | % Y = Y(end); 54 | 55 | % Alternatively, user can choose the desired end-effector position 56 | % directly as absolute coordinates too. 57 | X = 5; 58 | Y = 7; 59 | 60 | fprintf("The selected point for end effector is (%0.2f, %0.2f)\n", [X, Y]) 61 | if hypot(X, Y) > L1 + L2 62 | error("Point out of reach"); 63 | end 64 | 65 | %% Compute the inverse algebraic solution for the final position 66 | 67 | C2 = (X^2 + Y^2 - L1^2 -L2^2)/(2 * L1 * L2); 68 | S2 = sqrt(1-C2^2); 69 | 70 | theta2 = atan2(S2, C2); 71 | 72 | K1 = L1 + L2* cos(theta2); 73 | K2 = L2 * sin(theta2); 74 | theta1 = atan2(Y, X) - atan2(K2, K1); 75 | 76 | X1 = L1*cos(theta1); 77 | Y1 = L1*sin(theta1); 78 | 79 | X2 = L1*cos(theta1) + L2*cos(theta1 + theta2); 80 | Y2 = L1*sin(theta1) + L2*sin(theta1 + theta2); 81 | 82 | plot([0, X1], [0,Y1]); 83 | plot([X1,X2], [Y1, Y2]); 84 | legend('1st Link of Arm - Initial', '2nd Link of Arm - Initial', ... 85 | '1st Link of Arm - Final', '2nd Link of Arm - Final'); 86 | hold off; 87 | 88 | fprintf("The final end effector position is (%0.2f, %0.2f)\n", [X2,Y2]); 89 | 90 | -------------------------------------------------------------------------------- /4_Planar_3DOF_Manipulator_Trajectory/INV-functions/PLANAR_ARM_3DOF.m: -------------------------------------------------------------------------------- 1 | function [Jacobian, Joint] = PLANAR_ARM_3DOF(L1, L2, L3, theta1, theta2, theta3, draw) 2 | 3 | orgX = 0; 4 | orgY = 0; 5 | 6 | %% Modelling of the Manipulator 7 | X(1)= orgX+ L1*cosd(theta1); 8 | Y(1)= orgY+ L1*sind(theta1); 9 | X(2)= X(1)+ L2*cosd(theta1 + theta2); 10 | Y(2)= Y(1)+ L2*sind(theta1 + theta2); 11 | X(3)= X(2)+ L3*cosd(theta1 + theta2 + theta3); 12 | Y(3)= Y(2)+ L3*sind(theta1 + theta2 + theta3); 13 | 14 | % Storing the Joint Locations of the Manipulator 15 | Joint = [orgX, orgY; X(1), Y(1); X(2), Y(2); X(3), Y(3)]; 16 | 17 | % Storing the Jacobian matrix for the current Manipulator configuration 18 | Jacobian = [- L2*sind(theta1 + theta2) - L1*sind(theta1) - L3*sind(theta1 + theta2 + theta3), - L2*sind(theta1 + theta2) - L3*sind(theta1 + theta2 + theta3), -L3*sind(theta1 + theta2 + theta3); 19 | L2*cosd(theta1 + theta2) + L1*cosd(theta1) + L3*cosd(theta1 + theta2 + theta3), L2*cosd(theta1 + theta2) + L3*cosd(theta1 + theta2 + theta3), L3*cosd(theta1 + theta2 + theta3)]; 20 | 21 | % If it was mentioned in the parameter 'draw' then draw the manipulator. 22 | 23 | if draw == true 24 | hold on; 25 | plot([orgX,X(1)],[orgY,Y(1)]); 26 | plot([X(1),X(2)],[Y(1),Y(2)]); 27 | plot([X(2),X(3)],[Y(2),Y(3)]); 28 | hold off; 29 | axis([-(L1 + L2 + L3) (L1 + L2 + L3) -(L1 + L2 + L3) (L1 + L2 + L3)]); 30 | % mark the graph with details 31 | title('3 DOF Planar Manipulator'); 32 | legend('1st Link of Arm', '2nd Link of Arm' , '3rd Link of Arm'); 33 | drawnow; 34 | end 35 | 36 | end 37 | 38 | -------------------------------------------------------------------------------- /4_Planar_3DOF_Manipulator_Trajectory/INV-functions/PLANAR_INV_KIN_3DOF.m: -------------------------------------------------------------------------------- 1 | function [expPoint, Joint, Theta] = PLANAR_INV_KIN_3DOF(L1, L2, L3, expX, expY, theta1, theta2, theta3) 2 | %% Variable Initialization 3 | % Variables to store error related data 4 | Kp = 0.3; % Proportional constant for inverse jacobian controller 5 | errorThreshold = 0.01; 6 | 7 | % Matrix containing Joint angle Information and Desired Point Location 8 | Theta = [theta1; theta2; theta3]; 9 | expPoint = [expX; expY]; 10 | 11 | %% Inverse Kinematics Initialization 12 | 13 | % Draw the original position of the manipulator 14 | [Jacobian, Joint] = PLANAR_ARM_3DOF(L1, L2, L3, theta1, theta2, theta3, false); 15 | [m, ~] = size(Joint); 16 | currPoint = [Joint(m,1); Joint(m,2)]; 17 | 18 | % Get the error vector representing difference between current and desired 19 | % position of the manipulator 20 | errorVec = (expPoint - currPoint); 21 | 22 | % Apply the control equation to the error vector 23 | deltaTheta = transpose(Jacobian) * errorVec * Kp; 24 | 25 | % Apply the control changes to the Joint Angles 26 | Theta(1,1) = Theta(1,1) + deltaTheta(1,1); 27 | Theta(2,1) = Theta(2,1) + deltaTheta(2,1); 28 | Theta(3,1) = Theta(3,1) + deltaTheta(3,1); 29 | 30 | % Find the euclidean distance between the desired and current position of 31 | % the manipulator. 32 | dist = hypot((expPoint(1,1) - currPoint(1,1)), (expPoint(2,1)-currPoint(2,1))); 33 | 34 | %% Iterative Inverse Kinematics for correcting the manipulator position. 35 | 36 | % Correct the manipulator position to reduce the distance. 37 | while (dist > errorThreshold) 38 | [Jacobian, Joint] = PLANAR_ARM_3DOF(L1, L2, L3, Theta(1,1), ... 39 | Theta(2,1), Theta(3,1), false); 40 | currPoint = [Joint(m,1); Joint(m,2)]; 41 | errorVec = (expPoint - currPoint); 42 | deltaTheta = transpose(Jacobian) * errorVec * Kp; 43 | Theta(1,1) = Theta(1,1) + deltaTheta(1,1); 44 | Theta(2,1) = Theta(2,1) + deltaTheta(2,1); 45 | Theta(3,1) = Theta(3,1) + deltaTheta(3,1); 46 | dist = hypot((expPoint(1,1) - currPoint(1,1)), (expPoint(2,1)-currPoint(2,1))); 47 | end 48 | 49 | [~, Joint] = PLANAR_ARM_3DOF(L1, L2, L3, Theta(1,1), Theta(2,1), Theta(3,1), true); 50 | end 51 | -------------------------------------------------------------------------------- /4_Planar_3DOF_Manipulator_Trajectory/PLANAR_JACOB_3DOF.m: -------------------------------------------------------------------------------- 1 | %% 2 | % Author: Yash Bansod 3 | % 4 | % GitHub: 5 | % 6 | % Demonstrates the calculation of Jacobian for a 3DOF 2D Planar Robot 7 | 8 | 9 | %% Clear the environment and the command line 10 | clear; 11 | clc; 12 | close all; 13 | 14 | %% Calculation of Jacobian for 3DOF 2D Planar Robot 15 | syms t1 t2 t3 L1 L2 L3 16 | orgX = 0; 17 | orgY = 0; 18 | 19 | X(1)= orgX+ L1*cos(t1); 20 | Y(1)= orgY+ L1*sin(t1); 21 | X(2)= X(1)+ L2*cos(t1 + t2); 22 | Y(2)= Y(1)+ L2*sin(t1 + t2); 23 | X(3)= X(2)+ L3*cos(t1 + t2 + t3); 24 | Y(3)= Y(2)+ L3*sin(t1 + t2 + t3); 25 | 26 | Jacob = [diff(X(3),t1) , diff(X(3), t2) , diff(X(3), t3); 27 | diff(Y(3), t1), diff(Y(3), t2) , diff(Y(3), t3)] -------------------------------------------------------------------------------- /4_Planar_3DOF_Manipulator_Trajectory/PLANAR_main_3DOF.m: -------------------------------------------------------------------------------- 1 | %% 2 | % Author: Yash Bansod 3 | % 4 | % GitHub: 5 | % 6 | % This is the main program. 7 | 8 | %% Clear the environment and the command line 9 | clear; 10 | clc; 11 | close all; 12 | 13 | %% Add the directory containing relevant functions to the path variables 14 | addpath('./INV-functions/') 15 | 16 | %% Define the input parameters and simulate 17 | 18 | % Set the length of the links of the manipulator robot. 19 | L1 = 5; 20 | L2 = 5; 21 | L3 = 5; 22 | 23 | % Set the initial orientation of the robot. 24 | theta1 = 10; 25 | theta2 = 0; 26 | theta3 = 15; 27 | 28 | % Define the radius of the circle the end effector should follow 29 | radius = 10; 30 | r_sq = radius ^ 2; 31 | 32 | hold on; 33 | % Code for drawing a circle 34 | for i = -radius: radius/10: radius 35 | expX = i; 36 | expY = sqrt(r_sq - expX^2); 37 | cla; 38 | images.roi.Circle(gca,'Center',[0 0],'Radius',radius, 'Facealpha', 0.05); 39 | % You can modify the Kp value defined in PLANAR_INV_KIN_3DOF to modify 40 | % the inverse jacobian controller behavior. 41 | [expPoint, Joint, Theta] = PLANAR_INV_KIN_3DOF(L1, L2, L3, expX, ... 42 | expY, theta1, theta2, theta3); 43 | scatter(Joint(end,1), Joint(end,2)); 44 | theta1 = Theta(1, 1); 45 | theta2 = Theta(2, 1); 46 | theta3 = Theta(3, 1); 47 | end 48 | 49 | for i = radius: -radius/10: -radius 50 | expX = i; 51 | expY = -sqrt(r_sq - expX^2); 52 | cla; 53 | images.roi.Circle(gca,'Center',[0 0],'Radius',radius, 'Facealpha', 0.05); 54 | % You can modify the Kp value defined in PLANAR_INV_KIN_3DOF to modify 55 | % the inverse jacobian controller behavior. 56 | [expPoint, Joint, Theta] = PLANAR_INV_KIN_3DOF(L1, L2, L3, expX, ... 57 | expY, theta1, theta2, theta3); 58 | theta1 = Theta(1, 1); 59 | theta2 = Theta(2, 1); 60 | theta3 = Theta(3, 1); 61 | end 62 | -------------------------------------------------------------------------------- /4_Planar_3DOF_Manipulator_Trajectory/README.md: -------------------------------------------------------------------------------- 1 | Author: Yash Bansod 2 | GitHub: https://github.com/YashBansod 3 | 4 | This is the main program. 5 | 6 | ## Clear the environment and the command line 7 | 8 | ```matlab 9 | clear; 10 | clc; 11 | close all; 12 | ``` 13 | 14 | ## Add the directory containing relevant functions to the path variables 15 | 16 | ```matlab 17 | addpath('./INV-functions/') 18 | ``` 19 | 20 | ## Define the input parameters and simulate 21 | 22 | ```matlab 23 | % Set the length of the links of the manipulator robot. 24 | L1 = 5; 25 | L2 = 5; 26 | L3 = 5; 27 | 28 | % Set the initial orientation of the robot. 29 | theta1 = 10; 30 | theta2 = 0; 31 | theta3 = 15; 32 | 33 | % Define the radius of the circle the end effector should follow 34 | radius = 10; 35 | r_sq = radius ^ 2; 36 | 37 | hold on; 38 | % Code for drawing a circle 39 | for i = -radius: radius/10: radius 40 | expX = i; 41 | expY = sqrt(r_sq - expX^2); 42 | cla; 43 | images.roi.Circle(gca,'Center',[0 0],'Radius',radius, 'Facealpha', 0.05); 44 | % You can modify the Kp value defined in PLANAR_INV_KIN_3DOF to modify 45 | % the inverse jacobian controller behavior. 46 | [expPoint, Joint, Theta] = PLANAR_INV_KIN_3DOF(L1, L2, L3, expX, ... 47 | expY, theta1, theta2, theta3); 48 | scatter(Joint(end,1), Joint(end,2)); 49 | theta1 = Theta(1, 1); 50 | theta2 = Theta(2, 1); 51 | theta3 = Theta(3, 1); 52 | end 53 | 54 | for i = radius: -radius/10: -radius 55 | expX = i; 56 | expY = -sqrt(r_sq - expX^2); 57 | cla; 58 | images.roi.Circle(gca,'Center',[0 0],'Radius',radius, 'Facealpha', 0.05); 59 | % You can modify the Kp value defined in PLANAR_INV_KIN_3DOF to modify 60 | % the inverse jacobian controller behavior. 61 | [expPoint, Joint, Theta] = PLANAR_INV_KIN_3DOF(L1, L2, L3, expX, ... 62 | expY, theta1, theta2, theta3); 63 | theta1 = Theta(1, 1); 64 | theta2 = Theta(2, 1); 65 | theta3 = Theta(3, 1); 66 | end 67 | ``` 68 | 69 | ## Results 70 | 71 |
72 | 73 | 74 | 75 | 76 | 77 |
78 |
79 | 80 | 81 |
82 | -------------------------------------------------------------------------------- /4_Planar_3DOF_Manipulator_Trajectory/html.7z: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YashBansod/Robotics-Planning-Dynamics-and-Control/ee8984dd5f090b803c87ac9fdf4f9be625787b2b/4_Planar_3DOF_Manipulator_Trajectory/html.7z -------------------------------------------------------------------------------- /4_Planar_3DOF_Manipulator_Trajectory/images/PLANAR_main_3DOF_01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YashBansod/Robotics-Planning-Dynamics-and-Control/ee8984dd5f090b803c87ac9fdf4f9be625787b2b/4_Planar_3DOF_Manipulator_Trajectory/images/PLANAR_main_3DOF_01.png -------------------------------------------------------------------------------- /4_Planar_3DOF_Manipulator_Trajectory/images/results_1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YashBansod/Robotics-Planning-Dynamics-and-Control/ee8984dd5f090b803c87ac9fdf4f9be625787b2b/4_Planar_3DOF_Manipulator_Trajectory/images/results_1.gif -------------------------------------------------------------------------------- /4_Planar_3DOF_Manipulator_Trajectory/images/results_2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YashBansod/Robotics-Planning-Dynamics-and-Control/ee8984dd5f090b803c87ac9fdf4f9be625787b2b/4_Planar_3DOF_Manipulator_Trajectory/images/results_2.gif -------------------------------------------------------------------------------- /5_PUMA560_Robot_Simulation/PUMA-functions/PUMA_Draw.m: -------------------------------------------------------------------------------- 1 | function [] = PUMA_Draw(X, Y, Z) 2 | % This is used to draw lines in a 3D space using an array of coordinates 3 | % passed in as the parameter 4 | 5 | %% Handle errors (if any) due to invalid dimensions of the parameters 6 | [r(1), c(1)] = size(X); 7 | [r(2), c(2)] = size(Y); 8 | [r(3), c(3)] = size(Z); 9 | 10 | if (c(1) ~= c(2) | c(1) ~= c(3)) 11 | error('Invalid arguments. Size of all the arguments should be same') 12 | return 13 | end 14 | 15 | if (c(1) < 2) 16 | error('The input coordinate arrays should have atleast 2 entries') 17 | end 18 | 19 | %% Draw the line through the points marked by the parameter 20 | % hold on 21 | for i = 1:(c(1)-1) 22 | plot3([X(i), X(i+1)], [Y(i), Y(i+1)], [Z(i), Z(i+1)]); 23 | end 24 | drawnow; 25 | 26 | end 27 | -------------------------------------------------------------------------------- /5_PUMA560_Robot_Simulation/PUMA-functions/PUMA_InverseKinematics.m: -------------------------------------------------------------------------------- 1 | function [] = PUMA_InverseKinematics(Trans, theta, th, dstPoint) 2 | %% Variable Initialization 3 | 4 | % Variables to store error related data 5 | Kp = 0.0045; 6 | errorThreshold = 0.1; 7 | 8 | % Matrix containing Joint angle Information and Desired Point Location 9 | t1 = th(1); t2 = th(2); t3 = th(3); 10 | Theta = [double(subs(theta(1))); double(subs(theta(2))); double(subs(theta(3)))]; 11 | 12 | % Make a column matrix representing the Position of the End-Effector 13 | endPoint = [Trans(1,4,6); Trans(2,4,6); Trans(3,4,6)]; 14 | % Find the current numeric end-effector position 15 | currPoint = double(subs(endPoint)); 16 | % Find the error vector 17 | errorVec = (dstPoint - currPoint); 18 | % Create a Jacobian Matrix using the given Theta values 19 | Jacobian = PUMA_Jacob(Theta(1,1), Theta(2,1), Theta(3,1)); 20 | 21 | for i=1:3 22 | [X(i), Y(i), Z(i)] = PUMA_Plot(double(subs(Trans(:,:,i)))); 23 | end 24 | for i=4:6 25 | X(i) = double(subs(Trans(1,4,i))); 26 | Y(i) = double(subs(Trans(2,4,i))); 27 | Z(i) = double(subs(Trans(3,4,i))); 28 | end 29 | 30 | PUMA_Draw(X, Y, Z); 31 | 32 | %% Inverse Kinematics Initialization 33 | 34 | % Apply the control equation to the error vector 35 | deltaTheta = transpose(Jacobian) * errorVec * Kp; 36 | % Apply the control changes to the Joint Angles 37 | Theta(1,1) = Theta(1,1) + deltaTheta(1,1); 38 | Theta(2,1) = Theta(2,1) + deltaTheta(2,1); 39 | Theta(3,1) = Theta(3,1) + deltaTheta(3,1); 40 | t1 = Theta(1,1); t2 = Theta(2,1); t3 = Theta(3,1); 41 | 42 | % Find the euclidean distance between the desired and current position of 43 | % the manipulator. 44 | dist = sqrt((dstPoint(1,1) - currPoint(1,1))^2 + ... 45 | (dstPoint(2,1)-currPoint(2,1))^2 +(dstPoint(3,1)-currPoint(3,1))^2); 46 | 47 | %% Iterative Inverse Kinematics for correcting the manipulator position. 48 | 49 | % Correct the manipulator position to reduce the distance. 50 | while (dist > errorThreshold) 51 | Jacobian = PUMA_Jacob(Theta(1,1), Theta(2,1), Theta(3,1)); 52 | currPoint = double(subs(endPoint)); 53 | errorVec = (dstPoint - currPoint); 54 | deltaTheta = transpose(Jacobian) * errorVec * Kp; 55 | Theta(1,1) = Theta(1,1) + deltaTheta(1,1); 56 | Theta(2,1) = Theta(2,1) + deltaTheta(2,1); 57 | Theta(3,1) = Theta(3,1) + deltaTheta(3,1); 58 | t1 = Theta(1,1); t2 = Theta(2,1); t3 = Theta(3,1); 59 | dist = sqrt((dstPoint(1,1) - currPoint(1,1))^2 + ... 60 | (dstPoint(2,1)-currPoint(2,1))^2 +(dstPoint(3,1)-currPoint(3,1))^2); 61 | 62 | for i=1:3 63 | [X(i), Y(i), Z(i)] = PUMA_Plot(double(subs(Trans(:,:,i)))); 64 | end 65 | for i=4:6 66 | X(i) = double(subs(Trans(1,4,i))); 67 | Y(i) = double(subs(Trans(2,4,i))); Z(i) = double(subs(Trans(3,4,i))); 68 | end 69 | PUMA_Draw(X, Y, Z); 70 | 71 | end 72 | 73 | cla 74 | for i=1:3 75 | [X(i), Y(i), Z(i)] = PUMA_Plot(double(subs(Trans(:,:,i)))); 76 | end 77 | for i=4:6 78 | X(i) = double(subs(Trans(1,4,i))); 79 | Y(i) = double(subs(Trans(2,4,i))); 80 | Z(i) = double(subs(Trans(3,4,i))); 81 | end 82 | PUMA_Draw(X, Y, Z); 83 | end 84 | -------------------------------------------------------------------------------- /5_PUMA560_Robot_Simulation/PUMA-functions/PUMA_Plot.m: -------------------------------------------------------------------------------- 1 | function [orgX, orgY, orgZ] = PUMA_Plot(Trans) 2 | % This function takes in the Transformation matrix as a parameter and draws 3 | % the coordinate system using it. It also returns the origin of the newly 4 | % created coordinate system. 5 | 6 | %% Handle errors (if any) due to incorrect dimension of Parameter 7 | [r, c] = size(Trans); 8 | if (r ~= 4 || c ~= 4) 9 | error('Invalid Argument. Transformation Matrix not of correct dimension.'); 10 | end 11 | 12 | %% Plot the robot configuration using the current transformation matrices 13 | 14 | orgX = Trans(1,4); 15 | orgY = Trans(2,4); 16 | orgZ = Trans(3,4); 17 | 18 | X = plot3([orgX,orgX+Trans(1,1)], [orgY, orgY+Trans(2,1)], [orgZ, orgZ+Trans(3,1)]); 19 | % hold on 20 | Y = plot3([orgX,orgX+Trans(1,2)], [orgY, orgY+Trans(2,2)], [orgZ, orgZ+Trans(3,2)]); 21 | Z = plot3([orgX,orgX+Trans(1,3)], [orgY, orgY+Trans(2,3)], [orgZ, orgZ+Trans(3,3)]); 22 | 23 | end -------------------------------------------------------------------------------- /5_PUMA560_Robot_Simulation/PUMA-functions/PUMA_TransMatrices.m: -------------------------------------------------------------------------------- 1 | function Trans = PUMA_TransMatrices(alpha, a, d, theta) 2 | % This creates the Robot Manipulator Link(s) Transformation Matrix. 3 | % The entries of the DH Table are passed in as the parameter. 4 | 5 | %% Handle errors (if any) for the parameters passed into the function 6 | 7 | % Check for the dimensions of the parameters passed 8 | [~, c(1)] = size(alpha); 9 | [~, c(2)] = size(a); 10 | [~, c(3)] = size(d); 11 | [~, c(4)] = size(theta); 12 | 13 | if (c(1) ~= c(2) || c(1) ~= c(3) || c(1) ~= c(4)) 14 | error('Invalid arguments. Size of all the arguments should be same') 15 | return 16 | end 17 | 18 | %% Generate the Transformation Matrices from the DH-parameters passed 19 | 20 | % Find the Transformation matrix from the first row of DH-Table 21 | % This will give us the position and orientation of the end of first link 22 | % of the Robot Manipulator 23 | Trans(:,:,1) = PUMA_Transformation(alpha(1), a(1), d(1), theta(1)); 24 | 25 | % Find the Transformation matrix from the rest of the rows of DH-Table 26 | % This will give us the position and orientation of the rest of the links 27 | % of the Robot Manipulator 28 | for i = 2:c(1) 29 | Trans(:,:,i)= PUMA_Transformation(alpha(i), a(i), d(i), theta(i), Trans(:,:, i-1)); 30 | end 31 | 32 | end 33 | 34 | -------------------------------------------------------------------------------- /5_PUMA560_Robot_Simulation/PUMA-functions/PUMA_Transformation.m: -------------------------------------------------------------------------------- 1 | function Trans = PUMA_Transformation(alpha, a, d, theta, Trans) 2 | % This generates the Transformation Matrix using the DH-parameters 3 | % The parameteres required are the values of a row in DH Table 4 | 5 | %% Create the Transformation Matrix based on number of parameters passed 6 | 7 | % If only 4 parameters were passed, assume that the transformation matrix 8 | % generated has reference frame same as global reference frame (origin). 9 | if (nargin == 4) 10 | Trans = [cos(theta), -sin(theta), 0, a; 11 | sin(theta)*cos(alpha), cos(theta)*cos(alpha), -sin(alpha), -sin(alpha)*d; 12 | sin(theta)*sin(alpha), cos(theta)*sin(alpha), cos(alpha), cos(alpha)*d; 13 | 0, 0, 0, 1]; 14 | 15 | % If all 5 parameters were passed, the passed transfomation matrix is used 16 | % as reference frame and new transformation matrix is generated using DH 17 | % parameters. It is then multiplied with passed transformation matrix to 18 | % give new transfomation matrix with respect to global reference frame. 19 | else 20 | tempTrans = [cos(theta), -sin(theta), 0, a; 21 | sin(theta)*cos(alpha), cos(theta)*cos(alpha), -sin(alpha), -sin(alpha)*d; 22 | sin(theta)*sin(alpha), cos(theta)*sin(alpha), cos(alpha), cos(alpha)*d; 23 | 0, 0, 0, 1]; 24 | 25 | Trans = Trans * tempTrans; 26 | end 27 | 28 | end -------------------------------------------------------------------------------- /5_PUMA560_Robot_Simulation/PUMA_Jacob.m: -------------------------------------------------------------------------------- 1 | function tempJacob = PUMA_Jacob(t1,t2,t3) 2 | %PUMA_JACOB 3 | % TEMPJACOB = PUMA_JACOB(T1,T2,T3) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.4. 6 | % 04-Jun-2020 01:25:07 7 | 8 | t5 = cos(t1); 9 | t6 = cos(t2); 10 | t7 = cos(t3); 11 | t8 = sin(t1); 12 | t9 = sin(t2); 13 | t10 = sin(t3); 14 | t11 = t5.*t6; 15 | t12 = t5.*t9; 16 | t13 = t6.*t8; 17 | t14 = t6.*t10; 18 | t15 = t7.*t9; 19 | t16 = t8.*t9; 20 | t17 = t6.*t7.*1.0e+1; 21 | t19 = t9.*t10.*1.0e+1; 22 | t18 = -t16; 23 | t20 = -t17; 24 | t21 = t11.*6.123233995736766e-17; 25 | t22 = t12.*6.123233995736766e-17; 26 | t23 = t13.*6.123233995736766e-17; 27 | t24 = t16.*6.123233995736766e-17; 28 | t25 = -t24; 29 | t26 = t12+t23; 30 | t27 = t13+t22; 31 | t29 = t18+t21; 32 | t28 = t11+t25; 33 | t30 = t7.*t27; 34 | t31 = t10.*t26; 35 | t34 = t7.*t26.*1.0e+1; 36 | t35 = t10.*t29; 37 | t36 = t10.*t27.*1.0e+1; 38 | t41 = t7.*t29.*1.0e+1; 39 | t32 = t7.*t28; 40 | t33 = -t30; 41 | t38 = -t34; 42 | t39 = t10.*t28.*1.0e+1; 43 | t40 = -t36; 44 | t42 = -t35; 45 | t37 = -t32; 46 | t43 = -t39; 47 | tempJacob = reshape([t5.*(-1.0)-t12.*6.123233995736766e-16-t13.*1.0e+1-t30.*1.0e+1-t35.*1.0e+1-t7.*t29+t10.*t27,t8.*(-1.0)+t11.*1.0e+1-t16.*6.123233995736766e-16-t31.*1.0e+1+t32.*1.0e+1-t7.*t26-t10.*t28,0.0,t12.*-1.0e+1-t13.*6.123233995736766e-16+t31+t37+t38+t43,t11.*6.123233995736766e-16-t16.*1.0e+1+t33+t40+t41+t42,t6.*-1.0e+1+t14+t15+t19+t20,t31+t37+t38+t43,t33+t40+t41+t42,t14+t15+t19+t20,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0],[3,6]); 48 | -------------------------------------------------------------------------------- /5_PUMA560_Robot_Simulation/PUMA_main.m: -------------------------------------------------------------------------------- 1 | %% 2 | % Author: Yash Bansod 3 | % 4 | % GitHub: 5 | % 6 | % This is the main program. 7 | 8 | %% Clear the environment and the command line 9 | clear; 10 | clc; 11 | close all; 12 | 13 | %% Add the directory containing relevant functions to the path variables 14 | addpath('./PUMA-functions/') 15 | 16 | %% Define the input parameters and simulate 17 | 18 | % Initialize the DH-Parameters 19 | % Angle values must be in radians 20 | alpha = [0, -pi/2, 0, -pi/2, pi/2, -pi/2]; 21 | a = [0, 0, 10, 10, 0, 0]; 22 | d = [0, 0, 1, 1, 0, 0]; 23 | % Create the symbolic variables for theta1, theta2, ... , theta6 24 | theta = sym('t', [1 6]); 25 | 26 | % Find the Transformation Matrices using the DH-Parameters 27 | Trans = PUMA_TransMatrices(alpha, a, d, theta); 28 | % Make a column matrix representing the Position of the End-Effector 29 | endPoint = [Trans(1,4,6); Trans(2,4,6); Trans(3,4,6)]; 30 | % Find the Jacobian Matrix using the Position matrix of the End-Effector 31 | tempJacob = jacobian(endPoint, theta); 32 | 33 | % Create a simplified Jacobian matrix using 'matlabFunction' 34 | matlabFunction(tempJacob, 'File', 'PUMA_Jacob'); 35 | 36 | 37 | % Set the desired destination position of the end-effector 38 | dstPoint = [-1;3;-10]; 39 | 40 | % Set the values of the theta at present configuration 41 | th(1) = pi/2; th(2) = pi/3; th(3) = pi/6; 42 | 43 | % Calculate the initial transform matrix 44 | % Matrix containing Joint angle Information and Desired Point Location 45 | t1 = th(1); t2 = th(2); t3 = th(3); 46 | currPoint = [double(subs(Trans(1,4,6))); double(subs(Trans(2,4,6))); ... 47 | double(subs(Trans(3,4,6)))]; 48 | 49 | % Initialize the plot 50 | scatter3(0, 0, 0, '*'); 51 | hold on 52 | scatter3(currPoint(1), currPoint(2), currPoint(3)); 53 | scatter3(dstPoint(1), dstPoint(2), dstPoint(3)); 54 | title('PUMA 560 Manipulator Simulation') 55 | xlabel('X-Coordinate Axis') 56 | ylabel('Y-Coordinate Axis') 57 | zlabel('Z-Coordinate Axis') 58 | axis square 59 | hold on 60 | 61 | PUMA_InverseKinematics(Trans, theta, th, dstPoint) 62 | 63 | % Finalize the plot 64 | figure(1) 65 | scatter3(0, 0, 0, '*'); 66 | scatter3(currPoint(1), currPoint(2), currPoint(3)); 67 | scatter3(dstPoint(1), dstPoint(2), dstPoint(3)); 68 | title('PUMA 560 Manipulator Simulation') 69 | xlabel('X-Coordinate Axis') 70 | ylabel('Y-Coordinate Axis') 71 | zlabel('Z-Coordinate Axis') -------------------------------------------------------------------------------- /5_PUMA560_Robot_Simulation/README.md: -------------------------------------------------------------------------------- 1 | Author: Yash Bansod 2 | GitHub: https://github.com/YashBansod 3 | 4 | This is the main program. 5 | 6 | ## Clear the environment and the command line 7 | 8 | ```matlab 9 | clear; 10 | clc; 11 | close all; 12 | ``` 13 | 14 | ## Add the directory containing relevant functions to the path variables 15 | 16 | ```matlab 17 | addpath('./PUMA-functions/') 18 | ``` 19 | 20 | ## Define the input parameters and simulate 21 | 22 | ```matlab 23 | % Initialize the DH-Parameters 24 | % Angle values must be in radians 25 | alpha = [0, -pi/2, 0, -pi/2, pi/2, -pi/2]; 26 | a = [0, 0, 10, 10, 0, 0]; 27 | d = [0, 0, 1, 1, 0, 0]; 28 | % Create the symbolic variables for theta1, theta2, ... , theta6 29 | theta = sym('t', [1 6]); 30 | 31 | % Find the Transformation Matrices using the DH-Parameters 32 | Trans = PUMA_TransMatrices(alpha, a, d, theta); 33 | % Make a column matrix representing the Position of the End-Effector 34 | endPoint = [Trans(1,4,6); Trans(2,4,6); Trans(3,4,6)]; 35 | % Find the Jacobian Matrix using the Position matrix of the End-Effector 36 | tempJacob = jacobian(endPoint, theta); 37 | 38 | % Create a simplified Jacobian matrix using 'matlabFunction' 39 | matlabFunction(tempJacob, 'File', 'PUMA_Jacob'); 40 | 41 | 42 | % Set the desired destination position of the end-effector 43 | dstPoint = [-1;3;-10]; 44 | 45 | % Set the values of the theta at present configuration 46 | th(1) = pi/2; th(2) = pi/3; th(3) = pi/6; 47 | 48 | % Calculate the initial transform matrix 49 | % Matrix containing Joint angle Information and Desired Point Location 50 | t1 = th(1); t2 = th(2); t3 = th(3); 51 | currPoint = [double(subs(Trans(1,4,6))); double(subs(Trans(2,4,6))); ... 52 | double(subs(Trans(3,4,6)))]; 53 | 54 | % Initialize the plot 55 | scatter3(0, 0, 0, '*'); 56 | hold on 57 | scatter3(currPoint(1), currPoint(2), currPoint(3)); 58 | scatter3(dstPoint(1), dstPoint(2), dstPoint(3)); 59 | title('PUMA 560 Manipulator Simulation') 60 | xlabel('X-Coordinate Axis') 61 | ylabel('Y-Coordinate Axis') 62 | zlabel('Z-Coordinate Axis') 63 | axis square 64 | hold on 65 | 66 | PUMA_InverseKinematics(Trans, theta, th, dstPoint) 67 | 68 | % Finalize the plot 69 | figure(1) 70 | scatter3(0, 0, 0, '*'); 71 | scatter3(currPoint(1), currPoint(2), currPoint(3)); 72 | scatter3(dstPoint(1), dstPoint(2), dstPoint(3)); 73 | title('PUMA 560 Manipulator Simulation') 74 | xlabel('X-Coordinate Axis') 75 | ylabel('Y-Coordinate Axis') 76 | zlabel('Z-Coordinate Axis') 77 | ``` 78 | ## Results 79 |
80 | 81 |
-------------------------------------------------------------------------------- /5_PUMA560_Robot_Simulation/html.7z: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YashBansod/Robotics-Planning-Dynamics-and-Control/ee8984dd5f090b803c87ac9fdf4f9be625787b2b/5_PUMA560_Robot_Simulation/html.7z -------------------------------------------------------------------------------- /5_PUMA560_Robot_Simulation/images/PUMA_main_01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YashBansod/Robotics-Planning-Dynamics-and-Control/ee8984dd5f090b803c87ac9fdf4f9be625787b2b/5_PUMA560_Robot_Simulation/images/PUMA_main_01.png -------------------------------------------------------------------------------- /5_PUMA560_Robot_Simulation/images/results_1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YashBansod/Robotics-Planning-Dynamics-and-Control/ee8984dd5f090b803c87ac9fdf4f9be625787b2b/5_PUMA560_Robot_Simulation/images/results_1.gif -------------------------------------------------------------------------------- /6_Spring_Mass_Damper_System_Control/springMassSystem1.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YashBansod/Robotics-Planning-Dynamics-and-Control/ee8984dd5f090b803c87ac9fdf4f9be625787b2b/6_Spring_Mass_Damper_System_Control/springMassSystem1.slx -------------------------------------------------------------------------------- /7_Dijkstra_Algorithm/README.md: -------------------------------------------------------------------------------- 1 | Author: Yash Bansod 2 | Date: 15th September 2017 3 | Brief: This demonstrates the Dijkstra path planning algorithm 4 | GitHub: https://github.com/YashBansod 5 | 6 | ## Initialize the maps, color maps, start points and destination points 7 | 8 | ```matlab 9 | input_map = false(10); % Create an Input Map 10 | input_map (3:9, 5:7) = 1; % Add an obstacle 11 | start_coords = [6, 1]; % Save the location of start coordinate 12 | dest_coords = [8, 9]; % Save location of destination coordinate 13 | cmap = [1 1 1; % Create a color map 14 | 0 0 0; 15 | 1 0 0; 16 | 0 0 1; 17 | 0 1 0; 18 | 1 1 0; 19 | 0.5 0.5 0.5]; 20 | colormap(cmap); % Sets the colormap for the current figure 21 | [nrows, ncols] = size(input_map); % Save the size of the input_map 22 | 23 | map = zeros(nrows,ncols); % Create map to save the states of each grid cell 24 | map(~input_map) = 1; % Mark free cells on map 25 | map(input_map) = 2; % Mark obstacle cells on map 26 | start_node = sub2ind(size(map), start_coords(1), start_coords(2)); % Generate linear indices of start node 27 | dest_node = sub2ind(size(map), dest_coords(1), dest_coords(2)); % Generate linear indices of dest node 28 | map(start_node) = 5; % Mark start node on map 29 | map(dest_node) = 6; % Mark destination node on map 30 | 31 | % Initialize distance from start array to inifinity 32 | distanceFromStart = Inf(nrows,ncols); 33 | 34 | % Create a map that holds the index of its parent for each grid cell 35 | parent = zeros(nrows, ncols); 36 | 37 | distanceFromStart(start_node) = 0; % distance of start node is zero 38 | 39 | image(1.5, 1.5, map); 40 | grid on; % Display grid lines 41 | axis image; % Set axis limits 42 | drawnow; % Update figure 43 | 44 | drawMapEveryTime = true; % To see how nodes expand on the grid 45 | ``` 46 | 47 |
48 | 49 | ## Process the map to update the parent information and distance from start 50 | 51 | ```matlab 52 | while true % Create an infinite loop 53 | map(start_node) = 5; % Mark start node on map 54 | map(dest_node) = 6; % Mark destination node on map 55 | 56 | if (drawMapEveryTime) 57 | image(1.5, 1.5, map); 58 | grid on; % Display grid lines 59 | axis image; % Set axis limits 60 | drawnow; % Update figure 61 | end 62 | 63 | % Find the node with the minimum distance 64 | [min_dist, current] = min(distanceFromStart(:)); 65 | % Compute row, column coordinates of current node from linear index 66 | [i, j] = ind2sub(size(distanceFromStart), current); 67 | 68 | % Create an exit condition for the infinite loop to end 69 | if ((current == dest_node) || isinf(min_dist)) break 70 | end 71 | 72 | % Update distance value of element right of current element 73 | if (i+1 <= nrows && distanceFromStart(i+1, j) > min_dist + 1) 74 | if (parent(i+1, j) == 0 && input_map(i+1,j)~=1 && parent(current)~= sub2ind(size(map), i+1, j)) 75 | distanceFromStart(i+1, j) = min_dist + 1; 76 | map(sub2ind(size(map), i+1, j)) = 4; % Mark the neighbour of current as processing 77 | parent(i+1, j)= current; 78 | end 79 | end 80 | 81 | % Update distance value of element left of current element 82 | if (i-1 >= 1 && distanceFromStart(i-1, j) > min_dist + 1) 83 | if (parent(i-1, j) == 0 && input_map(i-1,j)~=1 && parent(current)~= sub2ind(size(map), i-1, j)) 84 | distanceFromStart(i-1, j) = min_dist + 1; 85 | map(sub2ind(size(map), i-1, j)) = 4; % Mark the neighbour of current as processing 86 | parent(i-1, j)= current; 87 | end 88 | end 89 | 90 | % Update distance value of element top of current element 91 | if (j-1 >= 1 && distanceFromStart(i, j-1) > min_dist + 1) 92 | if (parent(i, j-1) == 0 && input_map(i,j-1)~=1 && parent(current)~= sub2ind(size(map), i, j-1)) 93 | distanceFromStart(i, j-1) = min_dist + 1; 94 | map(sub2ind(size(map), i, j-1)) = 4; % Mark the neighbour of current as processing 95 | parent(i, j-1)= current; 96 | end 97 | end 98 | 99 | % Update distance value of element bottom of current element 100 | if (j+1 <= ncols && distanceFromStart(i, j+1) > min_dist + 1) 101 | if (parent(i, j+1) == 0 && input_map(i,j+1)~=1 && parent(current)~= sub2ind(size(map), i, j+1)) 102 | distanceFromStart(i, j+1) = min_dist + 1; 103 | map(sub2ind(size(map), i, j+1)) = 4; % Mark the neighbour of current as processing 104 | parent(i, j+1)= current; 105 | end 106 | end 107 | 108 | distanceFromStart(current) = -log(0); % change the distance of current from start as infinity 109 | map(current) = 3; % mark the current point as processed 110 | end 111 | ``` 112 |
113 | 114 | ## Construct route from start to dest by following the parent links 115 | 116 | ```matlab 117 | if (isinf(distanceFromStart(dest_node))) route = []; % if distance to destination node is infinity 118 | else route = [dest_node]; % else backtrace the route from destination node 119 | while (parent(route(1)) ~= 0) % check front of route for start node 120 | route = [parent(route(1)), route]; % add parent of current node to front of route 121 | end 122 | 123 | for k = 2:length(route) - 1 % To visualize the map and the path 124 | map(route(k)) = 7; 125 | pause(0.001); % Pause the code for a while 126 | image(1.5, 1.5, map); 127 | grid on; % Display grid lines 128 | axis image; % Set axis lengths 129 | end 130 | end 131 | 132 | % Add legends to the colormapped image 133 | hold on; 134 | for K = 1:7 135 | hidden_h(K) = surf(zeros(2, 2), 'edgecolor', 'none', ... 136 | 'facecolor', cmap(K, :)); 137 | end 138 | hold off 139 | 140 | uistack(hidden_h, 'bottom'); 141 | legend(hidden_h, {'free space', 'obstacles', 'closed nodes', ... 142 | 'open nodes', 'start node', 'goal node', 'shortest path'} ) 143 | ``` 144 | 145 |
146 | 147 | ## Results 148 |
149 | -------------------------------------------------------------------------------- /7_Dijkstra_Algorithm/dijkstra.m: -------------------------------------------------------------------------------- 1 | %% File Information 2 | %! @author Yash Bansod 3 | %! @file dijkstra.m 4 | %! @date 15th September 2017 5 | %! @breif This demostrates the dijkstra path planning algorithm 6 | 7 | %% Initialize the maps, color maps, start points and destination points 8 | input_map = false(10); % Create an Input Map 9 | input_map (3:9, 5:7) = 1; % Add an obstacle 10 | start_coords = [6, 1]; % Save the location of start coordinate 11 | dest_coords = [8, 9]; % Save location of destination coordinate 12 | cmap = [1 1 1; % Create a color map 13 | 0 0 0; 14 | 1 0 0; 15 | 0 0 1; 16 | 0 1 0; 17 | 1 1 0; 18 | 0.5 0.5 0.5]; 19 | colormap(cmap); % Sets the colormap for the current figure 20 | [nrows, ncols] = size(input_map); % Save the size of the input_map 21 | 22 | map = zeros(nrows,ncols); % Create map to save the states of each grid cell 23 | map(~input_map) = 1; % Mark free cells on map 24 | map(input_map) = 2; % Mark obstacle cells on map 25 | start_node = sub2ind(size(map), start_coords(1), start_coords(2)); % Generate linear indices of start node 26 | dest_node = sub2ind(size(map), dest_coords(1), dest_coords(2)); % Generate linear indices of dest node 27 | map(start_node) = 5; % Mark start node on map 28 | map(dest_node) = 6; % Mark destination node on map 29 | 30 | % Initialize distance from start array to inifinity 31 | distanceFromStart = Inf(nrows,ncols); 32 | 33 | % Create a map that holds the index of its parent for each grid cell 34 | parent = zeros(nrows, ncols); 35 | 36 | distanceFromStart(start_node) = 0; % distance of start node is zero 37 | 38 | image(1.5, 1.5, map); 39 | grid on; % Display grid lines 40 | axis image; % Set axis limits 41 | drawnow; % Update figure 42 | 43 | drawMapEveryTime = true; % To see how nodes expand on the grid 44 | 45 | %% Process the map to update the parent information and distance from start 46 | while true % Create an infinite loop 47 | map(start_node) = 5; % Mark start node on map 48 | map(dest_node) = 6; % Mark destination node on map 49 | 50 | if (drawMapEveryTime) 51 | image(1.5, 1.5, map); 52 | grid on; % Display grid lines 53 | axis image; % Set axis limits 54 | drawnow; % Update figure 55 | end 56 | 57 | % Find the node with the minimum distance 58 | [min_dist, current] = min(distanceFromStart(:)); 59 | % Compute row, column coordinates of current node from linear index 60 | [i, j] = ind2sub(size(distanceFromStart), current); 61 | 62 | % Create an exit condition for the infinite loop to end 63 | if ((current == dest_node) || isinf(min_dist)) break 64 | end 65 | 66 | % Update distance value of element right of current element 67 | if (i+1 <= nrows && distanceFromStart(i+1, j) > min_dist + 1) 68 | if (parent(i+1, j) == 0 && input_map(i+1,j)~=1 && parent(current)~= sub2ind(size(map), i+1, j)) 69 | distanceFromStart(i+1, j) = min_dist + 1; 70 | map(sub2ind(size(map), i+1, j)) = 4; % Mark the neighbour of current as processing 71 | parent(i+1, j)= current; 72 | end 73 | end 74 | 75 | % Update distance value of element left of current element 76 | if (i-1 >= 1 && distanceFromStart(i-1, j) > min_dist + 1) 77 | if (parent(i-1, j) == 0 && input_map(i-1,j)~=1 && parent(current)~= sub2ind(size(map), i-1, j)) 78 | distanceFromStart(i-1, j) = min_dist + 1; 79 | map(sub2ind(size(map), i-1, j)) = 4; % Mark the neighbour of current as processing 80 | parent(i-1, j)= current; 81 | end 82 | end 83 | 84 | % Update distance value of element top of current element 85 | if (j-1 >= 1 && distanceFromStart(i, j-1) > min_dist + 1) 86 | if (parent(i, j-1) == 0 && input_map(i,j-1)~=1 && parent(current)~= sub2ind(size(map), i, j-1)) 87 | distanceFromStart(i, j-1) = min_dist + 1; 88 | map(sub2ind(size(map), i, j-1)) = 4; % Mark the neighbour of current as processing 89 | parent(i, j-1)= current; 90 | end 91 | end 92 | 93 | % Update distance value of element bottom of current element 94 | if (j+1 <= ncols && distanceFromStart(i, j+1) > min_dist + 1) 95 | if (parent(i, j+1) == 0 && input_map(i,j+1)~=1 && parent(current)~= sub2ind(size(map), i, j+1)) 96 | distanceFromStart(i, j+1) = min_dist + 1; 97 | map(sub2ind(size(map), i, j+1)) = 4; % Mark the neighbour of current as processing 98 | parent(i, j+1)= current; 99 | end 100 | end 101 | 102 | distanceFromStart(current) = -log(0); % change the distance of current from start as infinity 103 | map(current) = 3; % mark the current point as processed 104 | end 105 | 106 | %% Construct route from start to dest by following the parent links 107 | if (isinf(distanceFromStart(dest_node))) route = []; % if distance to destination node is infinity 108 | else route = [dest_node]; % else backtrace the route from destination node 109 | while (parent(route(1)) ~= 0) % check front of route for start node 110 | route = [parent(route(1)), route]; % add parent of current node to front of route 111 | end 112 | 113 | for k = 2:length(route) - 1 % To visualize the map and the path 114 | map(route(k)) = 7; 115 | pause(0.001); % Pause the code for a while 116 | image(1.5, 1.5, map); 117 | grid on; % Display grid lines 118 | axis image; % Set axis lengths 119 | end 120 | end 121 | 122 | % Add legends to the colormapped image 123 | hold on; 124 | for K = 1:7 125 | hidden_h(K) = surf(zeros(2, 2), 'edgecolor', 'none', ... 126 | 'facecolor', cmap(K, :)); 127 | end 128 | hold off 129 | 130 | uistack(hidden_h, 'bottom'); 131 | legend(hidden_h, {'free space', 'obstacles', 'closed nodes', ... 132 | 'open nodes', 'start node', 'goal node', 'shortest path'} ) -------------------------------------------------------------------------------- /7_Dijkstra_Algorithm/html.7z: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YashBansod/Robotics-Planning-Dynamics-and-Control/ee8984dd5f090b803c87ac9fdf4f9be625787b2b/7_Dijkstra_Algorithm/html.7z -------------------------------------------------------------------------------- /7_Dijkstra_Algorithm/images/dijkstra_01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YashBansod/Robotics-Planning-Dynamics-and-Control/ee8984dd5f090b803c87ac9fdf4f9be625787b2b/7_Dijkstra_Algorithm/images/dijkstra_01.png -------------------------------------------------------------------------------- /7_Dijkstra_Algorithm/images/dijkstra_02.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YashBansod/Robotics-Planning-Dynamics-and-Control/ee8984dd5f090b803c87ac9fdf4f9be625787b2b/7_Dijkstra_Algorithm/images/dijkstra_02.png -------------------------------------------------------------------------------- /7_Dijkstra_Algorithm/images/dijkstra_03.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YashBansod/Robotics-Planning-Dynamics-and-Control/ee8984dd5f090b803c87ac9fdf4f9be625787b2b/7_Dijkstra_Algorithm/images/dijkstra_03.png -------------------------------------------------------------------------------- /7_Dijkstra_Algorithm/images/results_1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YashBansod/Robotics-Planning-Dynamics-and-Control/ee8984dd5f090b803c87ac9fdf4f9be625787b2b/7_Dijkstra_Algorithm/images/results_1.gif -------------------------------------------------------------------------------- /8_Astar_Algorithm/Astar.m: -------------------------------------------------------------------------------- 1 | %% 2 | % Author: Yash Bansod 3 | % 4 | % GitHub: 5 | 6 | %% Clear the environment and the command line 7 | clear; 8 | close all; 9 | clc; 10 | 11 | %% Initialize the maps, color maps, start points and destination points 12 | map_size = [20, 20]; 13 | input_map = false(map_size); % Create an Input Map 14 | 15 | input_map (6:7, 8:9) = 1; % Add an obstacle 16 | input_map (6:9, 12) = 1; % Add another obstacle 17 | input_map (12, 5:10) = 1; % Add another obstacle 18 | input_map (13:14, 13:16) = 1; % Add another obstacle 19 | input_map (16:18, 6:8) = 1; % Add another obstacle 20 | input_map (4:6, 17:18) = 1; % Add another obstacle 21 | 22 | start_coords = [3, 6]; % Save the location of start coordinate 23 | dest_coords = [18, 15]; % Save location of destination coordinate 24 | 25 | drawMapEveryTime = true; % To see how nodes expand on the grid 26 | 27 | cmap = [1 1 1; % Create a color map 28 | 0 0 0; 29 | 1 0 0; 30 | 0 0 1; 31 | 0 1 0; 32 | 1 1 0; 33 | 0.5 0.5 0.5]; 34 | 35 | colormap(cmap); % Sets the colormap for the current figure 36 | [nrows, ncols] = size(input_map); % Save the size of the input_map 37 | 38 | map = zeros(nrows,ncols); % Create map to save the states of each grid cell 39 | map(~input_map) = 1; % Mark free cells on map 40 | map(input_map) = 2; % Mark obstacle cells on map 41 | 42 | start_node = sub2ind(size(map), start_coords(1), start_coords(2)); % Generate linear indices of start node 43 | dest_node = sub2ind(size(map), dest_coords(1), dest_coords(2)); % Generate linear indices of dest node 44 | 45 | map(start_node) = 5; % Mark start node on map 46 | map(dest_node) = 6; % Mark destination node on map 47 | 48 | distanceFromStart = Inf(nrows,ncols); % Initialize distance from start array to inifinity 49 | distanceFromEnd = Inf(nrows,ncols); % Initialize distance from end array to inifinity 50 | 51 | parent = zeros(nrows, ncols); % Create a map for holding parent's index for each grid cell 52 | 53 | distanceFromStart(start_node) = 0; % distance of start node from start is zero 54 | distanceFromEnd(dest_node) = 0; % distance of end node from end is zero 55 | 56 | % Update the values of all grid pixels for distance from end 57 | [X, Y] = meshgrid(1:ncols, 1:nrows); 58 | xd = dest_coords(1); 59 | yd = dest_coords(2); 60 | distanceFromEnd = abs(X - yd) + abs(Y - xd); % Manhattan Distance 61 | 62 | image([0.5, map_size(1)-0.5], [0.5, map_size(2)-0.5], map); 63 | ax = gca; 64 | ax.YTick = 0:1:map_size(1); 65 | ax.XTick = 0:1:map_size(2); 66 | grid on; % Display grid lines 67 | % drawnow limitrate nocallbacks; % Update figure 68 | drawnow; 69 | 70 | %% Process the map to update the parent information and distance from start 71 | while true % Create an infinite loop 72 | map(start_node) = 5; % Mark start node on map 73 | map(dest_node) = 6; % Mark destination node on map 74 | 75 | if (drawMapEveryTime) 76 | image([0.5, map_size(1)-0.5], [0.5, map_size(2)-0.5], map); 77 | ax = gca; 78 | ax.YTick = 0:1:map_size(1); 79 | ax.XTick = 0:1:map_size(2); 80 | grid on; % Display grid lines 81 | % drawnow limitrate nocallbacks; % Update figure 82 | drawnow; 83 | end 84 | 85 | % Find the node with the minimum heuristic distance 86 | heuristicDist = distanceFromStart + distanceFromEnd; 87 | [min_dist, current] = min(heuristicDist(:)); 88 | 89 | % Compute row, column coordinates of current node from linear index 90 | [i, j] = ind2sub(size(heuristicDist), current); 91 | 92 | % Create an exit condition for the infinite loop to end 93 | if ((current == dest_node) || isinf(min_dist)) 94 | break 95 | end 96 | 97 | % Update distance value of element right of current element 98 | if (i+1 <= nrows && distanceFromStart(i+1, j) > distanceFromStart(i,j) + 1) 99 | if (parent(i+1, j) == 0 && input_map(i+1,j)~=1 && parent(current)~= sub2ind(size(map), i+1, j)) 100 | distanceFromStart(i+1, j) = distanceFromStart(i,j) + 1; 101 | map(sub2ind(size(map), i+1, j)) = 4; % Mark the neighbour of current as processing 102 | parent(i+1, j)= current; 103 | end 104 | end 105 | 106 | % Update distance value of element left of current element 107 | if (i-1 >= 1 && distanceFromStart(i-1, j) > distanceFromStart(i,j) + 1) 108 | if (parent(i-1, j) == 0 && input_map(i-1,j)~=1 && parent(current)~= sub2ind(size(map), i-1, j)) 109 | distanceFromStart(i-1, j) = distanceFromStart(i,j) + 1; 110 | map(sub2ind(size(map), i-1, j)) = 4; % Mark the neighbour of current as processing 111 | parent(i-1, j)= current; 112 | end 113 | end 114 | 115 | % Update distance value of element top of current element 116 | if (j-1 >= 1 && distanceFromStart(i, j-1) > distanceFromStart(i,j) + 1) 117 | if (parent(i, j-1) == 0 && input_map(i,j-1)~=1 && parent(current)~= sub2ind(size(map), i, j-1)) 118 | distanceFromStart(i, j-1) = distanceFromStart(i,j) + 1; 119 | map(sub2ind(size(map), i, j-1)) = 4; % Mark the neighbour of current as processing 120 | parent(i, j-1)= current; 121 | end 122 | end 123 | 124 | % Update distance value of element bottom of current element 125 | if (j+1 <= ncols && distanceFromStart(i, j+1) > distanceFromStart(i,j) + 1) 126 | if (parent(i, j+1) == 0 && input_map(i,j+1)~=1 && parent(current)~= sub2ind(size(map), i, j+1)) 127 | distanceFromStart(i, j+1) = distanceFromStart(i,j) + 1; 128 | map(sub2ind(size(map), i, j+1)) = 4; % Mark the neighbour of current as processing 129 | parent(i, j+1)= current; 130 | end 131 | end 132 | 133 | distanceFromStart(current) = -log(0); % change the distance of current from start as infinity 134 | map(current) = 3; % mark the current point as processed 135 | end 136 | 137 | %% Construct route from start to dest by following the parent links 138 | if (isinf(distanceFromStart(dest_node))) 139 | route = []; % if distance to destination node is infinity 140 | else 141 | route = [dest_node]; % else backtrace the route from destination node 142 | while (parent(route(1)) ~= 0) % check front of route for start node 143 | route = [parent(route(1)), route]; % add parent of current node to front of route 144 | end 145 | 146 | for k = 2:length(route) - 1 % To visualize the map and the path 147 | map(route(k)) = 7; 148 | pause(0.001); % Pause the code for a while 149 | image([0.5, map_size(1)-0.5], [0.5, map_size(2)-0.5], map); 150 | ax = gca; 151 | ax.YTick = 0:1:map_size(1); 152 | ax.XTick = 0:1:map_size(2); 153 | grid on; % Display grid lines 154 | % drawnow limitrate nocallbacks; % Update figure 155 | drawnow; 156 | end 157 | end 158 | 159 | % Add legends to the colormapped image 160 | hold on; 161 | for K = 1:7 162 | hidden_h(K) = surf(zeros(2, 2), 'edgecolor', 'none', ... 163 | 'facecolor', cmap(K, :)); 164 | end 165 | hold off 166 | 167 | uistack(hidden_h, 'bottom'); 168 | legend(hidden_h, {'free space', 'obstacles', 'closed nodes', ... 169 | 'open nodes', 'start node', 'goal node', 'shortest path'} ) 170 | 171 | -------------------------------------------------------------------------------- /8_Astar_Algorithm/README.md: -------------------------------------------------------------------------------- 1 | Author: Yash Bansod 2 | GitHub: https://github.com/YashBansod 3 | 4 | ## Clear the environment and the command line 5 | 6 | ```matlab 7 | clear; 8 | close all; 9 | clc; 10 | ``` 11 | 12 | ## Initialize the maps, color maps, start points and destination points 13 | 14 | ```matlab 15 | map_size = [20, 20]; 16 | input_map = false(map_size); % Create an Input Map 17 | 18 | input_map (6:7, 8:9) = 1; % Add an obstacle 19 | input_map (6:9, 12) = 1; % Add another obstacle 20 | input_map (12, 5:10) = 1; % Add another obstacle 21 | input_map (13:14, 13:16) = 1; % Add another obstacle 22 | input_map (16:18, 6:8) = 1; % Add another obstacle 23 | input_map (4:6, 17:18) = 1; % Add another obstacle 24 | 25 | start_coords = [3, 6]; % Save the location of start coordinate 26 | dest_coords = [18, 15]; % Save location of destination coordinate 27 | 28 | drawMapEveryTime = true; % To see how nodes expand on the grid 29 | 30 | cmap = [1 1 1; % Create a color map 31 | 0 0 0; 32 | 1 0 0; 33 | 0 0 1; 34 | 0 1 0; 35 | 1 1 0; 36 | 0.5 0.5 0.5]; 37 | 38 | colormap(cmap); % Sets the colormap for the current figure 39 | [nrows, ncols] = size(input_map); % Save the size of the input_map 40 | 41 | map = zeros(nrows,ncols); % Create map to save the states of each grid cell 42 | map(~input_map) = 1; % Mark free cells on map 43 | map(input_map) = 2; % Mark obstacle cells on map 44 | 45 | start_node = sub2ind(size(map), start_coords(1), start_coords(2)); % Generate linear indices of start node 46 | dest_node = sub2ind(size(map), dest_coords(1), dest_coords(2)); % Generate linear indices of dest node 47 | 48 | map(start_node) = 5; % Mark start node on map 49 | map(dest_node) = 6; % Mark destination node on map 50 | 51 | distanceFromStart = Inf(nrows,ncols); % Initialize distance from start array to inifinity 52 | distanceFromEnd = Inf(nrows,ncols); % Initialize distance from end array to inifinity 53 | 54 | parent = zeros(nrows, ncols); % Create a map for holding parent's index for each grid cell 55 | 56 | distanceFromStart(start_node) = 0; % distance of start node from start is zero 57 | distanceFromEnd(dest_node) = 0; % distance of end node from end is zero 58 | 59 | % Update the values of all grid pixels for distance from end 60 | [X, Y] = meshgrid(1:ncols, 1:nrows); 61 | xd = dest_coords(1); 62 | yd = dest_coords(2); 63 | distanceFromEnd = abs(X - yd) + abs(Y - xd); % Manhattan Distance 64 | 65 | image([0.5, map_size(1)-0.5], [0.5, map_size(2)-0.5], map); 66 | ax = gca; 67 | ax.YTick = 0:1:map_size(1); 68 | ax.XTick = 0:1:map_size(2); 69 | grid on; % Display grid lines 70 | % drawnow limitrate nocallbacks; % Update figure 71 | drawnow; 72 | ``` 73 | 74 |
75 | 76 | ## Process the map to update the parent information and distance from start 77 | 78 | ```matlab 79 | while true % Create an infinite loop 80 | map(start_node) = 5; % Mark start node on map 81 | map(dest_node) = 6; % Mark destination node on map 82 | 83 | if (drawMapEveryTime) 84 | image([0.5, map_size(1)-0.5], [0.5, map_size(2)-0.5], map); 85 | ax = gca; 86 | ax.YTick = 0:1:map_size(1); 87 | ax.XTick = 0:1:map_size(2); 88 | grid on; % Display grid lines 89 | % drawnow limitrate nocallbacks; % Update figure 90 | drawnow; 91 | end 92 | 93 | % Find the node with the minimum heuristic distance 94 | heuristicDist = distanceFromStart + distanceFromEnd; 95 | [min_dist, current] = min(heuristicDist(:)); 96 | 97 | % Compute row, column coordinates of current node from linear index 98 | [i, j] = ind2sub(size(heuristicDist), current); 99 | 100 | % Create an exit condition for the infinite loop to end 101 | if ((current == dest_node) || isinf(min_dist)) 102 | break 103 | end 104 | 105 | % Update distance value of element right of current element 106 | if (i+1 <= nrows && distanceFromStart(i+1, j) > distanceFromStart(i,j) + 1) 107 | if (parent(i+1, j) == 0 && input_map(i+1,j)~=1 && parent(current)~= sub2ind(size(map), i+1, j)) 108 | distanceFromStart(i+1, j) = distanceFromStart(i,j) + 1; 109 | map(sub2ind(size(map), i+1, j)) = 4; % Mark the neighbour of current as processing 110 | parent(i+1, j)= current; 111 | end 112 | end 113 | 114 | % Update distance value of element left of current element 115 | if (i-1 >= 1 && distanceFromStart(i-1, j) > distanceFromStart(i,j) + 1) 116 | if (parent(i-1, j) == 0 && input_map(i-1,j)~=1 && parent(current)~= sub2ind(size(map), i-1, j)) 117 | distanceFromStart(i-1, j) = distanceFromStart(i,j) + 1; 118 | map(sub2ind(size(map), i-1, j)) = 4; % Mark the neighbour of current as processing 119 | parent(i-1, j)= current; 120 | end 121 | end 122 | 123 | % Update distance value of element top of current element 124 | if (j-1 >= 1 && distanceFromStart(i, j-1) > distanceFromStart(i,j) + 1) 125 | if (parent(i, j-1) == 0 && input_map(i,j-1)~=1 && parent(current)~= sub2ind(size(map), i, j-1)) 126 | distanceFromStart(i, j-1) = distanceFromStart(i,j) + 1; 127 | map(sub2ind(size(map), i, j-1)) = 4; % Mark the neighbour of current as processing 128 | parent(i, j-1)= current; 129 | end 130 | end 131 | 132 | % Update distance value of element bottom of current element 133 | if (j+1 <= ncols && distanceFromStart(i, j+1) > distanceFromStart(i,j) + 1) 134 | if (parent(i, j+1) == 0 && input_map(i,j+1)~=1 && parent(current)~= sub2ind(size(map), i, j+1)) 135 | distanceFromStart(i, j+1) = distanceFromStart(i,j) + 1; 136 | map(sub2ind(size(map), i, j+1)) = 4; % Mark the neighbour of current as processing 137 | parent(i, j+1)= current; 138 | end 139 | end 140 | 141 | distanceFromStart(current) = -log(0); % change the distance of current from start as infinity 142 | map(current) = 3; % mark the current point as processed 143 | end 144 | ``` 145 | 146 |
147 | 148 | ## Construct route from start to dest by following the parent links 149 | 150 | ```matlab 151 | if (isinf(distanceFromStart(dest_node))) 152 | route = []; % if distance to destination node is infinity 153 | else 154 | route = [dest_node]; % else backtrace the route from destination node 155 | while (parent(route(1)) ~= 0) % check front of route for start node 156 | route = [parent(route(1)), route]; % add parent of current node to front of route 157 | end 158 | 159 | for k = 2:length(route) - 1 % To visualize the map and the path 160 | map(route(k)) = 7; 161 | pause(0.001); % Pause the code for a while 162 | image([0.5, map_size(1)-0.5], [0.5, map_size(2)-0.5], map); 163 | ax = gca; 164 | ax.YTick = 0:1:map_size(1); 165 | ax.XTick = 0:1:map_size(2); 166 | grid on; % Display grid lines 167 | % drawnow limitrate nocallbacks; % Update figure 168 | drawnow; 169 | end 170 | end 171 | 172 | % Add legends to the colormapped image 173 | hold on; 174 | for K = 1:7 175 | hidden_h(K) = surf(zeros(2, 2), 'edgecolor', 'none', ... 176 | 'facecolor', cmap(K, :)); 177 | end 178 | hold off 179 | 180 | uistack(hidden_h, 'bottom'); 181 | legend(hidden_h, {'free space', 'obstacles', 'closed nodes', ... 182 | 'open nodes', 'start node', 'goal node', 'shortest path'} ) 183 | ``` 184 | 185 |
186 | 187 | ## Results 188 |
189 | -------------------------------------------------------------------------------- /8_Astar_Algorithm/html.7z: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YashBansod/Robotics-Planning-Dynamics-and-Control/ee8984dd5f090b803c87ac9fdf4f9be625787b2b/8_Astar_Algorithm/html.7z -------------------------------------------------------------------------------- /8_Astar_Algorithm/images/Astar_01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YashBansod/Robotics-Planning-Dynamics-and-Control/ee8984dd5f090b803c87ac9fdf4f9be625787b2b/8_Astar_Algorithm/images/Astar_01.png -------------------------------------------------------------------------------- /8_Astar_Algorithm/images/Astar_02.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YashBansod/Robotics-Planning-Dynamics-and-Control/ee8984dd5f090b803c87ac9fdf4f9be625787b2b/8_Astar_Algorithm/images/Astar_02.png -------------------------------------------------------------------------------- /8_Astar_Algorithm/images/Astar_03.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YashBansod/Robotics-Planning-Dynamics-and-Control/ee8984dd5f090b803c87ac9fdf4f9be625787b2b/8_Astar_Algorithm/images/Astar_03.png -------------------------------------------------------------------------------- /8_Astar_Algorithm/images/results_1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YashBansod/Robotics-Planning-Dynamics-and-Control/ee8984dd5f090b803c87ac9fdf4f9be625787b2b/8_Astar_Algorithm/images/results_1.gif -------------------------------------------------------------------------------- /Aux1_Rotation_Matrices/README.md: -------------------------------------------------------------------------------- 1 | Author: Yash Bansod 2 | Date: 30th January, 2020 3 | GitHub: https://github.com/YashBansod 4 | 5 | 6 | 7 | #### Clear the environment and the command line 8 | 9 | ```matlab 10 | clear; 11 | clc; 12 | ``` 13 | 14 | 15 | 16 | #### Define some initial parameters 17 | 18 | ```matlab 19 | % Define the origin coordinates 20 | o_x = 0; 21 | o_y = 0; 22 | o_z = 0; 23 | 24 | % Define the rotation across various axes 25 | theta_x = 30; 26 | theta_y = 30; 27 | theta_z = 30; 28 | ``` 29 | 30 | 31 | 32 | #### Plot the Reference frame 33 | 34 | ```matlab 35 | % Plot X axis 36 | X = plot3([o_x, o_x + 1], [o_y, o_y], [o_z, o_z], '-', 'color', 'red'); 37 | grid on; 38 | hold on; 39 | 40 | % Plot Y axis 41 | Y = plot3([o_x, o_x], [o_y, o_y + 1], [o_z, o_z], '-', 'color', 'green'); 42 | 43 | % Plot Z axis 44 | Z = plot3([o_x, o_x], [o_y, o_y], [o_z, o_z + 1], '-', 'color', 'blue'); 45 | 46 | % Add text tags to the ends of the axes 47 | text_x = text(o_x + 1, o_y, o_z, 'X-Axis', 'color', 'red'); 48 | text_y = text(o_x, o_y + 1, o_z, 'Y-Axis', 'color', 'green'); 49 | text_z = text(o_x, o_y, o_z + 1, 'Z-Axis', 'color', 'blue'); 50 | ``` 51 | 52 |
Reference Frame
53 | 54 | 55 | 56 | #### Calculate the rotation matrix 57 | 58 | ```matlab 59 | % Counter-Clockwise Rotation across X axis 60 | Rx = [ 1, 0 , 0; 61 | 0, cosd(theta_x), -sind(theta_x); 62 | 0, sind(theta_x), cosd(theta_x)]; 63 | 64 | % Couter-Clockwise Rotation across Y axis 65 | Ry = [ cosd(theta_y), 0, -sind(theta_y); 66 | 0, 1, 0; 67 | sind(theta_y), 0, cosd(theta_y)]; 68 | 69 | % Couter-Clockwise Rotation across Z axis 70 | Rz = [ cosd(theta_z), -sind(theta_z), 0; 71 | sind(theta_z), cosd(theta_z), 0; 72 | 0, 0, 1]; 73 | ``` 74 | 75 | 76 | 77 | #### Calculate Overall Rotation matrix 78 | 79 | ```matlab 80 | % Rotation Matrix 1 = Rotation across X followed by Y followed by Z axis 81 | R_1 = Rz * Ry * Rx; 82 | 83 | % Rotation Matrix 2 = Rotation across Z followed by Y followed by X axis 84 | R_2 = Rx * Ry * Rz; 85 | ``` 86 | 87 | 88 | 89 | #### Plot the Frame after applying Rotation Matrix 1 to Reference Frame 90 | 91 | ```matlab 92 | X_1 = plot3([o_x, o_x + R_1(1, 1)], [o_y, o_y + R_1(2, 1)], [o_z, o_z + R_1(3, 1)], '--', 'color', 'red'); 93 | Y_1 = plot3([o_x, o_x + R_1(1, 2)], [o_y, o_y + R_1(2, 2)], [o_z, o_z + R_1(3, 2)], '--', 'color', 'green'); 94 | Z_1 = plot3([o_x, o_x + R_1(1, 3)], [o_y, o_y + R_1(2, 3)], [o_z, o_z + R_1(3, 3)], '--', 'color', 'blue'); 95 | 96 | text_x_1 = text(o_x + R_1(1, 1), o_y + R_1(2, 1), o_z + R_1(3, 1), 'X1-Axis', 'color', 'red'); 97 | text_y_1 = text(o_x + R_1(1, 2), o_y + R_1(2, 2), o_z + R_1(3, 2), 'Y1-Axis', 'color', 'green'); 98 | text_z_1 = text(o_x + R_1(1, 3), o_y + R_1(2, 3), o_z + R_1(3, 3), 'Z1-Axis', 'color', 'blue'); 99 | ``` 100 | 101 |
Reference Frame
102 | 103 | #### Plot the Frame after applying Rotation Matrix 2 to Reference Frame 104 | 105 | ```matlab 106 | X_2 = plot3([o_x, o_x + R_2(1, 1)], [o_y, o_y + R_2(2, 1)], [o_z, o_z + R_2(3, 1)], '-.', 'color', 'red'); 107 | Y_2 = plot3([o_x, o_x + R_2(1, 2)], [o_y, o_y + R_2(2, 2)], [o_z, o_z + R_2(3, 2)], '-.', 'color', 'green'); 108 | Z_2 = plot3([o_x, o_x + R_2(1, 3)], [o_y, o_y + R_2(2, 3)], [o_z, o_z + R_2(3, 3)], '-.', 'color', 'blue'); 109 | 110 | text_x_2 = text(o_x + R_2(1, 1), o_y + R_2(2, 1), o_z + R_2(3, 1), 'X2-Axis', 'color', 'red'); 111 | text_y_2 = text(o_x + R_2(1, 2), o_y + R_2(2, 2), o_z + R_2(3, 2), 'Y2-Axis', 'color', 'green'); 112 | text_z_2 = text(o_x + R_2(1, 3), o_y + R_2(2, 3), o_z + R_2(3, 3), 'Z2-Axis', 'color', 'blue'); 113 | ``` 114 | 115 |
Reference Frame
116 | -------------------------------------------------------------------------------- /Aux1_Rotation_Matrices/html.7z: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YashBansod/Robotics-Planning-Dynamics-and-Control/ee8984dd5f090b803c87ac9fdf4f9be625787b2b/Aux1_Rotation_Matrices/html.7z -------------------------------------------------------------------------------- /Aux1_Rotation_Matrices/images/rotation_matrices_01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YashBansod/Robotics-Planning-Dynamics-and-Control/ee8984dd5f090b803c87ac9fdf4f9be625787b2b/Aux1_Rotation_Matrices/images/rotation_matrices_01.png -------------------------------------------------------------------------------- /Aux1_Rotation_Matrices/images/rotation_matrices_02.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YashBansod/Robotics-Planning-Dynamics-and-Control/ee8984dd5f090b803c87ac9fdf4f9be625787b2b/Aux1_Rotation_Matrices/images/rotation_matrices_02.png -------------------------------------------------------------------------------- /Aux1_Rotation_Matrices/images/rotation_matrices_03.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YashBansod/Robotics-Planning-Dynamics-and-Control/ee8984dd5f090b803c87ac9fdf4f9be625787b2b/Aux1_Rotation_Matrices/images/rotation_matrices_03.png -------------------------------------------------------------------------------- /Aux1_Rotation_Matrices/rotation_matrices.m: -------------------------------------------------------------------------------- 1 | %% 2 | % Author: Yash Bansod 3 | % Date: 30th January, 2020 4 | % 5 | % GitHub: 6 | 7 | %% Clear the environment and the command line 8 | clear; 9 | clc; 10 | 11 | %% Define some initial parameters 12 | 13 | % Define the origin coordinates 14 | o_x = 0; 15 | o_y = 0; 16 | o_z = 0; 17 | 18 | % Define the rotation across various axes 19 | theta_x = 30; 20 | theta_y = 30; 21 | theta_z = 30; 22 | 23 | %% Plot the Reference frame 24 | 25 | % Plot X axis 26 | X = plot3([o_x, o_x + 1], [o_y, o_y], [o_z, o_z], '-', 'color', 'red'); 27 | grid on; 28 | hold on; 29 | 30 | % Plot Y axis 31 | Y = plot3([o_x, o_x], [o_y, o_y + 1], [o_z, o_z], '-', 'color', 'green'); 32 | 33 | % Plot Z axis 34 | Z = plot3([o_x, o_x], [o_y, o_y], [o_z, o_z + 1], '-', 'color', 'blue'); 35 | 36 | % Add text tags to the ends of the axes 37 | text_x = text(o_x + 1, o_y, o_z, 'X-Axis', 'color', 'red'); 38 | text_y = text(o_x, o_y + 1, o_z, 'Y-Axis', 'color', 'green'); 39 | text_z = text(o_x, o_y, o_z + 1, 'Z-Axis', 'color', 'blue'); 40 | 41 | %% Calculate the rotation matrix 42 | 43 | % Counter-Clockwise Rotation across X axis 44 | Rx = [ 1, 0 , 0; 45 | 0, cosd(theta_x), -sind(theta_x); 46 | 0, sind(theta_x), cosd(theta_x)]; 47 | 48 | % Couter-Clockwise Rotation across Y axis 49 | Ry = [ cosd(theta_y), 0, -sind(theta_y); 50 | 0, 1, 0; 51 | sind(theta_y), 0, cosd(theta_y)]; 52 | 53 | % Couter-Clockwise Rotation across Z axis 54 | Rz = [ cosd(theta_z), -sind(theta_z), 0; 55 | sind(theta_z), cosd(theta_z), 0; 56 | 0, 0, 1]; 57 | 58 | %% Calculate Overall Rotation matrix 59 | 60 | % Rotation Matrix 1 = Rotation across X followed by Y followed by Z axis 61 | R_1 = Rz * Ry * Rx; 62 | 63 | % Rotation Matrix 2 = Rotation across Z followed by Y followed by X axis 64 | R_2 = Rx * Ry * Rz; 65 | 66 | %% Plot the Frame after applying Rotation Matrix 1 to Reference Frame 67 | X_1 = plot3([o_x, o_x + R_1(1, 1)], [o_y, o_y + R_1(2, 1)], [o_z, o_z + R_1(3, 1)], '--', 'color', 'red'); 68 | Y_1 = plot3([o_x, o_x + R_1(1, 2)], [o_y, o_y + R_1(2, 2)], [o_z, o_z + R_1(3, 2)], '--', 'color', 'green'); 69 | Z_1 = plot3([o_x, o_x + R_1(1, 3)], [o_y, o_y + R_1(2, 3)], [o_z, o_z + R_1(3, 3)], '--', 'color', 'blue'); 70 | 71 | text_x_1 = text(o_x + R_1(1, 1), o_y + R_1(2, 1), o_z + R_1(3, 1), 'X1-Axis', 'color', 'red'); 72 | text_y_1 = text(o_x + R_1(1, 2), o_y + R_1(2, 2), o_z + R_1(3, 2), 'Y1-Axis', 'color', 'green'); 73 | text_z_1 = text(o_x + R_1(1, 3), o_y + R_1(2, 3), o_z + R_1(3, 3), 'Z1-Axis', 'color', 'blue'); 74 | 75 | %% Plot the Frame after applying Rotation Matrix 2 to Reference Frame 76 | X_2 = plot3([o_x, o_x + R_2(1, 1)], [o_y, o_y + R_2(2, 1)], [o_z, o_z + R_2(3, 1)], '-.', 'color', 'red'); 77 | Y_2 = plot3([o_x, o_x + R_2(1, 2)], [o_y, o_y + R_2(2, 2)], [o_z, o_z + R_2(3, 2)], '-.', 'color', 'green'); 78 | Z_2 = plot3([o_x, o_x + R_2(1, 3)], [o_y, o_y + R_2(2, 3)], [o_z, o_z + R_2(3, 3)], '-.', 'color', 'blue'); 79 | 80 | text_x_2 = text(o_x + R_2(1, 1), o_y + R_2(2, 1), o_z + R_2(3, 1), 'X2-Axis', 'color', 'red'); 81 | text_y_2 = text(o_x + R_2(1, 2), o_y + R_2(2, 2), o_z + R_2(3, 2), 'Y2-Axis', 'color', 'green'); 82 | text_z_2 = text(o_x + R_2(1, 3), o_y + R_2(2, 3), o_z + R_2(3, 3), 'Z2-Axis', 'color', 'blue'); 83 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 Yash Bansod 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # [RPDC : Robotics-Planning-Dynamics-and-Control](https://github.com/YashBansod/Robotics-Planning-Dynamics-and-Control) 2 | RPDC : This contains all my MATLAB codes for the Robotics, Planning, Dynamics and Control . The implementations model various kinds of manipulators and mobile robots for position control, trajectory planning and path planning problems. 3 | 4 | GitHub Page: https://yashbansod.github.io/projects/Robotics-Planning-Dynamics-and-Control/ 5 | 6 | 7 | 8 | ## Project Contents: 9 | 10 | The following are summarizing results from each of the sub-projects in this repository. 11 | 12 |
13 | 14 | 15 | 19 | 23 | 24 | 25 | 29 | 33 | 34 | 35 | 39 | 42 | 43 | 44 | 48 | 52 | 53 | 54 | 58 | 60 | 61 |
16 | 1_Robot_Manipulator_and_Coordinates 17 |
18 | 		20 | 2_2DOF_Manipulator_Inverse_Kinematics 21 |
22 |
26 | 3_Inverse_Algebraic_Manipulator_Control 27 |
28 | 		30 | 4_Planar_3DOF_Manipulator_Trajectory 31 |
32 |
36 | 5_PUMA560_Robot_Simulation 37 |
38 | 		40 | 6_Spring_Mass_Damper_System_Control 41 |
45 | 7_Dijkstra_Algorithm 46 |
47 | 		49 | 8_Astar_Algorithm 50 |
51 |
55 | Aux1_Rotation_Matrices 56 |
57 | 		59 |
62 |
63 | 64 | 65 | --------------------------------------------------------------------------------