├── LICENSE ├── README.md ├── animate_3dquad_load.m ├── display_message.m ├── get_load_traj.m ├── get_nom_traj.m ├── main.m └── odefun_control.m /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Hybrid Robotics 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 | # Quad-Load 2 | 3 | ## References 4 | If you find this project useful in your work, please consider citing following paper: 5 | K. Sreenath, T. Lee and V. Kumar "Geometric control and differential flatness of a quadrotor UAV with a cable-suspended load." *IEEE Conference on Decision and Control (CDC)*. [[IEEE]](https://ieeexplore.ieee.org/document/6760219) 6 | ``` 7 | @inproceedings{sreenath2013geometric, 8 | title={Geometric control and differential flatness of a quadrotor UAV with a cable-suspended load}, 9 | author={Sreenath, Koushil and Lee, Taeyoung and Kumar, Vijay}, 10 | booktitle={52nd IEEE Conference on Decision and Control}, 11 | pages={2269--2274}, 12 | year={2013}, 13 | organization={IEEE} 14 | } 15 | -------------------------------------------------------------------------------- /animate_3dquad_load.m: -------------------------------------------------------------------------------- 1 | function animate_3dquad_load(torig, x, t_pd, xLd) 2 | RATE = 25 * 1; 3 | %========================================== 4 | % initialize the animation figure and axes 5 | %========================================== 6 | figure_x_limits = [-4 8]; 7 | figure_y_limits = [-9 5]; 8 | figure_z_limits = [-2 3] ; 9 | fig1 = figure; 10 | 11 | set(0,'Units','pixels') 12 | scnsize = get(0,'ScreenSize'); 13 | 14 | screen_width = scnsize(3); 15 | screen_height = scnsize(4); 16 | 17 | % find the minimum scaling factor 18 | figure_x_size = figure_x_limits(2) - figure_x_limits(1); 19 | figure_y_size = figure_y_limits(2) - figure_y_limits(1); 20 | 21 | xfactor = screen_width/figure_x_size; 22 | yfactor = screen_height/figure_y_size; 23 | 24 | if (xfactor < yfactor) 25 | screen_factor = 0.5*xfactor; 26 | else 27 | screen_factor = 0.5*yfactor; 28 | end 29 | 30 | % calculate screen offsets 31 | screen_x_offset = (screen_width - screen_factor*figure_x_size)/2; 32 | screen_y_offset = (screen_height - screen_factor*figure_y_size)/2; 33 | 34 | % draw figure and axes 35 | set(fig1,'Position', [screen_x_offset screen_y_offset screen_factor*figure_x_size screen_factor*figure_y_size]); 36 | set(fig1,'MenuBar', 'none'); 37 | axes1 = axes; 38 | set(axes1,'XLim',figure_x_limits,'YLim',figure_y_limits); 39 | % set(axes1,'Position',[0 0 1 1]); 40 | % set(axes1,'Color','w'); 41 | %set(axes1,'TickDir','out'); 42 | axis equal ; 43 | box on; 44 | xorig = x ; 45 | xLdorig = xLd ; 46 | [t, x] = even_sample(torig, x, RATE); 47 | t = t+torig(1) ; 48 | [~, xLd] = even_sample(t_pd, xLd, RATE) ; 49 | L = 1 ; % Cable length 50 | %% Define plotting length 51 | hist = 2500; 52 | MAKE_MOVIE = 1; 53 | if(MAKE_MOVIE) 54 | M = moviein(length(t)) ; 55 | end 56 | 57 | % aviobj = avifile('sample2.avi','compression','None'); 58 | for i=1:length(t) 59 | %set(axes1,'XLim',figure_x_limits+pH(i,1)) ; 60 | drawone(axes1, x(i,:)'); 61 | %% No shading plotting 62 | plot3(x(max(1,i-hist):i, 1), x(max(1,i-hist):i, 2), x(max(1,i-hist):i, 3), 'b'); 63 | plot3(xLd(max(1,i-hist):i, 1), xLd(max(1,i-hist):i, 2), xLd(max(1,i-hist):i, 3), 'r'); 64 | 65 | % plot3(xorig(:, 1), xorig(:, 2), xorig(:, 3), 'b') ; 66 | % plot3(xLdorig(:, 1), xLdorig(:, 2), xLdorig(:, 3), 'r') ; 67 | % s = sprintf('Running\n t = %1.2fs \n 1/%d realtime speed',t(i), RATE/25); 68 | % text(-1.3,2.4,s,'FontAngle','italic','FontWeight','bold'); 69 | drawnow; 70 | set(axes1,'XLim',figure_x_limits,'YLim',figure_y_limits,'ZLim',figure_z_limits); 71 | if MAKE_MOVIE, M(:,i) = getframe; end 72 | % aviobj = addframe(aviobj, fig1); 73 | 74 | end 75 | % aviobj = close(aviobj); 76 | 77 | % v = VideoWriter('newfile.avi','Uncompressed AVI'); 78 | % writeVideo(v,M); 79 | 80 | % if(MAKE_MOVIE) 81 | % % for i = 1:length(M) 82 | % % if (i==1) 83 | % % [r1,c1,s] = size(M(i).cdata); 84 | % % rk = r1; 85 | % % ck = c1; 86 | % % else 87 | % % [rk,ck,s] = size(M(i).cdata); 88 | % % end 89 | % % r1 = min(r1,rk); 90 | % % c1 = min(c1,ck); 91 | % % end 92 | % % 93 | % % for i = 1:length(M) 94 | % % [rk,ck] = size(M(i).cdata); 95 | % % if ((rk~= r1) || (ck ~=c1)) 96 | % % M(i).cdata = M(i).cdata(1:r1, 1:c1,:); 97 | % % end 98 | % % end 99 | % % mpgwrite(M,jet,'movie.mpg', [1 0 0 1 10 8 10 25]); 100 | % mpgwrite(M,jet,'movie.mpg', [1 0 0 1 4 1 1 1]); 101 | % end 102 | end 103 | 104 | function drawone(parent, x) 105 | tem = get(parent,'Children'); 106 | delete(tem); 107 | 108 | s.L = 0.175; %length of quadrotor boom 109 | s.R = 0.1; %radius of propeller prop 110 | 111 | L = 1 ; % Cable length 112 | 113 | % Extract state x 114 | xL = x(1:3) ; 115 | % vL = x(4:6) ; 116 | p = x(7:9) ; 117 | % omega = x(10:12) ; 118 | R = reshape(x(13:21), 3,3) ; 119 | % Omega = x(22:24) ; 120 | xQ = xL - L*p ; 121 | 122 | % plot3(xQ(1), xQ(2), xQ(3), 'r.') ; 123 | 124 | BRW = R' ; 125 | 126 | point1 = BRW'*[s.L,0,0]'; 127 | point2 = BRW'*[0,s.L,0]'; 128 | point3 = BRW'*[-s.L,0,0]'; 129 | point4 = BRW'*[0,-s.L,0]'; 130 | 131 | nprop = 40; 132 | propangs = linspace(0,2*pi,nprop); 133 | proppts = s.R*BRW'*[cos(propangs);sin(propangs);zeros(1,nprop)]; 134 | 135 | wp = xQ ; 136 | wp1 = wp + point1; 137 | wp2 = wp + point2; 138 | wp3 = wp + point3; 139 | wp4 = wp + point4; 140 | wp_cable_attach = wp + BRW'*[0;0;0] ; %[0;0;-params.cable_attach_d]; 141 | 142 | prop1 = proppts + wp1*ones(1,nprop); 143 | prop2 = proppts + wp2*ones(1,nprop); 144 | prop3 = proppts + wp3*ones(1,nprop); 145 | prop4 = proppts + wp4*ones(1,nprop); 146 | 147 | lwp = 2 ; 148 | lw1 = 1 ; 149 | lwc = 2 ; 150 | lwl = 2 ; 151 | 152 | s.qhandle1 = line([wp1(1),wp3(1)],[wp1(2),wp3(2)],[wp1(3),wp3(3)]); hold on ; 153 | s.qhandle2 = line([wp2(1),wp4(1)],[wp2(2),wp4(2)],[wp2(3),wp4(3)]); 154 | set(s.qhandle1,'Color','k', 'LineWidth',lw1); 155 | set(s.qhandle2,'Color','k', 'LineWidth',lw1); 156 | 157 | s.hprop1 = plot3(prop1(1,:),prop1(2,:),prop1(3,:),'r-', 'LineWidth',lwp); 158 | s.hprop2 = plot3(prop2(1,:),prop2(2,:),prop2(3,:),'b-', 'LineWidth',lwp); 159 | s.hprop3 = plot3(prop3(1,:),prop3(2,:),prop3(3,:),'b-', 'LineWidth',lwp); 160 | s.hprop4 = plot3(prop4(1,:),prop4(2,:),prop4(3,:),'b-', 'LineWidth',lwp); 161 | 162 | s.hload = plot3(xL(1,1), xL(2,1), xL(3,1), 'ko', 'LineWidth',lwl) ; 163 | s.cable = plot3([wp_cable_attach(1) xL(1,1)], [wp_cable_attach(2) xL(2,1)], [wp_cable_attach(3) xL(3,1)], 'k') ; 164 | s.cable_attach = plot3([wp(1) wp_cable_attach(1)], [wp(2) wp_cable_attach(2)], [wp(3) wp_cable_attach(3)], 'r') ; 165 | 166 | % axis equal 167 | % set(handles.axes10,'Xlim',[max(min(s.xs(s.goodv))-offset,-20), min(max(s.xs(s.goodv))+offset,20)]); 168 | % set(handles.axes10,'Ylim',[max(min(s.ys(s.goodv))-offset,-20), min(max(s.ys(s.goodv))+offset,20)]); 169 | % set(handles.axes10,'Zlim',[max(min(s.zs(s.goodv))-offset,-20), min(max(s.zs(s.goodv))+offset,20)]); 170 | grid on 171 | view([-1,-1,1]) 172 | xlabel('X') 173 | ylabel('Y') 174 | zlabel('Z') 175 | 176 | 177 | 178 | 179 | % L_quad = 0.15 ; 180 | % L_blade = 0.07 ; 181 | % L_axis = 0.05 ; 182 | % 183 | % delta_p = L_quad*[cos(psi) ; sin(psi)] ; 184 | % p1 = [x ; y] - delta_p ; 185 | % p2 = [x ; y] + delta_p ; 186 | % plot([p1(1) p2(1)], [p1(2) p2(2)], 'k', 'LineWidth', 2) ; 187 | % hold on ; 188 | % 189 | % delta_a = L_axis*[cos(pi/2+psi) ; sin(pi/2+psi)] ; 190 | % a1 = p1 + delta_a ; 191 | % a2 = p2 + delta_a ; 192 | % plot([p1(1) a1(1)], [p1(2) a1(2)], 'k', 'LineWidth', 2) ; 193 | % plot([p2(1) a2(1)], [p2(2) a2(2)], 'k', 'LineWidth', 2) ; 194 | % 195 | % delta_b = L_blade*[cos(psi) ; sin(psi)] ; 196 | % b11 = a1 - delta_b ; 197 | % b12 = a1 + delta_b ; 198 | % b21 = a2 - delta_b ; 199 | % b22 = a2 + delta_b ; 200 | % plot([b11(1) b12(1)], [b11(2) b12(2)], 'k', 'LineWidth', 2) ; 201 | % plot([b21(1) b22(1)], [b21(2) b22(2)], 'k', 'LineWidth', 2) ; 202 | % 203 | % 204 | % % Plot load 205 | % L = 1 ; 206 | % plot([x xL], [y yL], 'k', 'LineWidth', 1) ; 207 | % plot(xL, yL, 'ko') ; 208 | % 209 | % % Plot Obstacles 210 | % for j=1:length(Obstacles_Lines) 211 | % L = Obstacles_Lines{j} ; 212 | % plot([L(:,1); L(1,1)], [L(:,2); L(1,2)], 'k', 'LineWidth', 2) ; 213 | % end 214 | end 215 | 216 | function drawtwo(parent, x) 217 | % tem = get(parent,'Children'); 218 | % delete(tem); 219 | s.L = 0.175; %length of quadrotor boom 220 | s.R = 0.1; %radius of propeller prop 221 | 222 | % Extract state x 223 | xL = x(1:3) ; 224 | % vL = x(4:6) ; 225 | p = x(7:9) ; 226 | % omega = x(10:12) ; 227 | R = reshape(x(13:21), 3,3) ; 228 | % Omega = x(22:24) ; 229 | L = 1; 230 | xQ = xL - L*p ; 231 | 232 | % plot3(xQ(1), xQ(2), xQ(3), 'r.') ; 233 | 234 | BRW = R' ; 235 | 236 | point1 = BRW'*[s.L,0,0]'; 237 | point2 = BRW'*[0,s.L,0]'; 238 | point3 = BRW'*[-s.L,0,0]'; 239 | point4 = BRW'*[0,-s.L,0]'; 240 | 241 | nprop = 40; 242 | propangs = linspace(0,2*pi,nprop); 243 | proppts = s.R*BRW'*[cos(propangs);sin(propangs);zeros(1,nprop)]; 244 | 245 | wp = xQ ; 246 | wp1 = wp + point1; 247 | wp2 = wp + point2; 248 | wp3 = wp + point3; 249 | wp4 = wp + point4; 250 | wp_cable_attach = wp + BRW'*[0;0;0] ; %[0;0;-params.cable_attach_d]; 251 | 252 | prop1 = proppts + wp1*ones(1,nprop); 253 | prop2 = proppts + wp2*ones(1,nprop); 254 | prop3 = proppts + wp3*ones(1,nprop); 255 | prop4 = proppts + wp4*ones(1,nprop); 256 | 257 | lwp = 2 ; 258 | lw1 = 1 ; 259 | lwc = 2 ; 260 | lwl = 2 ; 261 | 262 | s.qhandle1 = line([wp1(1),wp3(1)],[wp1(2),wp3(2)],[wp1(3),wp3(3)]); hold on ; 263 | s.qhandle2 = line([wp2(1),wp4(1)],[wp2(2),wp4(2)],[wp2(3),wp4(3)]); 264 | set(s.qhandle1,'Color','k', 'LineWidth',lw1); 265 | set(s.qhandle2,'Color','k', 'LineWidth',lw1); 266 | 267 | s.hprop1 = plot3(prop1(1,:),prop1(2,:),prop1(3,:),'r-', 'LineWidth',lwp); 268 | s.hprop2 = plot3(prop2(1,:),prop2(2,:),prop2(3,:),'b-', 'LineWidth',lwp); 269 | s.hprop3 = plot3(prop3(1,:),prop3(2,:),prop3(3,:),'b-', 'LineWidth',lwp); 270 | s.hprop4 = plot3(prop4(1,:),prop4(2,:),prop4(3,:),'b-', 'LineWidth',lwp); 271 | 272 | s.hload = plot3(xL(1,1), xL(2,1), xL(3,1), 'ko', 'LineWidth',lwl) ; 273 | s.cable = plot3([wp_cable_attach(1) xL(1,1)], [wp_cable_attach(2) xL(2,1)], [wp_cable_attach(3) xL(3,1)], 'k') ; 274 | s.cable_attach = plot3([wp(1) wp_cable_attach(1)], [wp(2) wp_cable_attach(2)], [wp(3) wp_cable_attach(3)], 'r') ; 275 | 276 | offset = 0.5; 277 | 278 | % axis equal 279 | % set(handles.axes10,'Xlim',[max(min(s.xs(s.goodv))-offset,-20), min(max(s.xs(s.goodv))+offset,20)]); 280 | % set(handles.axes10,'Ylim',[max(min(s.ys(s.goodv))-offset,-20), min(max(s.ys(s.goodv))+offset,20)]); 281 | % set(handles.axes10,'Zlim',[max(min(s.zs(s.goodv))-offset,-20), min(max(s.zs(s.goodv))+offset,20)]); 282 | grid on 283 | 284 | view([-1,-1,1]) 285 | xlabel('X') 286 | ylabel('Y') 287 | zlabel('Z') 288 | 289 | 290 | 291 | 292 | % L_quad = 0.15 ; 293 | % L_blade = 0.07 ; 294 | % L_axis = 0.05 ; 295 | % 296 | % delta_p = L_quad*[cos(psi) ; sin(psi)] ; 297 | % p1 = [x ; y] - delta_p ; 298 | % p2 = [x ; y] + delta_p ; 299 | % plot([p1(1) p2(1)], [p1(2) p2(2)], 'k', 'LineWidth', 2) ; 300 | % hold on ; 301 | % 302 | % delta_a = L_axis*[cos(pi/2+psi) ; sin(pi/2+psi)] ; 303 | % a1 = p1 + delta_a ; 304 | % a2 = p2 + delta_a ; 305 | % plot([p1(1) a1(1)], [p1(2) a1(2)], 'k', 'LineWidth', 2) ; 306 | % plot([p2(1) a2(1)], [p2(2) a2(2)], 'k', 'LineWidth', 2) ; 307 | % 308 | % delta_b = L_blade*[cos(psi) ; sin(psi)] ; 309 | % b11 = a1 - delta_b ; 310 | % b12 = a1 + delta_b ; 311 | % b21 = a2 - delta_b ; 312 | % b22 = a2 + delta_b ; 313 | % plot([b11(1) b12(1)], [b11(2) b12(2)], 'k', 'LineWidth', 2) ; 314 | % plot([b21(1) b22(1)], [b21(2) b22(2)], 'k', 'LineWidth', 2) ; 315 | % 316 | % 317 | % % Plot load 318 | % L = 1 ; 319 | % plot([x xL], [y yL], 'k', 'LineWidth', 1) ; 320 | % plot(xL, yL, 'ko') ; 321 | % 322 | % % Plot Obstacles 323 | % for j=1:length(Obstacles_Lines) 324 | % L = Obstacles_Lines{j} ; 325 | % plot([L(:,1); L(1,1)], [L(:,2); L(1,2)], 'k', 'LineWidth', 2) ; 326 | % end 327 | end -------------------------------------------------------------------------------- /display_message.m: -------------------------------------------------------------------------------- 1 | function h = display_message() 2 | h = msgbox('Starting ODE Calculations'); 3 | end -------------------------------------------------------------------------------- /get_load_traj.m: -------------------------------------------------------------------------------- 1 | function load_traj = get_load_traj(t) 2 | %% Global Test 3 | f1 = 1/4; 4 | f2 = 1/5; 5 | f3 = 1/7; 6 | f4 = 1/9; 7 | 8 | a_x = 2; 9 | a_y = 2.5; 10 | a_z = 1.5; 11 | 12 | load_traj.xL = [a_x*(1 - cos(2*pi*f1*t)); 13 | a_y * sin(2*pi*f2*t); 14 | a_z * cos(2*pi*f3*t)]; 15 | load_traj.dxL = 2*pi*[f1*a_x*sin(2*pi*f1*t); 16 | f2*a_y * cos(2*pi*f2*t); 17 | f3*1.5 * -sin(2*pi*f3*t)]; 18 | load_traj.d2xL = (2*pi)^2*[f1^2*a_x*cos(2*pi*f1*t); 19 | f2^2*a_y * -sin(2*pi*f2*t); 20 | f3^2*a_z * -cos(2*pi*f3*t)]; 21 | load_traj.d3xL = (2*pi)^3*[f1^3*a_x*-sin(2*pi*f1*t); 22 | f2^3*a_y * -cos(2*pi*f2*t); 23 | f3^3*a_z * sin(2*pi*f3*t)]; 24 | load_traj.d4xL = (2*pi)^4*[f1^4*a_x*-cos(2*pi*f1*t); 25 | f2^4*a_y * sin(2*pi*f2*t); 26 | f3^4*a_z * cos(2*pi*f3*t)]; 27 | load_traj.d5xL = (2*pi)^5*[f1^5*a_x*sin(2*pi*f1*t); 28 | f2^5*a_y * cos(2*pi*f2*t); 29 | f3^5*a_z * -sin(2*pi*f3*t)]; 30 | load_traj.d6xL = (2*pi)^6*[f1^6*a_x*cos(2*pi*f1*t); 31 | f2^6*a_y * -sin(2*pi*f2*t); 32 | f3^6*a_z * -cos(2*pi*f3*t)]; 33 | 34 | end -------------------------------------------------------------------------------- /get_nom_traj.m: -------------------------------------------------------------------------------- 1 | % function [xLd vLd aLd daLd d2aLd d3aLd d4aLd p dp d2p d3p d4p b1 b2 b3 db1 db2 db3 d2b1 d2b2 d2b3] = get_nom_traj(t) 2 | % Use differential flatness to compute various system quantities, given 3 | % load trajectory and higher order time deriviatives. 4 | function [xLd,vLd,aLd,p,dp,d2p,... 5 | R,omega,domega,Omega,dOmega,daLd,d2aLd,d3p,dR,d2R,f,M] = get_nom_traj(params, load_traj) 6 | %% Constants 7 | mQ = params.mQ ; 8 | mL = params.mL ; 9 | J = params.J ; 10 | g = params.g ; 11 | e1 = params.e1 ; 12 | e2 = params.e2 ; 13 | e3 = params.e3 ; 14 | l = params.l; 15 | 16 | %% Derivative of Load Position 17 | xLd = load_traj.xL; 18 | vLd = load_traj.dxL; 19 | aLd = load_traj.d2xL; 20 | daLd = load_traj.d3xL; 21 | d2aLd = load_traj.d4xL; 22 | d3aLd = load_traj.d5xL; 23 | d4aLd = load_traj.d6xL; 24 | 25 | %% Cable Tension and Directional Vector 26 | Tp = -mL*(aLd + g*e3) ; 27 | norm_Tp = norm(Tp); 28 | p = Tp / norm_Tp; 29 | xQ = xLd - l*p; 30 | 31 | dTp = -mL*daLd ; 32 | dnorm_Tp = 1/norm_Tp * vec_dot(Tp, dTp) ; 33 | dp = (dTp - p*dnorm_Tp) / norm_Tp ; 34 | 35 | 36 | d2Tp = -mL*d2aLd ; 37 | d2norm_Tp = ( vec_dot(dTp, dTp) + vec_dot(Tp, d2Tp) - dnorm_Tp^2 ) / norm_Tp ; 38 | d2p = ( d2Tp - dp*dnorm_Tp - p*d2norm_Tp - dp*dnorm_Tp) / norm_Tp ; 39 | 40 | d3Tp = -mL*d3aLd ; 41 | d3norm_Tp = ( 2*vec_dot(d2Tp, dTp) + vec_dot(dTp, d2Tp)+vec_dot(Tp, d3Tp) - 3*dnorm_Tp*d2norm_Tp) / norm_Tp ; 42 | d3p = (d3Tp - d2p*dnorm_Tp-dp*d2norm_Tp - dp*d2norm_Tp-p*d3norm_Tp - d2p*dnorm_Tp-dp*d2norm_Tp - d2p*dnorm_Tp) / norm_Tp ; 43 | 44 | d4Tp = -mL*d4aLd; 45 | d4norm_Tp = ( 2*vec_dot(d3Tp, dTp)+2*vec_dot(d2Tp, d2Tp) + vec_dot(d2Tp, d2Tp)+vec_dot(dTp, d3Tp) + vec_dot(dTp, d3Tp)+vec_dot(Tp, d4Tp) - 3*d2norm_Tp^2-3*dnorm_Tp*d3norm_Tp ... 46 | - d3norm_Tp*dnorm_Tp) / norm_Tp ; 47 | d4p = ( d4Tp - d3p*dnorm_Tp-d2p*d2norm_Tp - d2p*d2norm_Tp-dp*d3norm_Tp - d2p*d2norm_Tp-dp*d3norm_Tp - dp*d3norm_Tp-p*d4norm_Tp ... 48 | - d3p*dnorm_Tp-d2p*d2norm_Tp - d2p*d2norm_Tp-dp*d3norm_Tp - d3p*dnorm_Tp-d2p*d2norm_Tp - d3p*dnorm_Tp ) / norm_Tp ; 49 | 50 | %% Derivatives of Load Angular Velocity 51 | omega = vec_cross(p,dp); 52 | domega = vec_cross(dp,dp)+vec_cross(p,d2p); 53 | 54 | %% Derivatives of Quadrotor's Position 55 | vxQ = vLd - l*dp; 56 | axQ = aLd - l*d2p; 57 | daxQ = daLd - l*d3p; 58 | d2axQ = d2aLd - l*d4p; 59 | 60 | b1d = e1; 61 | db1d = zeros(3,1); 62 | d2b1d = zeros(3,1); 63 | 64 | fb3 = mQ*(axQ+g*e3) - Tp; 65 | norm_fb3 = norm(fb3); 66 | f = norm_fb3; 67 | b3 = fb3 / norm_fb3; 68 | b3_b1d = vec_cross(b3, b1d); 69 | norm_b3_b1d = norm(b3_b1d); 70 | b1 = - vec_cross(b3, b3_b1d) / norm_b3_b1d; 71 | b2 = vec_cross(b3, b1); 72 | R = [b1 b2 b3]; 73 | 74 | dfb3 = mQ*(daxQ) - dTp; 75 | dnorm_fb3 = vec_dot(fb3, dfb3) / norm_fb3; 76 | db3 = (dfb3*norm_fb3 - fb3*dnorm_fb3) / norm_fb3^2; 77 | db3_b1d = vec_cross(db3, b1d) + vec_cross(b3, db1d); 78 | dnorm_b3_b1d = vec_dot(b3_b1d, db3_b1d) / norm_b3_b1d; 79 | db1 = (-vec_cross(db3,b3_b1d)-vec_cross(b3,db3_b1d) - b1*dnorm_b3_b1d) / norm_b3_b1d; 80 | db2 = vec_cross(db3, b1) + vec_cross(b3, db1); 81 | dR = [db1 db2 db3]; 82 | Omega = vee_map(R'*dR); 83 | 84 | d2fb3 = mQ*(d2axQ) - d2Tp ; 85 | d2norm_fb3 = (vec_dot(dfb3, dfb3)+vec_dot(fb3, d2fb3) - dnorm_fb3*dnorm_fb3) / norm_fb3 ; 86 | d2b3 = ( (d2fb3*norm_fb3+dfb3*dnorm_fb3 - dfb3*dnorm_fb3-fb3*d2norm_fb3)*norm_fb3^2 - db3*norm_fb3^2*2*norm_fb3*dnorm_fb3 ) / norm_fb3^4 ; 87 | d2b3_b1d = vec_cross(d2b3, b1d)+vec_cross(db3, db1d) + vec_cross(db3, db1d)+vec_cross(b3, d2b1d) ; 88 | d2norm_b3_b1d = ( (vec_dot(db3_b1d,db3_b1d)+vec_dot(b3_b1d,d2b3_b1d))*norm_b3_b1d - vec_dot(b3_b1d, db3_b1d)*dnorm_b3_b1d ) / norm_b3_b1d^2 ; 89 | d2b1 = ( (-vec_cross(d2b3,b3_b1d)-vec_cross(db3,db3_b1d) - vec_cross(db3,db3_b1d)-vec_cross(b3,d2b3_b1d) - db1*dnorm_b3_b1d-b1*d2norm_b3_b1d )*norm_b3_b1d - db1*norm_b3_b1d*dnorm_b3_b1d ) / norm_b3_b1d^2 ; 90 | d2b2 = vec_cross(d2b3, b1)+vec_cross(db3, db1) + vec_cross(db3, db1)+vec_cross(b3, d2b1) ; 91 | d2R = [d2b1 d2b2 d2b3]; 92 | dOmega = vee_map( dR'*dR + R'*d2R ) ; %vee_map( dR'*dR + R'*d2R, true ) ; 93 | M = J*dOmega + vec_cross(Omega, J*Omega); 94 | end 95 | 96 | % Test-case Function to test the above function. 97 | function test_nom_traj() 98 | t = linspace(0, 10, 1000)' ; 99 | for j=1:length(t) 100 | [xLd(:,j) vLd(:,j) aLd(:,j) daLd(:,j) d2aLd(:,j) d3aLd(:,j) d4aLd(:,j) p(:,j) dp(:,j) d2p(:,j) d3p(:,j) d4p(:,j) ... 101 | b1(:,j) b2(:,j) b3(:,j) db1(:,j) db2(:,j) db3(:,j) d2b1(:,j) d2b2(:,j) d2b3(:,j)] = get_nom_traj(t(j)) ; 102 | end 103 | 104 | xLd_comp = cumtrapz_ic(t, vLd', xLd(:,1))' ; 105 | vLd_comp = cumtrapz_ic(t, aLd', vLd(:,1))' ; 106 | aLd_comp = cumtrapz_ic(t, daLd', aLd(:,1))' ; 107 | daLd_comp = cumtrapz_ic(t, d2aLd', daLd(:,1))' ; 108 | d2aLd_comp = cumtrapz_ic(t, d3aLd', d2aLd(:,1))' ; 109 | d3aLd_comp = cumtrapz_ic(t, d4aLd', d3aLd(:,1))' ; 110 | 111 | p_comp = cumtrapz_ic(t, dp', p(:,1))' ; 112 | dp_comp = cumtrapz_ic(t, d2p', dp(:,1))' ; 113 | d2p_comp = cumtrapz_ic(t, d3p', d2p(:,1))' ; 114 | d3p_comp = cumtrapz_ic(t, d4p', d3p(:,1))' ; 115 | 116 | b1_comp = cumtrapz_ic(t, db1', b1(:,1))' ; 117 | b2_comp = cumtrapz_ic(t, db2', b2(:,1))' ; 118 | b3_comp = cumtrapz_ic(t, db3', b3(:,1))' ; 119 | db1_comp = cumtrapz_ic(t, d2b1', db1(:,1))' ; 120 | db2_comp = cumtrapz_ic(t, d2b2', db2(:,1))' ; 121 | db3_comp = cumtrapz_ic(t, d2b3', db3(:,1))' ; 122 | 123 | close all 124 | figure ; plot(t, xLd, ':') ; hold on ; plot(t, xLd_comp) ; 125 | figure ; plot(t, vLd, ':') ; hold on ; plot(t, vLd_comp) ; 126 | figure ; plot(t, aLd, ':') ; hold on ; plot(t, aLd_comp) ; 127 | figure ; plot(t, daLd, ':') ; hold on ; plot(t, daLd_comp) ; 128 | figure ; plot(t, d2aLd, ':') ; hold on ; plot(t, d2aLd_comp) ; 129 | figure ; plot(t, d3aLd, ':') ; hold on ; plot(t, d3aLd_comp) ; 130 | 131 | figure ; plot(t, p, ':') ; hold on ; plot(t, p_comp) ; 132 | figure ; plot(t, dp, ':') ; hold on ; plot(t, dp_comp) ; 133 | figure ; plot(t, d2p, ':') ; hold on ; plot(t, d2p_comp) ; 134 | figure ; plot(t, d3p, ':') ; hold on ; plot(t, d3p_comp) ; 135 | 136 | figure ; plot(t, b1, ':') ; hold on ; plot(t, b1_comp) ; 137 | figure ; plot(t, b2, ':') ; hold on ; plot(t, b2_comp) ; 138 | figure ; plot(t, b3, ':') ; hold on ; plot(t, b3_comp) ; 139 | figure ; plot(t, db1, ':') ; hold on ; plot(t, db1_comp) ; 140 | figure ; plot(t, db2, ':') ; hold on ; plot(t, db2_comp) ; 141 | figure ; plot(t, db3, ':') ; hold on ; plot(t, db3_comp) ; 142 | end 143 | 144 | function x = cumtrapz_ic(t, dx, ic) 145 | x = cumtrapz(t, dx) ; 146 | for j=1:size(ic) 147 | x(:,j) = x(:,j) + ic(j) ; 148 | end 149 | end -------------------------------------------------------------------------------- /main.m: -------------------------------------------------------------------------------- 1 | %% Quadrotor Attitude Pendulum SO(3)x S(2)x R - Dynamics Simulation and Geometry Control 2 | close all; 3 | 4 | %% Parameters 5 | data.params.mQ = 0.5; 6 | % data.params.mL = 0.087; 7 | data.params.mL = 0.5; 8 | data.params.J = [2.32e-3,0,0;0,2.32e-3,0;0,0,4e-3]; 9 | data.params.g = 9.81; 10 | data.params.e1 = [1;0;0]; 11 | data.params.e2 = [0;1;0]; 12 | data.params.e3 = [0;0;1]; 13 | data.params.l = 1; 14 | 15 | %% Get initial condition from the nominal trajectory 16 | xL = [-3;-3;2]; 17 | vL = zeros(3,1); 18 | th = 90*pi/180; 19 | q = [-sin(th);0;cos(th)]; 20 | omega = [0;0;0]; 21 | R = eye(3,3); 22 | Omega = [0;0;0]; 23 | 24 | %% In-Sanity checks on initial condition 25 | if(abs(norm(q)-1) > 1e-2) 26 | disp('Error in q') ; keyboard ; 27 | end 28 | 29 | x_0 = [xL; vL; q; omega; reshape(R, 9,1); Omega]; 30 | 31 | %% Solving Dynamical Equations 32 | odeopts = odeset('RelTol',1e-6,'AbsTol',1e-6); 33 | [t, x] = ode45(@odefun_control, [0 10], x_0, odeopts, data); 34 | 35 | 36 | %% Compute various quantities 37 | 38 | disp('Computing State Variables and Configuration Errors ') ; 39 | ind = round(linspace(1, length(t), round(0.1*length(t)))) ; 40 | for j=ind 41 | [~, xLd_, Rd, qd_, f_, M_] = odefun_control(t(j), x(j,:)', data) ; 42 | [phi_d(j),theta_d(j),psi_d(j)] = RotToRPY_ZXY(Rd) ; 43 | [phi(j),theta(j),psi(j)] = RotToRPY_ZXY(reshape(x(j,13:21),3,3)); 44 | xLd(j,:)=xLd_'; 45 | M(j,:) = M_'; 46 | f(j,:) = f_'; 47 | qd(j,:) = qd_'; 48 | err_q(j) = 1 - qd_'*x(j, 7:9)'; 49 | err_R(j) = 0.5*trace(eye(3,3) - Rd'*reshape(x(j,13:21),3,3)); 50 | err_xL(j) = norm(x(j,1:3)-xLd_'); 51 | err_xL1(j) = x(j,1)-xLd_(1); 52 | err_xL2(j) = x(j,2)-xLd_(2); 53 | err_xL3(j) = x(j,3)-xLd_(3); 54 | end 55 | 56 | %% Plotting various states/outputs/inputs 57 | disp('Computing Plotting Figures') ; 58 | ind = round(linspace(1, length(t), round(0.1*length(t)))) ; 59 | 60 | % Input Moment 61 | figure; 62 | plot(t(ind), M(ind,1),t(ind), M(ind,2),t(ind), M(ind,3)) ; 63 | legend('M(1)','M(2)','M(3)');title('Quad-Moment'); 64 | grid on ; xlabel('Time (s)') ; 65 | 66 | % Input Thrust 67 | figure; 68 | plot(t(ind), f(ind,1)) ; 69 | legend('f'); title('Quad-Thrust'); 70 | grid on ; xlabel('Time (s)') 71 | 72 | % Configuration Errors 73 | figure ; plot(t(ind), err_R(ind)) ; 74 | grid on ; xlabel('Time (s)') ; title('psi-R') 75 | figure ; plot(t(ind), err_q(ind)) ; 76 | grid on ; xlabel('Time (s)') ; title('psi-q') 77 | figure; plot(t(ind),err_xL(ind)); 78 | grid on; xlabel('Time (s)'); title('position error'); 79 | figure; plot(t(ind),err_xL1(ind),t(ind),err_xL2(ind),t(ind),err_xL3(ind)) 80 | grid on; xlabel('Time (s)'); title('Position Error'); 81 | % Load Position 82 | figure ; plot(t, x(:, 1:3)) ; 83 | grid ; title('xL') ; legend('x','y','z') ; xlabel('Time (s)') ; 84 | 85 | % Quadrotor Attitude 86 | figure ; plot(t(ind), phi_d(ind)*180/pi, 'b:', t(ind), theta_d(ind)*180/pi, 'g:', t(ind), psi_d(ind)*180/pi, 'r:') ; 87 | hold on ; plot(t(ind), phi(ind)*180/pi, 'b', t(ind), theta(ind)*180/pi, 'g', t(ind), psi(ind)*180/pi, 'r') ; 88 | grid ; xlabel('Time (s)') ; ylabel('deg') ; title('Quad Atttitude') ; 89 | legend('phi_d','theta_d','psi_d','phi','theta','psi'); 90 | 91 | % Load Attitude 92 | figure ; plot(t(ind), qd(ind,1), 'b:',t(ind), qd(ind,2), 'g:',t(ind), qd(ind,3), 'r:') ; 93 | hold on ; plot(t(ind), x(ind, 7),'b',t(ind), x(ind, 8),'g',t(ind), x(ind, 9),'r') ; 94 | xlabel('Time (s)');title('Load Attitude') ; grid ; 95 | legend('q_d1','q_d2','q_d3','q_1','q_2','q_3'); 96 | 97 | %% Animation 98 | animate_3dquad_load(t, x, t(ind), xLd(ind,:)); -------------------------------------------------------------------------------- /odefun_control.m: -------------------------------------------------------------------------------- 1 | %% Geometric control design for quadrotor suspended load model 2 | function[dx, xLd, Rd, qd, f, M] = odefun_control(t,x,data) 3 | %% Constants 4 | mL = data.params.mL; 5 | g = data.params.g; 6 | mQ = data.params.mQ; 7 | J = data.params.J; 8 | e1 = data.params.e1; 9 | e2 = data.params.e2; 10 | e3 = data.params.e3; 11 | l = data.params.l; 12 | 13 | %% Desired States 14 | %---------------% 15 | 16 | % Case 1: Testing 17 | [xLd,vLd,aLd,qd,dqd,d2qd,~,Omegad,dOmegad] = get_nom_traj(data.params, get_load_traj(t)); 18 | 19 | %% Extracting States 20 | xL = x(1:3); 21 | vL = x(4:6); 22 | q = x(7:9); 23 | omega = x(10:12); 24 | dq = vec_cross(omega, q); 25 | R = reshape(x(13:21), 3,3); 26 | Omega = x(22:24); 27 | b3 = R(:,3); 28 | b1 = R(:,1); 29 | 30 | % LOAD POSITION TRACKING 31 | 32 | % Position errors 33 | eL = xL - xLd; 34 | deL = vL - vLd; 35 | 36 | epsilon_bar = 0.8; 37 | kp_xy = 0.3/epsilon_bar^2; kd_xy = 0.6/epsilon_bar; 38 | k1 = diag([kp_xy kp_xy 2]); k2 = diag([kd_xy kd_xy 1.5]); 39 | 40 | % PD force to track trajectory for Load with 41 | % feedforward 42 | A = (-k1*eL - k2*deL + (mQ+mL)*(aLd+g*e3) + mQ*l*vec_dot(dq,dq)*q); 43 | qd = -A/norm(A); 44 | 45 | epsilon_q = 0.5; 46 | kp = -1.5/epsilon_q^2; kom = -0.8/epsilon_q; 47 | err_q = hat_map(q)^2*qd; 48 | err_om = dq - vec_cross(vec_cross(qd, dqd), q); 49 | 50 | F_pd = -kp*err_q-kom*err_om; 51 | F_ff = (mQ*l)*vec_dot(q, vec_cross(qd,dqd))*vec_cross(q,dq)+... 52 | (mQ*l)*vec_cross( vec_cross(qd, d2qd), q); 53 | F_n = vec_dot(A,q)*q; 54 | 55 | F = F_pd - F_ff + F_n; 56 | 57 | b3c = F/norm(F); 58 | 59 | f = vec_dot(F, R(:,3)); 60 | 61 | % Load position 62 | xL_dot = vL; 63 | vL_dot = 1/(mQ+mL)*((vec_dot(q,f*b3)-mQ*l*vec_dot(dq,dq))*q-(mQ+mL)*g*e3); 64 | 65 | if(abs(norm(qd)-1) > 1e-2) 66 | disp('Error in pd'); keyboard; 67 | end 68 | 69 | % Load Attitude 70 | q_dot = dq; 71 | omega_dot = 1/(mQ*l) * vec_cross(-q, f*b3); 72 | 73 | % DESIRED YAW DIRECTION 74 | b1d = e1; 75 | b1c = -vec_cross(b3c,vec_cross(b3c,b1d))/norm(vec_cross(b3c,b1d)); 76 | Rc = [b1c, vec_cross(b3c,b1c),b3c]; 77 | Rd = Rc; 78 | if(norm(Rd'*Rd-eye(3)) > 1e-2) 79 | disp('Error in R'); keyboard; 80 | end 81 | kR = 4 ; kOm = 4 ; 82 | epsilon = 0.1 ; %.5 ; %0.01 ; 83 | 84 | err_R = 1/2 * vee_map(Rd'*R - R'*Rd); 85 | err_Om = Omega - R'*Rd*Omegad; 86 | M = -kR/epsilon^2*err_R - kOm/epsilon*err_Om + vec_cross(Omega, J*Omega)... 87 | - J*(hat_map(Omega)*R'*Rd*Omegad - R'*Rd*dOmegad); 88 | 89 | % Quadrotor Attitude 90 | R_dot = R*hat_map(Omega); 91 | Omega_dot = J\( -vec_cross(Omega, J*Omega) + M ); 92 | 93 | %% Output 94 | dx = [xL_dot; vL_dot; q_dot; omega_dot; reshape(R_dot, 9,1); Omega_dot]; 95 | 96 | if nargout <= 1 97 | fprintf('Simulation time %0.4f seconds \n',t); 98 | end 99 | 100 | end --------------------------------------------------------------------------------