├── ArmMatrix_dist.m ├── Cart_Pole_System.mlx ├── EulerLagrangeDynamicModel_MovingBase.PNG ├── Inv_Dynamics_LE_CartPole.m ├── Live_Script_Snippet1.PNG ├── Live_Script_Snippet2.PNG ├── Live_Script_Snippet3.PNG ├── Live_Script_Snippet4.PNG ├── Live_Script_Snippet5.PNG ├── Live_Script_Snippet6.PNG ├── Live_Script_Snippet7.PNG ├── Live_Script_Snippet8.PNG ├── Pole_Joint_Angle.png ├── Pole_Joint_Torque.fig ├── Pole_Joint_Torque.png ├── README.md ├── T_Concat_dist.m ├── U1_matrix.m ├── U_matrix.m ├── cart_pole_system.PNG ├── cart_trajectory.png ├── centri_cor_base_matrix.m ├── centri_cor_force_matrix.m ├── finite_diff_scalar.m ├── finite_diff_vector.m ├── gravity_loading_matrix.m ├── inertia_acceleration_matrix.m ├── inertia_matrix_dist.m ├── inertial_force_base.m └── state_space_CartPole.m /ArmMatrix_dist.m: -------------------------------------------------------------------------------- 1 | %************************************************************************** 2 | %************** FUNCTION TO CALCULATE THE ARM MATRIX ********************** 3 | %************************************************************************** 4 | 5 | function [A] = ArmMatrix_dist(A,sim_time,theta,link_lengths,NJ) 6 | 7 | 8 | % INPUT VARIBLES 9 | %all alphas and linear joint variables are zero 10 | alpha = zeros(NJ,1); 11 | d = zeros(NJ,1); 12 | 13 | 14 | % alpha = zeros(NJ,1); % alpha(k) = Twist Angle (Angle b/w Z(k-1) & Z(k) along X(k)) 15 | % a = zeros(NJ,1); % a(k) = Link Length (Distance from Z(k-1) to Z(k) along X(k)) 16 | % theta = zeros(NJ,1); % theta(k) = Joint Angle (Angle b/w X(k-1) & X(k) along Z(k-1) 17 | % d = zeros(NJ,1); % d(k) = Joint Offset(Distance from X(k-1) to X(k) along Z(k-1) 18 | % flag = zeros(NJ,1); % flag(k) = Decision about the Joint Type (Rotational(flag=1) or Prismatic (flag=0)) 19 | % dQ_dt = zeros(NJ,1); % dQ_dt(k) = Actuator Velocity 20 | 21 | for ii = 1:length(sim_time(1:end)) 22 | a{ii} = zeros(NJ,1); 23 | for k = 1:NJ 24 | a{ii}(k) = link_lengths{ii}(k); 25 | 26 | A{ii}(:,:,k) = [ cos(theta{ii}(k)) -cos(alpha(k))*sin(theta{ii}(k)) sin(alpha(k))*sin(theta{ii}(k)) a{ii}(k)*cos(theta{ii}(k)); 27 | sin(theta{ii}(k)) cos(alpha(k))*cos(theta{ii}(k)) -sin(alpha(k))*cos(theta{ii}(k)) a{ii}(k)*sin(theta{ii}(k)); 28 | 0 sin(alpha(k)) cos(alpha(k)) d(k); 29 | 0 0 0 1 ]; 30 | end 31 | end 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /Cart_Pole_System.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/average-engineer/Moving_Base_Robotic_Manipulator_Dynamic_Model/612ca3adbad165a1cb1246db312182f53aa3b994/Cart_Pole_System.mlx -------------------------------------------------------------------------------- /EulerLagrangeDynamicModel_MovingBase.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/average-engineer/Moving_Base_Robotic_Manipulator_Dynamic_Model/612ca3adbad165a1cb1246db312182f53aa3b994/EulerLagrangeDynamicModel_MovingBase.PNG -------------------------------------------------------------------------------- /Inv_Dynamics_LE_CartPole.m: -------------------------------------------------------------------------------- 1 | function [joint_torque] = Inv_Dynamics_LE_CartPole 2 | 3 | %% 4 | %************************************************************************** 5 | %******************** INVERSE DYNAMICS of CART POLE SYSTEM **************** 6 | %************************************************************************** 7 | % WHERE 8 | % NJ = Number of Joints 9 | % NF = Number of Frames 10 | % DOF = Degree of Freedom of the Cartesian Space 11 | % flag = Decision about the Joint Type 12 | % For Rotary Joint (Flag=1), 13 | % For Prismatic Joint (Flag=0) 14 | %************************************************************************** 15 | 16 | 17 | %% 18 | % clear all 19 | % close all 20 | % clc 21 | %% GLOBAL VARIABLES 22 | global NJ link_lengths link_masses COM_prox pole_angle sim_time g 23 | global cart_x 24 | sim_time = linspace(0,10,1000); 25 | 26 | %acceleration due to gravity 27 | g = 9.81; 28 | 29 | %number of joints 30 | NJ = 1; 31 | 32 | for ii = 1:length(sim_time) 33 | link_lengths{ii} = 0.1; 34 | end 35 | 36 | %position of COM wrt proximal frames 37 | for i = 1:NJ 38 | COM_prox(i) = 0.5; 39 | end 40 | 41 | %position of link i COM wrt to ith frame (frame of distal joint of link i) (right) 42 | for ii = 1:length(sim_time) 43 | for i = 1:NJ 44 | r{ii}{i} = [-link_lengths{ii}(i)*(1 - COM_prox(i));0;0;1]; 45 | end 46 | end 47 | 48 | %pole angle 49 | %rad 50 | pole_angle = 0.005*sin(sim_time); 51 | 52 | %pole mass 53 | for i = 1:NJ 54 | link_masses(i) = 50; 55 | end 56 | 57 | %cart mass 58 | cart_mass = 1000; 59 | 60 | %cart (base) movement 61 | for ii = 1:length(sim_time) 62 | for i = 1:NJ 63 | cart_x(ii) = ((0.005*link_masses(i)*link_lengths{ii}(i))/(cart_mass + link_masses(i)))*sin(sim_time(ii)); 64 | end 65 | end 66 | 67 | %cart velocity 68 | cart_x_dot = finite_diff_scalar(sim_time,cart_x); 69 | 70 | %cart acceleration 71 | cart_x_dot_dot = finite_diff_scalar(sim_time,cart_x_dot); 72 | 73 | 74 | %pole angle speeds (rad/s) 75 | pole_angle_vel = finite_diff_scalar(sim_time,pole_angle); 76 | 77 | %pole angle accelerations (rad/s2) 78 | pole_angle_acc = finite_diff_scalar(sim_time,pole_angle_vel); 79 | 80 | %% DISTAL DH PARAMETERS 81 | %joint variables (according to distal DH parameters of human leg) 82 | %radians 83 | %according to distal DH parameters, there will be 3 joint variables 84 | for ii = 1:length(sim_time) 85 | theta{ii} = zeros(NJ,1); 86 | for i = 1:NJ 87 | theta{ii}(i) = (pi/2) - pole_angle(ii); %radians 88 | end 89 | end 90 | 91 | 92 | %joint velocities (1st order time derivative of joint variables) 93 | for ii = 1:length(sim_time) 94 | theta_dot{ii} = zeros(NJ,1); 95 | for i = 1:NJ 96 | theta_dot{ii}(i) = -pole_angle_vel(ii); %radians/second 97 | end 98 | end 99 | 100 | %joint accelerations (2nd order time derivatives of joint variables 101 | for ii = 1:length(sim_time) 102 | theta_dot{ii} = zeros(NJ,1); 103 | for i = 1:NJ 104 | theta_dot_dot{ii}(i) = -pole_angle_acc(ii); %radians/second^2 105 | end 106 | end 107 | 108 | %Acceleration due to gravity matrix 109 | gravity_acc = zeros(1,4); 110 | gravity_acc(2) = -g; 111 | 112 | %MEMORY ALLOCATION 113 | for i = 1:length(sim_time) 114 | A{i} = zeros(4,4,NJ);% arm matrix 115 | end 116 | 117 | %% FUNCTION CALLS 118 | % Calculate homogeneous tranforms of each frame 119 | [A] = ArmMatrix_dist(A,sim_time,theta,link_lengths,NJ); 120 | 121 | %transformation matrix of base frame (hip) wrt inertial global FOR 122 | %the base frame is considered to have the same orientation as the inertial 123 | %frame 124 | %the translation of the base frame is decribed by the hip trajectory in the 125 | %sagittal plane (X-Y Plane) 126 | for ii = 1:length(sim_time) 127 | T_b{ii} = [1,0,0,cart_x(ii); 128 | 0,1,0,0; 129 | 0,0,1,0; 130 | 0,0,0,1]; 131 | end 132 | 133 | %first time derivative of base transformation matrix 134 | T_b_dot = finite_diff_vector(sim_time,T_b); 135 | 136 | %second time derivative of base transformation matrix 137 | 138 | T_b_dot_dot = finite_diff_vector(sim_time,T_b_dot); 139 | 140 | %inertia matrix/tensor for each link wrt to its distal joint frame 141 | J = inertia_matrix_dist(link_lengths,link_masses,r,NJ,sim_time); 142 | 143 | %matrices for effect of the movement of one joint on other joint (Uij) 144 | U = U_matrix(A,sim_time,NJ); 145 | 146 | %matrices for effect of the movement of two joints on another joint (Uijk) 147 | U1 = U1_matrix(A,sim_time,NJ); 148 | 149 | 150 | %dynamic coeffient matrices 151 | %inertial acceleration based matrix 152 | D = inertia_acceleration_matrix(sim_time,U,J,NJ); 153 | %coroilis and centrifugal force matrix 154 | h = centri_cor_force_matrix(sim_time,U,U1,J,NJ,theta_dot); 155 | %gravity loading based matrix 156 | c = gravity_loading_matrix(sim_time,link_masses,gravity_acc,U,r,T_b,NJ); 157 | %inertial force due to base movement matrix 158 | p_f = inertial_force_base(sim_time,U,J,A,T_b,T_b_dot_dot,NJ); 159 | %centrifugal and coroilis force due to base movement matrix 160 | p_v = centri_cor_base_matrix(sim_time,U,J,T_b,T_b_dot,NJ); 161 | 162 | aa = 1; 163 | 164 | 165 | %generalized torque matrix at each time instant 166 | for ii = 1:length(sim_time) 167 | joint_torques{ii} = D{ii}*theta_dot_dot{ii} + h{ii} + c{ii} + p_v{ii}*theta_dot{ii} + p_f{ii}; 168 | joint_torque(ii) = -joint_torques{ii}; 169 | end 170 | 171 | end -------------------------------------------------------------------------------- /Live_Script_Snippet1.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/average-engineer/Moving_Base_Robotic_Manipulator_Dynamic_Model/612ca3adbad165a1cb1246db312182f53aa3b994/Live_Script_Snippet1.PNG -------------------------------------------------------------------------------- /Live_Script_Snippet2.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/average-engineer/Moving_Base_Robotic_Manipulator_Dynamic_Model/612ca3adbad165a1cb1246db312182f53aa3b994/Live_Script_Snippet2.PNG -------------------------------------------------------------------------------- /Live_Script_Snippet3.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/average-engineer/Moving_Base_Robotic_Manipulator_Dynamic_Model/612ca3adbad165a1cb1246db312182f53aa3b994/Live_Script_Snippet3.PNG -------------------------------------------------------------------------------- /Live_Script_Snippet4.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/average-engineer/Moving_Base_Robotic_Manipulator_Dynamic_Model/612ca3adbad165a1cb1246db312182f53aa3b994/Live_Script_Snippet4.PNG -------------------------------------------------------------------------------- /Live_Script_Snippet5.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/average-engineer/Moving_Base_Robotic_Manipulator_Dynamic_Model/612ca3adbad165a1cb1246db312182f53aa3b994/Live_Script_Snippet5.PNG -------------------------------------------------------------------------------- /Live_Script_Snippet6.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/average-engineer/Moving_Base_Robotic_Manipulator_Dynamic_Model/612ca3adbad165a1cb1246db312182f53aa3b994/Live_Script_Snippet6.PNG -------------------------------------------------------------------------------- /Live_Script_Snippet7.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/average-engineer/Moving_Base_Robotic_Manipulator_Dynamic_Model/612ca3adbad165a1cb1246db312182f53aa3b994/Live_Script_Snippet7.PNG -------------------------------------------------------------------------------- /Live_Script_Snippet8.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/average-engineer/Moving_Base_Robotic_Manipulator_Dynamic_Model/612ca3adbad165a1cb1246db312182f53aa3b994/Live_Script_Snippet8.PNG -------------------------------------------------------------------------------- /Pole_Joint_Angle.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/average-engineer/Moving_Base_Robotic_Manipulator_Dynamic_Model/612ca3adbad165a1cb1246db312182f53aa3b994/Pole_Joint_Angle.png -------------------------------------------------------------------------------- /Pole_Joint_Torque.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/average-engineer/Moving_Base_Robotic_Manipulator_Dynamic_Model/612ca3adbad165a1cb1246db312182f53aa3b994/Pole_Joint_Torque.fig -------------------------------------------------------------------------------- /Pole_Joint_Torque.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/average-engineer/Moving_Base_Robotic_Manipulator_Dynamic_Model/612ca3adbad165a1cb1246db312182f53aa3b994/Pole_Joint_Torque.png -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Moving_Base_Robotic_Manipulator_Dynamic_Model 2 | Euler Lagrange based dynamic model for a robotic manipulator on a moving base. 3 | This dynamic model has been developed on the basis of the journal C.M. Wronka and M.W. Dunnigan, "Derivation and analysis of a dynamic model of a robotic manipulator on a moving base", *Robotic and Autonomus Systems*, vol. 59, pp. 758-769, June. 2011, doi: 10.1016/j.robot.2011.05.010. 4 | 5 | The need to create this dynamic model came while I was trying to match the results of the Euler Lagrange formulation code with the Recursive Newton Euler formulation (RNEA) code for the case of human leg dynamics (single leg) where the hip joint is considered the base. While it is fairly simple and intuitive to including motion of the hip (base) in the RNEA code, it is extremely challenging in the Euler Lagrange one, since the formulation itself has been developed on the back of some rigourous mathematics. The original code for the [Euler Lagrange based Dynamic Model](https://github.com/average-engineer/InvDynamics_Robotic_Manipulator_Euler-Lagrange_Formulation) is for the case of a **fixed** base manipulator, and thus needs to be modified in order to account for base movement. 6 | 7 | The main assumptions of this dynamic model are: 8 | - Base inertia and mass is significantly higher than the manipulator itself i.e. the manipulator motion doesn't effect the base movement in any way. 9 | - Base Trajectory and its derivatives are considered as model parameters rather than generalized coordinates i.e. if we create a multi-body system out of the manipulator, then the base trajectory won't be counted as an independent system degree of freedom, rather it will be considered in the system matrix in the system's state space representation (`w_dot = A*w + B, A: System Matrix, w: State vector/generalized coordinates of system)` 10 | 11 | The code structure is given below: 12 | 13 | ![Code Structure](https://github.com/average-engineer/Moving_Base_Robotic_Manipulator_Dynamic_Model/blob/main/EulerLagrangeDynamicModel_MovingBase.PNG) 14 | 15 | Here, only the newly added function scripts for the new matrices have been discussed. For the other function scripts, refer to [Euler Lagrange based Dynamic Model](https://github.com/average-engineer/InvDynamics_Robotic_Manipulator_Euler-Lagrange_Formulation) in my profile. 16 | 17 | 18 | 1). `inertial_force_base.m` computes the matrix representing the effect of the inertial acceleration of the base on the manipulator links. It is dependent on the base coordinates, velocity and acceleration and independent of the system generalized coordinates. It is a `nx1` matrix. 19 | 20 | 2). `centri_cor_base_matrix.m` computes the matrix representing the effect of the centrifugal and coroilis force on the manipulator due to base movement. It is dependent on the base coordinates, velocity and acceleration and independent of the system generalized coordinates. It is a `nxn` matrix and multiplies with the system generalized coordinate matrix. 21 | 22 | 3). `gravity_loading_matrix.m` is a function which calculates the gravity loading matrix, whose each term corresponds to the potential energy considerations of each link due to gravity. In the case of moving base, it depends on the base position coordinates. 23 | 24 | 25 | ## Validating Dynamic Model 26 | For validation, the simple example of a 1-D cart pole system is considered. This system is extremely simple, with the cart being a moving base and the pole can be considered as a single link manipulator. The pole is connected to the cart with the help of a revolute joint. 27 | 28 | The anaytical solution of the cart pole system has been derived using tradiational Euler-Lagrange method after which the hard code is developed for the analytical expression, from which the anaytical solution for pole joint torque is computed. The analytical hard code was compiled in a MATLAB Live Script, which can be accessed from this repository, but for illustration purposes, the snippets of the live script have been shared here: 29 | 30 | ![LiveScriptSnippet](https://github.com/average-engineer/Moving_Base_Robotic_Manipulator_Dynamic_Model/blob/main/Live_Script_Snippet1.PNG) 31 | 32 | 33 | ![LiveScriptSnippet](https://github.com/average-engineer/Moving_Base_Robotic_Manipulator_Dynamic_Model/blob/main/Live_Script_Snippet2.PNG) 34 | 35 | 36 | ![LiveScriptSnippet](https://github.com/average-engineer/Moving_Base_Robotic_Manipulator_Dynamic_Model/blob/main/Live_Script_Snippet3.PNG) 37 | 38 | 39 | ![LiveScriptSnippet](https://github.com/average-engineer/Moving_Base_Robotic_Manipulator_Dynamic_Model/blob/main/cart_trajectory.png) 40 | 41 | 42 | Since this cart pole system can be seen as a 1 link manipulator with a moving base, the [Moving Base Dynamic Model](https://github.com/average-engineer/Moving_Base_Robotic_Manipulator_Dynamic_Model) is modified accordingly. **Important thing to note is that for the dynamic model, the cart (base) movement isn't considered as a generalized coordinate. 43 | 44 | ![LiveScriptSnippet](https://github.com/average-engineer/Moving_Base_Robotic_Manipulator_Dynamic_Model/blob/main/Live_Script_Snippet4.PNG) 45 | 46 | Comparing the analytical torque values and the code computed torque values: 47 | 48 | ![Result](https://github.com/average-engineer/Moving_Base_Robotic_Manipulator_Dynamic_Model/blob/main/Pole_Joint_Torque.png) 49 | 50 | As can be seen, both the torques are more or less same across the time span. Thus, this dynamic model works. 51 | 52 | 53 | -------------------------------------------------------------------------------- /T_Concat_dist.m: -------------------------------------------------------------------------------- 1 | 2 | % CALCULATE THE CONCATENATED MATRIX 3 | function T = T_Concat_dist(A,tip,base) 4 | 5 | T = eye(4); 6 | if((tip == 0 && base == 0)||(tip == base)) 7 | T = eye(4); 8 | elseif (tip > base) 9 | for n = tip : -1 : base+1 10 | T = A(:,:,n)*T; 11 | end 12 | end 13 | end 14 | -------------------------------------------------------------------------------- /U1_matrix.m: -------------------------------------------------------------------------------- 1 | function [U1] = U1_matrix(A,sim_time,NJ) 2 | 3 | %Q matrix for revolute joint 4 | Q = [0,-1,0,0;1,0,0,0;0,0,0,0;0,0,0,0]; 5 | for ii = 1:length(sim_time(1:end)) 6 | for i = 1:NJ 7 | for j = 1:NJ 8 | for k = 1:NJ 9 | if j<=k&&k<=i 10 | T1 = T_Concat_dist(A{ii},j-1,0); 11 | T2 = T_Concat_dist(A{ii},k-1,j-1); 12 | T3 = T_Concat_dist(A{ii},i,k-1); 13 | U1{ii}{i}{j,k} = T1*Q*T2*Q*T3; 14 | elseif k<=j&&j<=i 15 | T4 = T_Concat_dist(A{ii},k-1,0); 16 | T5 = T_Concat_dist(A{ii},j-1,k-1); 17 | T6 = T_Concat_dist(A{ii},i,j-1); 18 | U1{ii}{i}{j,k} = T4*Q*T5*Q*T6; 19 | elseif j>i||k>i 20 | U1{ii}{i}{j,k} = zeros(4,4); 21 | end 22 | end 23 | end 24 | end 25 | end 26 | 27 | end -------------------------------------------------------------------------------- /U_matrix.m: -------------------------------------------------------------------------------- 1 | function [U] = U_matrix(A,sim_time,NJ) 2 | 3 | %Q matrix for revolute joint 4 | Q = [0,-1,0,0;1,0,0,0;0,0,0,0;0,0,0,0]; 5 | for ii = 1:length(sim_time(1:end)) 6 | for i = 1:NJ 7 | for j = 1:NJ 8 | if j>i 9 | U{ii}{i,j} = zeros(4,4); 10 | else 11 | T1 = T_Concat_dist(A{ii},j-1,0); 12 | T2 = T_Concat_dist(A{ii},i,j-1); 13 | U{ii}{i,j} = T1*Q*T2; 14 | end 15 | end 16 | end 17 | end 18 | end -------------------------------------------------------------------------------- /cart_pole_system.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/average-engineer/Moving_Base_Robotic_Manipulator_Dynamic_Model/612ca3adbad165a1cb1246db312182f53aa3b994/cart_pole_system.PNG -------------------------------------------------------------------------------- /cart_trajectory.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/average-engineer/Moving_Base_Robotic_Manipulator_Dynamic_Model/612ca3adbad165a1cb1246db312182f53aa3b994/cart_trajectory.png -------------------------------------------------------------------------------- /centri_cor_base_matrix.m: -------------------------------------------------------------------------------- 1 | function [p_v] = centri_cor_base_matrix(sim_time,U,J,T_b,T_b_dot,NJ) 2 | 3 | for ii = 1:length(sim_time) 4 | % for i = 1:NJ 5 | % for r = 1:NJ 6 | % p_v{ii}(i,r) = 0; 7 | % for j = max(i,r):NJ 8 | % pv_1 = 2*trace(T_b_dot{ii}*U{ii}{j,r}*J{ii}{j}*(U{ii}{j,i}')*(T_b{ii}')); 9 | % p_v{ii}(i,r) = pv_1 + p_v{ii}(i,r); 10 | % end 11 | % end 12 | % end 13 | 14 | for i = 1:NJ 15 | for j = i:NJ 16 | for r = 1:j 17 | p_v{ii}(i,r) = 0; 18 | pv_1 = trace(T_b_dot{ii}*U{ii}{j,r}*J{ii}{j}*(U{ii}{j,i}')*(T_b{ii}')); 19 | p_v{ii}(i,r) = pv_1 + p_v{ii}(i,r); 20 | end 21 | end 22 | end 23 | end 24 | end -------------------------------------------------------------------------------- /centri_cor_force_matrix.m: -------------------------------------------------------------------------------- 1 | function [h_star] = centri_cor_force_matrix(sim_time,U,U1,J,NJ,theta_dot) 2 | 3 | for ii = 1:length(sim_time(1:end)) 4 | for i = 1:NJ 5 | for k = 1:NJ 6 | for m = 1:NJ 7 | h{ii}{i}(k,m) = 0; 8 | for j = max([i,k,m]):NJ 9 | h1 = trace(U1{ii}{j}{k,m}*J{ii}{j}*(U{ii}{j,i}')); 10 | h{ii}{i}(k,m) = h{ii}{i}(k,m) + h1; 11 | end 12 | end 13 | end 14 | end 15 | for i = 1:NJ 16 | h_star{ii}(i) = (theta_dot{ii}')*h{ii}{i}*theta_dot{ii}; 17 | end 18 | h_star{ii} = h_star{ii}'; 19 | end 20 | end -------------------------------------------------------------------------------- /finite_diff_scalar.m: -------------------------------------------------------------------------------- 1 | %function for applying the finite difference method in order to compute the 2 | %differentiation of a scalar data set 3 | function output = finite_diff_scalar(sim_time,X) 4 | %for the first data point 5 | %each data point is a scalar quantity 6 | %On = (Xn+1 - Xn)/(tn+1 - tn) 7 | %for the last data point 8 | %On = (Xn - Xn-1)/(tn - tn-1) 9 | %for the rest of the data 10 | %On = (Xn+1 - Xn-1)/(tn+1 - tn-1) 11 | for i = 1:length(sim_time) 12 | if i == 1 13 | output(i) = (X(i+1) - X(i))/(sim_time(i+1) - sim_time(i)); 14 | elseif i == length(sim_time) 15 | output(i) = (X(i) - X(i-1))/(sim_time(i) - sim_time(i-1)); 16 | else 17 | output(i) = (X(i+1) - X(i-1))/(sim_time(i+1) - sim_time(i-1)); 18 | end 19 | end 20 | %output is a double (numerical value) matrix having the same length as the 21 | %kinematic time vector and each element is a scalar value 22 | end -------------------------------------------------------------------------------- /finite_diff_vector.m: -------------------------------------------------------------------------------- 1 | %function for applying the finite difference method in order to compute the 2 | %differentiation of a vector data set 3 | function output = finite_diff_vector(sim_time,X) 4 | %each data point is basically a vector, not a scalar point 5 | %for the first data point 6 | %On = (Xn+1 - Xn)/(tn+1 - tn) 7 | %for the last data point 8 | %On = (Xn - Xn-1)/(tn - tn-1) 9 | %for the rest of the data 10 | %On = (Xn+1 - Xn-1)/(tn+1 - tn-1) 11 | for i = 1:length(sim_time) 12 | if i == 1 13 | output{i} = (X{i+1} - X{i})/(sim_time(i+1) - sim_time(i)); 14 | elseif i == length(sim_time) 15 | output{i} = (X{i} - X{i-1})/(sim_time(i) - sim_time(i-1)); 16 | else 17 | output{i} = (X{i+1} - X{i-1})/(sim_time(i+1) - sim_time(i-1)); 18 | end 19 | end 20 | %output is a cell matrix having the same length as the kinematic time 21 | %vector and each element is a double (numerical) matrix 22 | end -------------------------------------------------------------------------------- /gravity_loading_matrix.m: -------------------------------------------------------------------------------- 1 | function [c] = gravity_loading_matrix(sim_time,m,g,U,r,T_b,NJ) 2 | 3 | for ii = 1:length(sim_time(1:end)) 4 | c{ii} = zeros(NJ,1); 5 | for i = 1:NJ 6 | c{ii}(i) = 0; 7 | for j = i:NJ 8 | c1 = -m(j)*g*T_b{ii}*U{ii}{j,i}*r{ii}{j}; 9 | c{ii}(i) = c{ii}(i) + c1; 10 | end 11 | end 12 | end 13 | end -------------------------------------------------------------------------------- /inertia_acceleration_matrix.m: -------------------------------------------------------------------------------- 1 | function [D] = inertia_acceleration_matrix(sim_time,U,J,NJ) 2 | 3 | for ii = 1:length(sim_time(1:end)) 4 | for i = 1:NJ 5 | for k = 1:NJ 6 | D{ii}(i,k) = 0; 7 | for j = max(i,k):NJ 8 | D_1 = trace(U{ii}{j,k}*J{ii}{j}*U{ii}{j,i}'); 9 | D{ii}(i,k) = D{ii}(i,k) + D_1; 10 | end 11 | end 12 | end 13 | end 14 | end -------------------------------------------------------------------------------- /inertia_matrix_dist.m: -------------------------------------------------------------------------------- 1 | function [J] = inertia_matrix_dist(link_lengths,m,r,NJ,sim_time) 2 | %units: kgm2 3 | %ASSUMPTIONS 4 | %the inertias of each segment along the X axis of its distal joint frame is assumed 5 | %to be negligible 6 | %the inertias of each segment along the Y and Z axes of its distal frame are 7 | %assumed to be equal 8 | %cross inertias (Ixx,Ixy,Ixz) are assumed to be zero 9 | 10 | %inertias of link i about the frame of distal joint 11 | for ii = 1:length(sim_time) 12 | Ixx{ii} = zeros(NJ,1); 13 | Iyy{ii} = zeros(NJ,1); 14 | Izz{ii} = zeros(NJ,1); 15 | Ixy{ii} = zeros(NJ,1); 16 | Iyz{ii} = zeros(NJ,1); 17 | Ixz{ii} = zeros(NJ,1); 18 | end 19 | 20 | for ii = 1:length(sim_time) 21 | for i = 1:NJ 22 | %inertia matrix for each link i 23 | %the inertias are about the joints distal to the links/segments 24 | %applying parallel axis theorem 25 | Iyy{ii}(i) = (m(i)*(link_lengths{ii}(i))^2)/3; 26 | Izz{ii}(i) =(m(i)*(link_lengths{ii}(i))^2)/3; 27 | 28 | J{ii}{i} = [(-Ixx{ii}(i) + Iyy{ii}(i) + Izz{ii}(i))/2,Ixy{ii}(i),Ixz{ii}(i),m(i)*r{ii}{i}(1); 29 | Ixy{ii}(i),(Ixx{ii}(i) - Iyy{ii}(i) + Izz{ii}(i))/2,Iyz{ii}(i),m(i)*r{ii}{i}(2); 30 | Ixz{ii}(i),Iyz{ii}(i),(Ixx{ii}(i) + Iyy{ii}(i) - Izz{ii}(i))/2,m(i)*r{ii}{i}(3); 31 | m(i)*r{ii}{i}(1),m(i)*r{ii}{i}(2),m(i)*r{ii}{i}(3),m(i)]; 32 | end 33 | end 34 | end -------------------------------------------------------------------------------- /inertial_force_base.m: -------------------------------------------------------------------------------- 1 | function [p_f] = inertial_force_base(sim_time,U,J,A,T_b,T_b_dot_dot,NJ) 2 | 3 | for ii = 1:length(sim_time) 4 | for i = 1:NJ 5 | p_f{ii}(i) = 0; 6 | for j = i:NJ 7 | T1 = T_Concat_dist(A{ii},j,0); 8 | p_f1 = trace(T_b_dot_dot{ii}*T1*J{ii}{j}*(U{ii}{j,i}')*(T_b{ii}')); 9 | p_f{ii}(i) = p_f{ii}(i) + p_f1; 10 | end 11 | end 12 | p_f{ii} = p_f{ii}'; 13 | end 14 | end -------------------------------------------------------------------------------- /state_space_CartPole.m: -------------------------------------------------------------------------------- 1 | function [dw_dt] = state_space_CartPole(t,w,m,g,L,M) 2 | theta_dotdot = -0.005*sin(t); 3 | x_dotdot = -((m*L)/((2)*(m + M)))*theta_dotdot; 4 | Q = -(L)*(((1/3)*(2*M + (m/2)))*(x_dotdot) + ((m*g)/(2))*(w(2))); 5 | % dw_dt_1 = w(3); 6 | % dw_dt_2 = w(4); 7 | % dw_dt_4 = ((1)/(((m*(L)^(2))/(3)) - (((m*L)^2)/((4)*(M + m)))))*((Q) + (((m*g*L)/(2))*(w(2)))); 8 | % dw_dt_3 = ((-m*L)/((2)*(M + m)))*(dw_dt_4); 9 | % dw_dt = [dw_dt_1;dw_dt_2;dw_dt_3;dw_dt_4]; 10 | % Mass Matrix 11 | M_mat = [m + M,(m*L)/2;(m*L)/2,(m*(L)^2)/3]; 12 | % Stiffness Matrix 13 | K_mat = [0,0;0,(-m*g*L)/2]; 14 | % system matrix 15 | A = [zeros(2,2),eye(2,2);-M_mat\K_mat,zeros(2,2)]; 16 | % input matrix 17 | B = [zeros(2,2);M_mat\eye(2,2)]; 18 | % input vector 19 | u = [0;Q]; 20 | 21 | % state space representation 22 | dw_dt = A*w + B*u; 23 | end --------------------------------------------------------------------------------