├── .gitignore ├── CITATION.cff ├── README.md ├── control ├── controller.m ├── crazyflie.m ├── quadEOM.m ├── quadEOM_readonly.m ├── runsim.m ├── trajectories │ ├── circle.m │ ├── diamond.m │ ├── step.m │ └── tj_from_line.m └── utils │ ├── QuadPlot.m │ ├── QuatToRot.m │ ├── RPYtoRot_ZXY.m │ ├── RotToQuat.m │ ├── RotToRPY_ZXY.m │ ├── collision_check.m │ ├── init_state.m │ ├── plot_state.m │ ├── qdToState.m │ ├── quad_pos.m │ ├── stateToQd.m │ └── terminate_check.m ├── gifs ├── p1p1_circle.gif ├── p1p1_diamond.gif ├── p1p1_step.gif ├── p1p3_map1.gif ├── p1p3_map1_acc.gif ├── p1p3_map1_snap.gif ├── p1p3_map2.gif ├── p1p3_map3.gif ├── p1p3_map3_mini_acc.gif └── p1p3_map3_snap.gif ├── imgs ├── hover.jpg ├── p1p1_circle_p.jpg ├── p1p1_circle_v.jpg ├── p1p1_diamond_p.jpg ├── p1p1_diamond_v.jpg ├── p1p1_step_p.jpg ├── p1p1_step_v.jpg ├── p1p3_map1_acc_p.jpg ├── p1p3_map1_acc_v.jpg ├── p1p3_map1_p.jpg ├── p1p3_map1_snap.jpg ├── p1p3_map1_snap_p.jpg ├── p1p3_map1_snap_v.jpg ├── p1p3_map1_v.jpg ├── p1p3_map2_p.jpg ├── p1p3_map2_v.jpg ├── p1p3_map3_mini_acc_p.jpg ├── p1p3_map3_mini_acc_v.jpg ├── p1p3_map3_p.jpg ├── p1p3_map3_snap_p.jpg ├── p1p3_map3_snap_v.jpg └── p1p3_map3_v.jpg ├── project_report.pdf ├── quadrotor_dynamics.pdf └── traj_planning ├── controller.m ├── crazyflie.m ├── generate_ts.m ├── init_script.m ├── maps ├── map1.txt ├── map2.txt ├── map3.txt ├── map_empty.txt └── map_one_block.txt ├── path_planning ├── collide.m ├── dijkstra.m ├── idx_to_points.m ├── load_map.m ├── plot_path.m ├── plot_path1.m ├── plot_path3.m └── points_to_idx.m ├── quadEOM.m ├── quadEOM_readonly.m ├── runsim.m ├── simplify_path.m ├── simplify_path2.m ├── test_trajectory.m ├── traj_opt3.m ├── traj_opt3c.m ├── traj_opt7.m ├── traj_opt7c.m ├── trajectory_generator.m └── utils ├── QuadPlot.m ├── QuatToRot.m ├── RPYtoRot_ZXY.m ├── RotToQuat.m ├── RotToRPY_ZXY.m ├── collision_check.m ├── init_state.m ├── plot_state.m ├── qdToState.m ├── quad_pos.m ├── stateToQd.m └── terminate_check.m /.gitignore: -------------------------------------------------------------------------------- 1 | *.DS_Store 2 | *~ 3 | *.mat 4 | *.zip 5 | *.avi 6 | *.mov -------------------------------------------------------------------------------- /CITATION.cff: -------------------------------------------------------------------------------- 1 | cff-version: 1.2.0 2 | message: "If you use this software, please cite it as below." 3 | authors: 4 | - family-names: "Lu" 5 | given-names: "Yiren" 6 | orcid: "https://orcid.org/0000-0002-7924-2488" 7 | title: "yrlu/quadrotor: Quadrotor Control, Path Planning and Trajectory Optimization" 8 | version: 1.0.0 9 | doi: 10.5281/zenodo.6796215 10 | date-released: 2017-07-01 11 | url: "https://github.com/yrlu/quadrotor" -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ## Quadrotor Control, Path Planning and Trajectory Optimization 2 | 3 | [![DOI](https://zenodo.org/badge/DOI/10.5281/zenodo.6796215.svg)](https://doi.org/10.5281/zenodo.6796215) 4 | 5 | 6 | step 7 | 8 | 9 | (Click the image above to watch real quadrotor demonstrations) 10 | 11 | Following [MEAM 620 Advanced Robotics](https://alliance.seas.upenn.edu/~meam620/wiki/) course at University of Pennsylvania. 12 | 13 | 🚫 For Penn students: Please *DO NOT* spoil the learning experience by using this repository as a shortcut for your assignments. Most importantly, *DO NOT* violate the honor code! 14 | 15 | ### Repository Contents 16 | 17 | This repository contains MATLAB code for: 18 | - Quadrotor PD controllers 19 | - Path planning algorithms (Dijkstra, A*) 20 | - Trajectory optimization algorithms (Minimum Snap/Acceleration Trajectory) 21 | 22 | If you use this software in your publications, please cite it using the following BibTeX entry: 23 | 24 | ```bibtex 25 | @misc{lu2017quadrotor, 26 | author = {Lu, Yiren and Cai, Myles and Ling, Wudao and Zhou, Xuanyu}, 27 | doi = {10.5281/zenodo.6796215}, 28 | month = {7}, 29 | title = {{Quadrotor control, path planning and trajectory optimization}}, 30 | url = {https://github.com/yrlu/quadrotor}, 31 | year = {2017} 32 | } 33 | ``` 34 | 35 | ## PD Controller 36 | 37 | - Run code: change trajectories in file `control/runsim.m` and run. 38 | - See [quadrotor_dynamics.pdf](quadrotor_dynamics.pdf) for dynamic modeling of the quadrotor. 39 | - See `control/controller.m` for implementation of the PD controller. 40 | - Visualization below. Desired (blue) vs Actual (red) 41 | 42 | #### Trajectory 1: Step 43 | 44 | step step step 45 | 46 | 47 | #### Trajectory 2: Circle 48 | 49 | step step step 50 | 51 | #### Trajectory 2: Diamond 52 | 53 | step step step 54 | 55 | ## Path Planning and Trajectory Optimization 56 | 57 | - Run code: `traj_planning/runsim.m` and run path 1 or path 3. 58 | - See [project_report.pdf](project_report.pdf) for more details about trajectory generation 59 | - See `traj_planning/path_planning/dijkstra.m` for implementation of path finding algorithms (dijstra, A*). 60 | - See `traj_planning/traj_opt7.m` for implementations of minimium snap trajectory. 61 | - See `traj_planning/traj_opt5.m` for implementations of minimium acceleration trajectory. 62 | - Visualization below. 63 | 64 | #### Minimum Acceleration Trajectory 65 | 66 | step step step 67 | 68 | step step step 69 | 70 | 71 | #### Minimum Snap Trajectory 72 | 73 | step step step 74 | 75 | (with way points constraints) 76 | 77 | step step step 78 | -------------------------------------------------------------------------------- /control/controller.m: -------------------------------------------------------------------------------- 1 | function [F, M, trpy, drpy] = controller(qd, t, qn, params) 2 | % CONTROLLER quadrotor controller 3 | % The current states are: 4 | % qd{qn}.pos, qd{qn}.vel, qd{qn}.euler = [roll;pitch;yaw], qd{qn}.omega 5 | % The desired states are: 6 | % qd{qn}.pos_des, qd{qn}.vel_des, qd{qn}.acc_des, qd{qn}.yaw_des, qd{qn}.yawdot_des 7 | % Using these current and desired states, you have to compute the desired controls 8 | 9 | % position controller params 10 | Kp = [15;15;30]; 11 | Kd = [12;12;10]; 12 | 13 | % attitude controller params 14 | KpM = ones(3,1)*3000; 15 | KdM = ones(3,1)*300; 16 | 17 | acc_des = qd{qn}.acc_des + Kd.*(qd{qn}.vel_des - qd{qn}.vel) + Kp.*(qd{qn}.pos_des - qd{qn}.pos); 18 | 19 | % Desired roll, pitch and yaw 20 | phi_des = 1/params.grav * (acc_des(1)*sin(qd{qn}.yaw_des) - acc_des(2)*cos(qd{qn}.yaw_des)); 21 | theta_des = 1/params.grav * (acc_des(1)*cos(qd{qn}.yaw_des) + acc_des(2)*sin(qd{qn}.yaw_des)); 22 | psi_des = qd{qn}.yaw_des; 23 | 24 | euler_des = [phi_des;theta_des;psi_des]; 25 | pqr_des = [0;0; qd{qn}.yawdot_des]; 26 | % Thurst 27 | qd{qn}.acc_des(3); 28 | F = params.mass*(params.grav + acc_des(3)); 29 | % Moment 30 | M = params.I*(KdM.*(pqr_des - qd{qn}.omega) + KpM.*(euler_des - qd{qn}.euler)); 31 | % =================== Your code ends here =================== 32 | 33 | % Output trpy and drpy as in hardware 34 | trpy = [F, phi_des, theta_des, psi_des]; 35 | drpy = [0, 0, 0, 0]; 36 | 37 | end 38 | -------------------------------------------------------------------------------- /control/crazyflie.m: -------------------------------------------------------------------------------- 1 | function params = crazyflie() 2 | % crazyflie: physical parameters for the Crazyflie 2.0 3 | % 4 | % 2016 Bernd Pfrommer 5 | % 6 | % This function creates a struct with the basic parameters for the 7 | % Crazyflie 2.0 quad rotor (without camera, but with about 5 vicon 8 | % markers) 9 | % 10 | % Model assumptions based on physical measurements: 11 | % 12 | % motor + mount + vicon marker = mass point of 3g 13 | % arm length of mass point: 0.046m from center 14 | % battery pack + main board are combined into cuboid (mass 18g) of 15 | % dimensions: 16 | % 17 | % width = 0.03m 18 | % depth = 0.03m 19 | % height = 0.012m 20 | % 21 | 22 | m = 0.030; % weight (in kg) with 5 vicon markers (each is about 0.25g) 23 | g = 9.81; % gravitational constant 24 | I = [1.43e-5, 0, 0; % inertial tensor in m^2 kg 25 | 0, 1.43e-5, 0; 26 | 0, 0, 2.89e-5]; 27 | L = 0.046; % arm length in m 28 | 29 | params.mass = m; 30 | params.I = I; 31 | params.invI = inv(I); 32 | params.grav = g; 33 | params.arm_length = L; 34 | 35 | params.maxangle = 40*pi/180; % you can specify the maximum commanded angle here 36 | params.maxF = 2.5*m*g; % left these untouched from the nano plus 37 | params.minF = 0.05*m*g; % left these untouched from the nano plus 38 | 39 | % You can add any fields you want in params 40 | % for example you can add your controller gains by 41 | % params.k = 0, and they will be passed into controller.m 42 | 43 | end 44 | -------------------------------------------------------------------------------- /control/quadEOM.m: -------------------------------------------------------------------------------- 1 | function sdot = quadEOM(t, s, qn, controlhandle, trajhandle, params) 2 | % QUADEOM Wrapper function for solving quadrotor equation of motion 3 | % quadEOM takes in time, state vector, controller, trajectory generator 4 | % and parameters and output the derivative of the state vector, the 5 | % actual calcution is done in quadEOM_readonly. 6 | % 7 | % INPUTS: 8 | % t - 1 x 1, time 9 | % s - 13 x 1, state vector = [x, y, z, xd, yd, zd, qw, qx, qy, qz, p, q, r] 10 | % qn - quad number (used for multi-robot simulations) 11 | % controlhandle - function handle of your controller 12 | % trajhandle - function handle of your trajectory generator 13 | % params - struct, output from crazyflie() and whatever parameters you want to pass in 14 | % 15 | % OUTPUTS: 16 | % sdot - 13 x 1, derivative of state vector s 17 | % 18 | % NOTE: You should not modify this function 19 | % See Also: quadEOM_readonly, crazyflie 20 | 21 | % convert state to quad stuct for control 22 | qd{qn} = stateToQd(s); 23 | 24 | % Get desired_state 25 | desired_state = trajhandle(t, qn); 26 | 27 | % The desired_state is set in the trajectory generator 28 | qd{qn}.pos_des = desired_state.pos; 29 | qd{qn}.vel_des = desired_state.vel; 30 | qd{qn}.acc_des = desired_state.acc; 31 | qd{qn}.yaw_des = desired_state.yaw; 32 | qd{qn}.yawdot_des = desired_state.yawdot; 33 | 34 | % get control outputs 35 | [F, M, trpy, drpy] = controlhandle(qd, t, qn, params); 36 | 37 | % compute derivative 38 | sdot = quadEOM_readonly(t, s, F, M, params); 39 | 40 | end 41 | -------------------------------------------------------------------------------- /control/quadEOM_readonly.m: -------------------------------------------------------------------------------- 1 | function sdot = quadEOM_readonly(t, s, F, M, params) 2 | % QUADEOM_READONLY Solve quadrotor equation of motion 3 | % quadEOM_readonly calculate the derivative of the state vector 4 | % 5 | % INPUTS: 6 | % t - 1 x 1, time 7 | % s - 13 x 1, state vector = [x, y, z, xd, yd, zd, qw, qx, qy, qz, p, q, r] 8 | % F - 1 x 1, thrust output from controller (only used in simulation) 9 | % M - 3 x 1, moments output from controller (only used in simulation) 10 | % params - struct, output from crazyflie() and whatever parameters you want to pass in 11 | % 12 | % OUTPUTS: 13 | % sdot - 13 x 1, derivative of state vector s 14 | % 15 | % NOTE: You should not modify this function 16 | % See Also: quadEOM_readonly, crazyflie 17 | 18 | %************ EQUATIONS OF MOTION ************************ 19 | % Limit the force and moments due to actuator limits 20 | A = [0.25, 0, -0.5/params.arm_length; 21 | 0.25, 0.5/params.arm_length, 0; 22 | 0.25, 0, 0.5/params.arm_length; 23 | 0.25, -0.5/params.arm_length, 0]; 24 | 25 | prop_thrusts = A*[F;M(1:2)]; % Not using moment about Z-axis for limits 26 | prop_thrusts_clamped = max(min(prop_thrusts, params.maxF/4), params.minF/4); 27 | 28 | B = [ 1, 1, 1, 1; 29 | 0, params.arm_length, 0, -params.arm_length; 30 | -params.arm_length, 0, params.arm_length, 0]; 31 | F = B(1,:)*prop_thrusts_clamped; 32 | M = [B(2:3,:)*prop_thrusts_clamped; M(3)]; 33 | 34 | % Assign states 35 | x = s(1); 36 | y = s(2); 37 | z = s(3); 38 | xdot = s(4); 39 | ydot = s(5); 40 | zdot = s(6); 41 | qW = s(7); 42 | qX = s(8); 43 | qY = s(9); 44 | qZ = s(10); 45 | p = s(11); 46 | q = s(12); 47 | r = s(13); 48 | 49 | quat = [qW; qX; qY; qZ]; 50 | bRw = QuatToRot(quat); 51 | wRb = bRw'; 52 | 53 | % Acceleration 54 | accel = 1 / params.mass * (wRb * [0; 0; F] - [0; 0; params.mass * params.grav]); 55 | 56 | % Angular velocity 57 | K_quat = 2; %this enforces the magnitude 1 constraint for the quaternion 58 | quaterror = 1 - (qW^2 + qX^2 + qY^2 + qZ^2); 59 | qdot = -1/2*[0, -p, -q, -r;... 60 | p, 0, -r, q;... 61 | q, r, 0, -p;... 62 | r, -q, p, 0] * quat + K_quat*quaterror * quat; 63 | 64 | % Angular acceleration 65 | omega = [p;q;r]; 66 | pqrdot = params.invI * (M - cross(omega, params.I*omega)); 67 | 68 | % Assemble sdot 69 | sdot = zeros(13,1); 70 | sdot(1) = xdot; 71 | sdot(2) = ydot; 72 | sdot(3) = zdot; 73 | sdot(4) = accel(1); 74 | sdot(5) = accel(2); 75 | sdot(6) = accel(3); 76 | sdot(7) = qdot(1); 77 | sdot(8) = qdot(2); 78 | sdot(9) = qdot(3); 79 | sdot(10) = qdot(4); 80 | sdot(11) = pqrdot(1); 81 | sdot(12) = pqrdot(2); 82 | sdot(13) = pqrdot(3); 83 | 84 | end 85 | -------------------------------------------------------------------------------- /control/runsim.m: -------------------------------------------------------------------------------- 1 | % NOTE: This srcipt will not run as expected unless you fill in proper 2 | % code in trajhandle and controlhandle 3 | % You should not modify any part of this script except for the 4 | % visualization part 5 | % 6 | % ***************** MEAM 620 QUADROTOR SIMULATION ***************** 7 | close all 8 | clear all 9 | addpath('utils') 10 | addpath('trajectories') 11 | 12 | % You can change trajectory here 13 | 14 | % trajectory generator 15 | % trajhandle = @step; 16 | % trajhandle = @circle; 17 | trajhandle = @diamond; 18 | 19 | % controller 20 | controlhandle = @controller; 21 | 22 | % real-time 23 | real_time = true; 24 | 25 | % *********** YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW ********** 26 | % number of quadrotors 27 | nquad = 1; 28 | 29 | % max time 30 | time_tol = 25; 31 | 32 | % parameters for simulation 33 | params = crazyflie(); 34 | 35 | %% **************************** FIGURES ***************************** 36 | fprintf('Initializing figures...\n') 37 | h_fig = figure; 38 | h_3d = gca; 39 | axis equal 40 | grid on 41 | view(3); 42 | xlabel('x [m]'); ylabel('y [m]'); zlabel('z [m]') 43 | quadcolors = lines(nquad); 44 | 45 | set(gcf,'Renderer','OpenGL') 46 | 47 | %% *********************** INITIAL CONDITIONS *********************** 48 | fprintf('Setting initial conditions...\n') 49 | max_iter = 5000; % max iteration 50 | starttime = 0; % start of simulation in seconds 51 | tstep = 0.01; % this determines the time step at which the solution is given 52 | cstep = 0.05; % image capture time interval 53 | nstep = cstep/tstep; 54 | time = starttime; % current time 55 | err = []; % runtime errors 56 | for qn = 1:nquad 57 | % Get start and stop position 58 | des_start = trajhandle(0, qn); 59 | des_stop = trajhandle(inf, qn); 60 | stop{qn} = des_stop.pos; 61 | x0{qn} = init_state( des_start.pos, 0 ); 62 | xtraj{qn} = zeros(max_iter*nstep, length(x0{qn})); 63 | ttraj{qn} = zeros(max_iter*nstep, 1); 64 | end 65 | 66 | x = x0; % state 67 | 68 | pos_tol = 0.01; 69 | vel_tol = 0.01; 70 | 71 | %% ************************* RUN SIMULATION ************************* 72 | OUTPUT_TO_VIDEO = 1; 73 | if OUTPUT_TO_VIDEO == 1 74 | v = VideoWriter('diamond.avi'); 75 | open(v) 76 | end 77 | 78 | fprintf('Simulation Running....') 79 | % Main loop 80 | for iter = 1:max_iter 81 | iter; 82 | timeint = time:tstep:time+cstep; 83 | 84 | tic; 85 | % Iterate over each quad 86 | for qn = 1:nquad 87 | % Initialize quad plot 88 | if iter == 1 89 | QP{qn} = QuadPlot(qn, x0{qn}, 0.1, 0.04, quadcolors(qn,:), max_iter, h_3d); 90 | desired_state = trajhandle(time, qn); 91 | QP{qn}.UpdateQuadPlot(x{qn}, [desired_state.pos; desired_state.vel], time); 92 | h_title = title(sprintf('iteration: %d, time: %4.2f', iter, time)); 93 | end 94 | 95 | % Run simulation 96 | [tsave, xsave] = ode45(@(t,s) quadEOM(t, s, qn, controlhandle, trajhandle, params), timeint, x{qn}); 97 | x{qn} = xsave(end, :)'; 98 | 99 | % Save to traj 100 | xtraj{qn}((iter-1)*nstep+1:iter*nstep,:) = xsave(1:end-1,:); 101 | ttraj{qn}((iter-1)*nstep+1:iter*nstep) = tsave(1:end-1); 102 | 103 | % Update quad plot 104 | desired_state = trajhandle(time + cstep, qn); 105 | QP{qn}.UpdateQuadPlot(x{qn}, [desired_state.pos; desired_state.vel], time + cstep); 106 | set(h_title, 'String', sprintf('iteration: %d, time: %4.2f', iter, time + cstep)) 107 | if OUTPUT_TO_VIDEO == 1 108 | im = frame2im(getframe(gcf)); 109 | writeVideo(v,im); 110 | end 111 | end 112 | time = time + cstep; % Update simulation time 113 | t = toc; 114 | % Check to make sure ode45 is not timing out 115 | if(t> cstep*50) 116 | err = 'Ode45 Unstable'; 117 | break; 118 | end 119 | 120 | % Pause to make real-time 121 | if real_time && (t < cstep) 122 | pause(cstep - t); 123 | end 124 | 125 | % Check termination criteria 126 | if terminate_check(x, time, stop, pos_tol, vel_tol, time_tol) 127 | break 128 | end 129 | end 130 | 131 | if OUTPUT_TO_VIDEO == 1 132 | close(v); 133 | end 134 | 135 | %% ************************* POST PROCESSING ************************* 136 | % Truncate xtraj and ttraj 137 | for qn = 1:nquad 138 | xtraj{qn} = xtraj{qn}(1:iter*nstep,:); 139 | ttraj{qn} = ttraj{qn}(1:iter*nstep); 140 | end 141 | 142 | % Plot the saved position and velocity of each robot 143 | for qn = 1:nquad 144 | % Truncate saved variables 145 | QP{qn}.TruncateHist(); 146 | % Plot position for each quad 147 | h_pos{qn} = figure('Name', ['Quad ' num2str(qn) ' : position']); 148 | plot_state(h_pos{qn}, QP{qn}.state_hist(1:3,:), QP{qn}.time_hist, 'pos', 'vic'); 149 | plot_state(h_pos{qn}, QP{qn}.state_des_hist(1:3,:), QP{qn}.time_hist, 'pos', 'des'); 150 | % Plot velocity for each quad 151 | h_vel{qn} = figure('Name', ['Quad ' num2str(qn) ' : velocity']); 152 | plot_state(h_vel{qn}, QP{qn}.state_hist(4:6,:), QP{qn}.time_hist, 'vel', 'vic'); 153 | plot_state(h_vel{qn}, QP{qn}.state_des_hist(4:6,:), QP{qn}.time_hist, 'vel', 'des'); 154 | end 155 | if(~isempty(err)) 156 | error(err); 157 | end 158 | 159 | fprintf('finished.\n') 160 | -------------------------------------------------------------------------------- /control/trajectories/circle.m: -------------------------------------------------------------------------------- 1 | function [desired_state] = circle(t, qn) 2 | % CIRCLE trajectory generator for a circle 3 | 4 | % =================== Your code goes here =================== 5 | % You have to set the pos, vel, acc, yaw and yawdot variables 6 | time_tol = 12; 7 | radius = 5; 8 | dt = 0.0001; 9 | 10 | function pos = pos_from_angle(a) 11 | pos = [radius*cos(a); radius*sin(a); 2.5*a/(2*pi)]; 12 | end 13 | 14 | function vel = get_vel(t) 15 | angle1 = tj_from_line(0, 2*pi, time_tol, t); 16 | pos1 = pos_from_angle(angle1); 17 | angle2 = tj_from_line(0, 2*pi, time_tol, t+dt); 18 | vel = (pos_from_angle(angle2) - pos1)/dt; 19 | end 20 | 21 | if t > time_tol 22 | pos = [radius; 0; 2.5]; 23 | vel = [0;0;0]; 24 | acc = [0;0;0]; 25 | else 26 | angle = tj_from_line(0, 2*pi, time_tol, t); 27 | pos = pos_from_angle(angle); 28 | vel = get_vel(t); 29 | acc = (get_vel(t+dt) - get_vel(t))/dt; 30 | end 31 | 32 | yaw = 0; 33 | yawdot = 0; 34 | % =================== Your code ends here =================== 35 | 36 | desired_state.pos = pos(:); 37 | desired_state.vel = vel(:); 38 | desired_state.acc = acc(:); 39 | desired_state.yaw = yaw; 40 | desired_state.yawdot = yawdot; 41 | 42 | end 43 | -------------------------------------------------------------------------------- /control/trajectories/diamond.m: -------------------------------------------------------------------------------- 1 | function [desired_state] = diamond(t, qn) 2 | % DIAMOND trajectory generator for a diamond 3 | 4 | % =================== Your code goes here =================== 5 | % You have to set the pos, vel, acc, yaw and yawdot variables 6 | 7 | time_tol = 12; 8 | dt = 0.0001; 9 | 10 | function [pos, vel] = get_pos_vel(t) 11 | if t >= time_tol 12 | pos = [1;0;0]; 13 | vel = [0;0;0]; 14 | elseif t >= 0 & t < time_tol/4 15 | % from [0;0;0] to [1/4;sqrt(2);sqrt(2)] 16 | [pos, vel, ~] = tj_from_line([0;0;0], [1/4;sqrt(2);sqrt(2)], time_tol/4, t); 17 | elseif t >= time_tol/4 & t < time_tol/2 18 | % from [1/4;sqrt(2);sqrt(2)] to [1/2;0;2*sqrt(2)] 19 | [pos, vel, ~] = tj_from_line([1/4;sqrt(2);sqrt(2)], [1/2;0;2*sqrt(2)], time_tol/4, t-time_tol/4); 20 | elseif t >= time_tol/2 & t < time_tol*3/4 21 | % from [1/2;0;2*sqrt(2)] to [3/4; -sqrt(2); sqrt(2)] 22 | [pos, vel, ~] = tj_from_line([1/2;0;2*sqrt(2)], [3/4; -sqrt(2); sqrt(2)], time_tol/4, t-time_tol/2); 23 | else 24 | % from [3/4; -sqrt(2); sqrt(2)] to [1;0;0] 25 | [pos, vel, ~] = tj_from_line([3/4; -sqrt(2); sqrt(2)], [1;0;0], time_tol/4, t-time_tol*3/4); 26 | end 27 | end 28 | 29 | if t >= time_tol 30 | pos = [1;0;0]; 31 | vel = [0;0;0]; 32 | acc = [0;0;0]; 33 | else 34 | [pos, vel] = get_pos_vel(t); 35 | [~, vel1] = get_pos_vel(t+dt); 36 | acc = (vel1-vel)/dt; 37 | end 38 | 39 | yaw = 0; 40 | yawdot = 0; 41 | 42 | % =================== Your code ends here =================== 43 | 44 | desired_state.pos = pos(:); 45 | desired_state.vel = vel(:); 46 | desired_state.acc = acc(:); 47 | desired_state.yaw = yaw; 48 | desired_state.yawdot = yawdot; 49 | 50 | end 51 | -------------------------------------------------------------------------------- /control/trajectories/step.m: -------------------------------------------------------------------------------- 1 | function [desired_state] = step(t, qn) 2 | % Hover trajectory generator for a circle 3 | 4 | % =================== Your code goes here =================== 5 | % You have to set the pos, vel, acc, yaw and yawdot variables 6 | time_tol = 5; 7 | length = 5; 8 | 9 | if t <= 0 10 | pos = [0;0;0]; 11 | vel = [0;0;0]; 12 | acc = [0;0;0]; 13 | else 14 | pos = [1;0;0]; 15 | vel = [0;0;0]; 16 | acc = [0;0;0]; 17 | end 18 | 19 | yaw = 0; 20 | yawdot = 0; 21 | % =================== Your code ends here =================== 22 | 23 | desired_state.pos = pos(:); 24 | desired_state.vel = vel(:); 25 | desired_state.acc = acc(:); 26 | desired_state.yaw = yaw; 27 | desired_state.yawdot = yawdot; 28 | 29 | end 30 | -------------------------------------------------------------------------------- /control/trajectories/tj_from_line.m: -------------------------------------------------------------------------------- 1 | function [pos, vel, acc] = tj_from_line(start_pos, end_pos, time_ttl, t_c) 2 | v_max = (end_pos-start_pos)*2/time_ttl; 3 | if t_c >= 0 & t_c < time_ttl/2 4 | vel = v_max*t_c/(time_ttl/2); 5 | pos = start_pos + t_c*vel/2; 6 | acc = [0;0;0]; 7 | else 8 | vel = v_max*(time_ttl-t_c)/(time_ttl/2); 9 | pos = end_pos - (time_ttl-t_c)*vel/2; 10 | acc = [0;0;0]; 11 | end 12 | end -------------------------------------------------------------------------------- /control/utils/QuadPlot.m: -------------------------------------------------------------------------------- 1 | classdef QuadPlot < handle 2 | %QUADPLOT Visualization class for quad 3 | 4 | properties (SetAccess = public) 5 | k = 0; 6 | qn; % quad number 7 | time = 0; % time 8 | state; % state 9 | des_state; % desried state [x; y; z; xdot; ydot; zdot]; 10 | rot; % rotation matrix body to world 11 | 12 | color; % color of quad 13 | wingspan; % wingspan 14 | height; % height of quad 15 | motor; % motor position 16 | 17 | state_hist % position history 18 | state_des_hist; % desired position history 19 | time_hist; % time history 20 | max_iter; % max iteration 21 | end 22 | 23 | properties (SetAccess = private) 24 | h_3d 25 | h_m13; % motor 1 and 3 handle 26 | h_m24; % motor 2 and 4 handle 27 | h_qz; % z axis of quad handle 28 | h_qn; % quad number handle 29 | h_pos_hist; % position history handle 30 | h_pos_des_hist; % desired position history handle 31 | text_dist; % distance of quad number to quad 32 | end 33 | 34 | methods 35 | % Constructor 36 | function Q = QuadPlot(qn, state, wingspan, height, color, max_iter, h_3d) 37 | Q.qn = qn; 38 | Q.state = state; 39 | Q.wingspan = wingspan; 40 | Q.color = color; 41 | Q.height = height; 42 | Q.rot = QuatToRot(Q.state(7:10)); 43 | Q.motor = quad_pos(Q.state(1:3), Q.rot, Q.wingspan, Q.height); 44 | Q.text_dist = Q.wingspan / 3; 45 | Q.des_state = Q.state(1:6); 46 | 47 | Q.max_iter = max_iter; 48 | Q.state_hist = zeros(6, max_iter); 49 | Q.state_des_hist = zeros(6, max_iter); 50 | Q.time_hist = zeros(1, max_iter); 51 | 52 | % Initialize plot handle 53 | if nargin < 7, h_3d = gca; end 54 | Q.h_3d = h_3d; 55 | hold(Q.h_3d, 'on') 56 | Q.h_pos_hist = plot3(Q.h_3d, Q.state(1), Q.state(2), Q.state(3), 'r.'); 57 | Q.h_pos_des_hist = plot3(Q.h_3d, Q.des_state(1), Q.des_state(2), Q.des_state(3), 'b.'); 58 | Q.h_m13 = plot3(Q.h_3d, ... 59 | Q.motor(1,[1 3]), ... 60 | Q.motor(2,[1 3]), ... 61 | Q.motor(3,[1 3]), ... 62 | '-ko', 'MarkerFaceColor', Q.color, 'MarkerSize', 5); 63 | Q.h_m24 = plot3(Q.h_3d, ... 64 | Q.motor(1,[2 4]), ... 65 | Q.motor(2,[2 4]), ... 66 | Q.motor(3,[2 4]), ... 67 | '-ko', 'MarkerFaceColor', Q.color, 'MarkerSize', 5); 68 | Q.h_qz = plot3(Q.h_3d, ... 69 | Q.motor(1,[5 6]), ... 70 | Q.motor(2,[5 6]), ... 71 | Q.motor(3,[5 6]), ... 72 | 'Color', Q.color, 'LineWidth', 2); 73 | Q.h_qn = text(... 74 | Q.motor(1,5) + Q.text_dist, ... 75 | Q.motor(2,5) + Q.text_dist, ... 76 | Q.motor(3,5) + Q.text_dist, num2str(qn)); 77 | hold(Q.h_3d, 'off') 78 | end 79 | 80 | % Update quad state 81 | function UpdateQuadState(Q, state, time) 82 | Q.state = state; 83 | Q.time = time; 84 | Q.rot = QuatToRot(state(7:10))'; % Q.rot needs to be body-to-world 85 | end 86 | 87 | % Update desired quad state 88 | function UpdateDesiredQuadState(Q, des_state) 89 | Q.des_state = des_state; 90 | end 91 | 92 | % Update quad history 93 | function UpdateQuadHist(Q) 94 | Q.k = Q.k + 1; 95 | Q.time_hist(Q.k) = Q.time; 96 | Q.state_hist(:,Q.k) = Q.state(1:6); 97 | Q.state_des_hist(:,Q.k) = Q.des_state(1:6); 98 | end 99 | 100 | % Update motor position 101 | function UpdateMotorPos(Q) 102 | Q.motor = quad_pos(Q.state(1:3), Q.rot, Q.wingspan, Q.height); 103 | end 104 | 105 | % Truncate history 106 | function TruncateHist(Q) 107 | Q.time_hist = Q.time_hist(1:Q.k); 108 | Q.state_hist = Q.state_hist(:, 1:Q.k); 109 | Q.state_des_hist = Q.state_des_hist(:, 1:Q.k); 110 | end 111 | 112 | % Update quad plot 113 | function UpdateQuadPlot(Q, state, des_state, time) 114 | Q.UpdateQuadState(state, time); 115 | Q.UpdateDesiredQuadState(des_state); 116 | Q.UpdateQuadHist(); 117 | Q.UpdateMotorPos(); 118 | set(Q.h_m13, ... 119 | 'XData', Q.motor(1,[1 3]), ... 120 | 'YData', Q.motor(2,[1 3]), ... 121 | 'ZData', Q.motor(3,[1 3])); 122 | set(Q.h_m24, ... 123 | 'XData', Q.motor(1,[2 4]), ... 124 | 'YData', Q.motor(2,[2 4]), ... 125 | 'ZData', Q.motor(3,[2 4])); 126 | set(Q.h_qz, ... 127 | 'XData', Q.motor(1,[5 6]), ... 128 | 'YData', Q.motor(2,[5 6]), ... 129 | 'ZData', Q.motor(3,[5 6])) 130 | set(Q.h_qn, 'Position', ... 131 | [Q.motor(1,5) + Q.text_dist, ... 132 | Q.motor(2,5) + Q.text_dist, ... 133 | Q.motor(3,5) + Q.text_dist]); 134 | set(Q.h_pos_hist, ... 135 | 'XData', Q.state_hist(1,1:Q.k), ... 136 | 'YData', Q.state_hist(2,1:Q.k), ... 137 | 'ZData', Q.state_hist(3,1:Q.k)); 138 | set(Q.h_pos_des_hist, ... 139 | 'XData', Q.state_des_hist(1,1:Q.k), ... 140 | 'YData', Q.state_des_hist(2,1:Q.k), ... 141 | 'ZData', Q.state_des_hist(3,1:Q.k)); 142 | drawnow; 143 | end 144 | end 145 | 146 | end 147 | -------------------------------------------------------------------------------- /control/utils/QuatToRot.m: -------------------------------------------------------------------------------- 1 | function R = QuatToRot(q) 2 | %QuatToRot Converts a Quaternion to Rotation matrix 3 | % written by Daniel Mellinger 4 | 5 | % normalize q 6 | q = q./sqrt(sum(q.^2)); 7 | 8 | qahat(1,2) = -q(4); 9 | qahat(1,3) = q(3); 10 | qahat(2,3) = -q(2); 11 | qahat(2,1) = q(4); 12 | qahat(3,1) = -q(3); 13 | qahat(3,2) = q(2); 14 | 15 | R = eye(3) + 2*qahat*qahat + 2*q(1)*qahat; 16 | 17 | end -------------------------------------------------------------------------------- /control/utils/RPYtoRot_ZXY.m: -------------------------------------------------------------------------------- 1 | function R = RPYtoRot_ZXY(phi,theta,psi) 2 | %RPYtoRot_ZXY Converts roll, pitch, yaw to a body-to-world Rotation matrix 3 | % The rotation matrix in this function is world to body [bRw] you will 4 | % need to transpose this matrix to get the body to world [wRb] such that 5 | % [wP] = [wRb] * [bP], where [bP] is a point in the body frame and [wP] 6 | % is a point in the world frame 7 | % written by Daniel Mellinger 8 | % 9 | 10 | R = [cos(psi)*cos(theta) - sin(phi)*sin(psi)*sin(theta), ... 11 | cos(theta)*sin(psi) + cos(psi)*sin(phi)*sin(theta), ... 12 | -cos(phi)*sin(theta); ... 13 | -cos(phi)*sin(psi),... 14 | cos(phi)*cos(psi), ... 15 | sin(phi);... 16 | cos(psi)*sin(theta) + cos(theta)*sin(phi)*sin(psi),... 17 | sin(psi)*sin(theta) - cos(psi)*cos(theta)*sin(phi),... 18 | cos(phi)*cos(theta)]; 19 | 20 | end 21 | -------------------------------------------------------------------------------- /control/utils/RotToQuat.m: -------------------------------------------------------------------------------- 1 | function q = RotToQuat(R) 2 | %ROTTOQUAT Converts a Rotation matrix into a Quaternion 3 | % written by Daniel Mellinger 4 | % from the following website, deals with the case when tr<0 5 | % http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm 6 | 7 | %takes in W_R_B rotation matrix 8 | 9 | tr = R(1,1) + R(2,2) + R(3,3); 10 | 11 | if (tr > 0) 12 | S = sqrt(tr+1.0) * 2; % S=4*qw 13 | qw = 0.25 * S; 14 | qx = (R(3,2) - R(2,3)) / S; 15 | qy = (R(1,3) - R(3,1)) / S; 16 | qz = (R(2,1) - R(1,2)) / S; 17 | elseif ((R(1,1) > R(2,2)) && (R(1,1) > R(3,3))) 18 | S = sqrt(1.0 + R(1,1) - R(2,2) - R(3,3)) * 2; % S=4*qx 19 | qw = (R(3,2) - R(2,3)) / S; 20 | qx = 0.25 * S; 21 | qy = (R(1,2) + R(2,1)) / S; 22 | qz = (R(1,3) + R(3,1)) / S; 23 | elseif (R(2,2) > R(3,3)) 24 | S = sqrt(1.0 + R(2,2) - R(1,1) - R(3,3)) * 2; % S=4*qy 25 | qw = (R(1,3) - R(3,1)) / S; 26 | qx = (R(1,2) + R(2,1)) / S; 27 | qy = 0.25 * S; 28 | qz = (R(2,3) + R(3,2)) / S; 29 | else 30 | S = sqrt(1.0 + R(3,3) - R(1,1) - R(2,2)) * 2; % S=4*qz 31 | qw = (R(2,1) - R(1,2)) / S; 32 | qx = (R(1,3) + R(3,1)) / S; 33 | qy = (R(2,3) + R(3,2)) / S; 34 | qz = 0.25 * S; 35 | end 36 | 37 | q = [qw;qx;qy;qz]; 38 | q = q*sign(qw); 39 | 40 | end 41 | -------------------------------------------------------------------------------- /control/utils/RotToRPY_ZXY.m: -------------------------------------------------------------------------------- 1 | function [phi,theta,psi] = RotToRPY_ZXY(R) 2 | %RotToRPY_ZXY Extract Roll, Pitch, Yaw from a world-to-body Rotation Matrix 3 | % The rotation matrix in this function is world to body [bRw] you will 4 | % need to transpose the matrix if you have a body to world [wRb] such 5 | % that [wP] = [wRb] * [bP], where [bP] is a point in the body frame and 6 | % [wP] is a point in the world frame 7 | % written by Daniel Mellinger 8 | % bRw = [ cos(psi)*cos(theta) - sin(phi)*sin(psi)*sin(theta), 9 | % cos(theta)*sin(psi) + cos(psi)*sin(phi)*sin(theta), 10 | % -cos(phi)*sin(theta)] 11 | % [-cos(phi)*sin(psi), cos(phi)*cos(psi), sin(phi)] 12 | % [ cos(psi)*sin(theta) + cos(theta)*sin(phi)*sin(psi), 13 | % sin(psi)*sin(theta) - cos(psi)*cos(theta)*sin(phi), 14 | % cos(phi)*cos(theta)] 15 | 16 | phi = asin(R(2,3)); 17 | psi = atan2(-R(2,1)/cos(phi),R(2,2)/cos(phi)); 18 | theta = atan2(-R(1,3)/cos(phi),R(3,3)/cos(phi)); 19 | 20 | end 21 | -------------------------------------------------------------------------------- /control/utils/collision_check.m: -------------------------------------------------------------------------------- 1 | function collide = collision_check(p, margin) 2 | collide = 0; 3 | if(size(p,1) <= 1) 4 | return; 5 | end 6 | p(:,3) = p(:,3)/3; % scale z-axis by 3 to make it ellipsoid 7 | dis = pdist(p); 8 | if min(dis) < 2*margin 9 | collide = 1; 10 | end 11 | -------------------------------------------------------------------------------- /control/utils/init_state.m: -------------------------------------------------------------------------------- 1 | function [ s ] = init_state( start, yaw ) 2 | %INIT_STATE Initialize 13 x 1 state vector 3 | 4 | s = zeros(13,1); 5 | phi0 = 0.0; 6 | theta0 = 0.0; 7 | psi0 = yaw; 8 | Rot0 = RPYtoRot_ZXY(phi0, theta0, psi0); 9 | Quat0 = RotToQuat(Rot0); 10 | s(1) = start(1); %x 11 | s(2) = start(2); %y 12 | s(3) = start(3); %z 13 | s(4) = 0; %xdot 14 | s(5) = 0; %ydot 15 | s(6) = 0; %zdot 16 | s(7) = Quat0(1); %qw 17 | s(8) = Quat0(2); %qx 18 | s(9) = Quat0(3); %qy 19 | s(10) = Quat0(4); %qz 20 | s(11) = 0; %p 21 | s(12) = 0; %q 22 | s(13) = 0; %r 23 | 24 | end -------------------------------------------------------------------------------- /control/utils/plot_state.m: -------------------------------------------------------------------------------- 1 | function [ h_fig ] = plot_state( h_fig, state, time, name, type, view ) 2 | %PLOT_STATE visualize state data 3 | 4 | if nargin < 6, view = 'sep'; end 5 | if nargin < 5, type = 'vic'; end 6 | if nargin < 4, name = 'pos'; end 7 | if isempty(h_fig), h_fig = figure(); end 8 | line_width = 2; 9 | 10 | switch type 11 | case 'vic' 12 | line_color = 'r'; 13 | case 'des' 14 | line_color = 'b'; 15 | case 'est' 16 | line_color = 'g'; 17 | end 18 | 19 | switch name 20 | case 'pos' 21 | labels = {'x [m]', 'y [m]', 'z [m]'}; 22 | case 'vel' 23 | labels = {'xdot [m/s]', 'ydot [m/s]', 'zdot [m/s]'}; 24 | case 'euler' 25 | labels = {'roll [rad]', 'pitch [rad]', 'yaw [rad]'}; 26 | end 27 | 28 | figure(h_fig) 29 | if strcmp(view, 'sep') 30 | % Plot seperate 31 | 32 | for i = 1:3 33 | subplot(3, 1, i) 34 | hold on 35 | plot(time, state(i,:), line_color, 'LineWidth', line_width); 36 | hold off 37 | xlim([time(1), time(end)]) 38 | grid on 39 | xlabel('time [s]') 40 | ylabel(labels{i}) 41 | end 42 | elseif strcmp(view, '3d') 43 | % Plot 3d 44 | hold on 45 | plot3(state(1,:), state(2,:), state(3,:), line_color, 'LineWidth', line_width) 46 | hold off 47 | grid on 48 | xlabel(labels{1}); 49 | ylabel(labels{2}); 50 | zlabel(labels{3}); 51 | end 52 | 53 | end 54 | -------------------------------------------------------------------------------- /control/utils/qdToState.m: -------------------------------------------------------------------------------- 1 | function [x] = qdToState(qd) 2 | % Converts state vector for simulation to qd struct used in hardware. 3 | % x is 1 x 13 vector of state variables [pos vel quat omega] 4 | % qd is a struct including the fields pos, vel, euler, and omega 5 | 6 | x = zeros(1,13); %initialize dimensions 7 | 8 | x(1:3) = qd.pos; 9 | x(4:6) = qd.vel; 10 | 11 | Rot = RPYtoRot_ZXY(qd.euler(1), qd.euler(2), qd.euler(3)); 12 | quat = RotToQuat(Rot); 13 | 14 | x(7:10) = quat; 15 | x(11:13) = qd.omega; 16 | 17 | end 18 | -------------------------------------------------------------------------------- /control/utils/quad_pos.m: -------------------------------------------------------------------------------- 1 | function [ quad ] = quad_pos( pos, rot, L, H ) 2 | %QUAD_POS Calculates coordinates of quadrotor's position in world frame 3 | % pos 3x1 position vector [x; y; z]; 4 | % rot 3x3 body-to-world rotation matrix 5 | % L 1x1 length of the quad 6 | 7 | if nargin < 4; H = 0.05; end 8 | 9 | % wRb = RPYtoRot_ZXY(euler(1), euler(2), euler(3))'; 10 | wHb = [rot pos(:); 0 0 0 1]; % homogeneous transformation from body to world 11 | 12 | quadBodyFrame = [L 0 0 1; 0 L 0 1; -L 0 0 1; 0 -L 0 1; 0 0 0 1; 0 0 H 1]'; 13 | quadWorldFrame = wHb * quadBodyFrame; 14 | quad = quadWorldFrame(1:3, :); 15 | 16 | end 17 | -------------------------------------------------------------------------------- /control/utils/stateToQd.m: -------------------------------------------------------------------------------- 1 | function [qd] = stateToQd(x) 2 | %Converts qd struct used in hardware to x vector used in simulation 3 | % x is 1 x 13 vector of state variables [pos vel quat omega] 4 | % qd is a struct including the fields pos, vel, euler, and omega 5 | 6 | %current state 7 | qd.pos = x(1:3); 8 | qd.vel = x(4:6); 9 | 10 | Rot = QuatToRot(x(7:10)'); 11 | [phi, theta, yaw] = RotToRPY_ZXY(Rot); 12 | 13 | qd.euler = [phi; theta; yaw]; 14 | qd.omega = x(11:13); 15 | 16 | end 17 | -------------------------------------------------------------------------------- /control/utils/terminate_check.m: -------------------------------------------------------------------------------- 1 | function [ terminate_cond ] = terminate_check( x, time, stop, pos_tol, vel_tol, time_tol) 2 | %TERMINATE_CHECK Check termination criteria, including position, velocity and time 3 | nquad = length(stop); 4 | 5 | % Initialize 6 | pos_check = true; 7 | vel_check = true; 8 | pos_col_check = zeros(nquad, 3); 9 | 10 | % Check position and velocity and still time for each quad 11 | for qn = 1:nquad 12 | pos_check = pos_check && (norm(x{qn}(1:3) - stop{qn}) < pos_tol); 13 | vel_check = vel_check && (norm(x{qn}(4:6)) < vel_tol); 14 | pos_col_check(qn,:) = x{qn}(1:3)'; 15 | end 16 | 17 | % Check total simulation time 18 | time_check = time > time_tol; 19 | % Check collision criteria 20 | col_check = collision_check(pos_col_check, 0.3); 21 | 22 | if (pos_check && vel_check) 23 | terminate_cond = 1; % Robot reaches goal and stops, successful 24 | elseif time_check 25 | terminate_cond = 2; % Robot doesn't reach goal within given time, not complete 26 | elseif col_check 27 | terminate_cond = 3; % Robot collide with each other 28 | else 29 | terminate_cond = 0; 30 | end 31 | 32 | end -------------------------------------------------------------------------------- /gifs/p1p1_circle.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yrlu/quadrotor/79ac78e470640dceaf1558c421bdc23a41ea229a/gifs/p1p1_circle.gif -------------------------------------------------------------------------------- /gifs/p1p1_diamond.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yrlu/quadrotor/79ac78e470640dceaf1558c421bdc23a41ea229a/gifs/p1p1_diamond.gif -------------------------------------------------------------------------------- /gifs/p1p1_step.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yrlu/quadrotor/79ac78e470640dceaf1558c421bdc23a41ea229a/gifs/p1p1_step.gif -------------------------------------------------------------------------------- /gifs/p1p3_map1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yrlu/quadrotor/79ac78e470640dceaf1558c421bdc23a41ea229a/gifs/p1p3_map1.gif -------------------------------------------------------------------------------- /gifs/p1p3_map1_acc.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yrlu/quadrotor/79ac78e470640dceaf1558c421bdc23a41ea229a/gifs/p1p3_map1_acc.gif -------------------------------------------------------------------------------- /gifs/p1p3_map1_snap.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yrlu/quadrotor/79ac78e470640dceaf1558c421bdc23a41ea229a/gifs/p1p3_map1_snap.gif -------------------------------------------------------------------------------- /gifs/p1p3_map2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yrlu/quadrotor/79ac78e470640dceaf1558c421bdc23a41ea229a/gifs/p1p3_map2.gif -------------------------------------------------------------------------------- /gifs/p1p3_map3.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yrlu/quadrotor/79ac78e470640dceaf1558c421bdc23a41ea229a/gifs/p1p3_map3.gif -------------------------------------------------------------------------------- /gifs/p1p3_map3_mini_acc.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yrlu/quadrotor/79ac78e470640dceaf1558c421bdc23a41ea229a/gifs/p1p3_map3_mini_acc.gif -------------------------------------------------------------------------------- /gifs/p1p3_map3_snap.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yrlu/quadrotor/79ac78e470640dceaf1558c421bdc23a41ea229a/gifs/p1p3_map3_snap.gif -------------------------------------------------------------------------------- /imgs/hover.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yrlu/quadrotor/79ac78e470640dceaf1558c421bdc23a41ea229a/imgs/hover.jpg -------------------------------------------------------------------------------- /imgs/p1p1_circle_p.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yrlu/quadrotor/79ac78e470640dceaf1558c421bdc23a41ea229a/imgs/p1p1_circle_p.jpg -------------------------------------------------------------------------------- /imgs/p1p1_circle_v.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yrlu/quadrotor/79ac78e470640dceaf1558c421bdc23a41ea229a/imgs/p1p1_circle_v.jpg -------------------------------------------------------------------------------- /imgs/p1p1_diamond_p.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yrlu/quadrotor/79ac78e470640dceaf1558c421bdc23a41ea229a/imgs/p1p1_diamond_p.jpg -------------------------------------------------------------------------------- /imgs/p1p1_diamond_v.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yrlu/quadrotor/79ac78e470640dceaf1558c421bdc23a41ea229a/imgs/p1p1_diamond_v.jpg -------------------------------------------------------------------------------- /imgs/p1p1_step_p.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yrlu/quadrotor/79ac78e470640dceaf1558c421bdc23a41ea229a/imgs/p1p1_step_p.jpg -------------------------------------------------------------------------------- /imgs/p1p1_step_v.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yrlu/quadrotor/79ac78e470640dceaf1558c421bdc23a41ea229a/imgs/p1p1_step_v.jpg -------------------------------------------------------------------------------- /imgs/p1p3_map1_acc_p.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yrlu/quadrotor/79ac78e470640dceaf1558c421bdc23a41ea229a/imgs/p1p3_map1_acc_p.jpg -------------------------------------------------------------------------------- /imgs/p1p3_map1_acc_v.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yrlu/quadrotor/79ac78e470640dceaf1558c421bdc23a41ea229a/imgs/p1p3_map1_acc_v.jpg -------------------------------------------------------------------------------- /imgs/p1p3_map1_p.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yrlu/quadrotor/79ac78e470640dceaf1558c421bdc23a41ea229a/imgs/p1p3_map1_p.jpg -------------------------------------------------------------------------------- /imgs/p1p3_map1_snap.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yrlu/quadrotor/79ac78e470640dceaf1558c421bdc23a41ea229a/imgs/p1p3_map1_snap.jpg -------------------------------------------------------------------------------- /imgs/p1p3_map1_snap_p.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yrlu/quadrotor/79ac78e470640dceaf1558c421bdc23a41ea229a/imgs/p1p3_map1_snap_p.jpg -------------------------------------------------------------------------------- /imgs/p1p3_map1_snap_v.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yrlu/quadrotor/79ac78e470640dceaf1558c421bdc23a41ea229a/imgs/p1p3_map1_snap_v.jpg -------------------------------------------------------------------------------- /imgs/p1p3_map1_v.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yrlu/quadrotor/79ac78e470640dceaf1558c421bdc23a41ea229a/imgs/p1p3_map1_v.jpg -------------------------------------------------------------------------------- /imgs/p1p3_map2_p.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yrlu/quadrotor/79ac78e470640dceaf1558c421bdc23a41ea229a/imgs/p1p3_map2_p.jpg -------------------------------------------------------------------------------- /imgs/p1p3_map2_v.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yrlu/quadrotor/79ac78e470640dceaf1558c421bdc23a41ea229a/imgs/p1p3_map2_v.jpg -------------------------------------------------------------------------------- /imgs/p1p3_map3_mini_acc_p.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yrlu/quadrotor/79ac78e470640dceaf1558c421bdc23a41ea229a/imgs/p1p3_map3_mini_acc_p.jpg -------------------------------------------------------------------------------- /imgs/p1p3_map3_mini_acc_v.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yrlu/quadrotor/79ac78e470640dceaf1558c421bdc23a41ea229a/imgs/p1p3_map3_mini_acc_v.jpg -------------------------------------------------------------------------------- /imgs/p1p3_map3_p.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yrlu/quadrotor/79ac78e470640dceaf1558c421bdc23a41ea229a/imgs/p1p3_map3_p.jpg -------------------------------------------------------------------------------- /imgs/p1p3_map3_snap_p.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yrlu/quadrotor/79ac78e470640dceaf1558c421bdc23a41ea229a/imgs/p1p3_map3_snap_p.jpg -------------------------------------------------------------------------------- /imgs/p1p3_map3_snap_v.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yrlu/quadrotor/79ac78e470640dceaf1558c421bdc23a41ea229a/imgs/p1p3_map3_snap_v.jpg -------------------------------------------------------------------------------- /imgs/p1p3_map3_v.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yrlu/quadrotor/79ac78e470640dceaf1558c421bdc23a41ea229a/imgs/p1p3_map3_v.jpg -------------------------------------------------------------------------------- /project_report.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yrlu/quadrotor/79ac78e470640dceaf1558c421bdc23a41ea229a/project_report.pdf -------------------------------------------------------------------------------- /quadrotor_dynamics.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yrlu/quadrotor/79ac78e470640dceaf1558c421bdc23a41ea229a/quadrotor_dynamics.pdf -------------------------------------------------------------------------------- /traj_planning/controller.m: -------------------------------------------------------------------------------- 1 | function [F, M, trpy, drpy] = controller(qd, t, qn, params) 2 | % CONTROLLER quadrotor controller 3 | % The current states are: 4 | % qd{qn}.pos, qd{qn}.vel, qd{qn}.euler = [roll;pitch;yaw], qd{qn}.omega 5 | % The desired states are: 6 | % qd{qn}.pos_des, qd{qn}.vel_des, qd{qn}.acc_des, qd{qn}.yaw_des, qd{qn}.yawdot_des 7 | % Using these current and desired states, you have to compute the desired controls 8 | 9 | % =================== Your code goes here =================== 10 | 11 | % ordinary linear 12 | % % position controller params 13 | % Kp = ones(3,1)*30; 14 | % Kd = ones(3,1)*10; 15 | % 16 | % % attitude controller params 17 | % KpM = ones(3,1)*10000; 18 | % KdM = ones(3,1)*500; 19 | 20 | 21 | % position controller params 22 | Kp = [15;15;30]; 23 | % Kd = [15;15;10]; 24 | Kd = [12;12;10]; 25 | 26 | % attitude controller params 27 | KpM = ones(3,1)*3000; 28 | KdM = ones(3,1)*300; 29 | 30 | % t = qd{qn}.vel_des/norm(qd{qn}.vel_des+eps); 31 | % n = qd{qn}.acc_des/norm(qd{qn}.acc_des+eps); 32 | % b = cross(t,n); 33 | % ep = ((qd{qn}.pos_des - qd{qn}.pos).*n).*n + ((qd{qn}.pos_des - qd{qn}.pos).*b).*b; 34 | % ev = qd{qn}.vel_des - qd{qn}.vel; 35 | 36 | acc_des = qd{qn}.acc_des + Kd.*(qd{qn}.vel_des - qd{qn}.vel) + Kp.*(qd{qn}.pos_des - qd{qn}.pos); 37 | % acc_des = qd{qn}.acc_des + Kd.*ev + Kp.*ep 38 | 39 | % Desired roll, pitch and yaw 40 | phi_des = 1/params.grav * (acc_des(1)*sin(qd{qn}.yaw_des) - acc_des(2)*cos(qd{qn}.yaw_des)); 41 | theta_des = 1/params.grav * (acc_des(1)*cos(qd{qn}.yaw_des) + acc_des(2)*sin(qd{qn}.yaw_des)); 42 | psi_des = qd{qn}.yaw_des; 43 | 44 | euler_des = [phi_des;theta_des;psi_des]; 45 | pqr_des = [0;0; qd{qn}.yawdot_des]; 46 | % Thurst 47 | qd{qn}.acc_des(3); 48 | F = params.mass*(params.grav + acc_des(3)); 49 | % Moment 50 | M = params.I*(KdM.*(pqr_des - qd{qn}.omega) + KpM.*(euler_des - qd{qn}.euler)); 51 | % =================== Your code ends here =================== 52 | 53 | % Output trpy and drpy as in hardware 54 | trpy = [F, phi_des, theta_des, psi_des]; 55 | drpy = [0, 0, 0, 0]; 56 | 57 | end 58 | -------------------------------------------------------------------------------- /traj_planning/crazyflie.m: -------------------------------------------------------------------------------- 1 | function params = crazyflie() 2 | % crazyflie: physical parameters for the Crazyflie 2.0 3 | % 4 | % 2016 Bernd Pfrommer 5 | % 6 | % This function creates a struct with the basic parameters for the 7 | % Crazyflie 2.0 quad rotor (without camera, but with about 5 vicon 8 | % markers) 9 | % 10 | % Model assumptions based on physical measurements: 11 | % 12 | % motor + mount + vicon marker = mass point of 3g 13 | % arm length of mass point: 0.046m from center 14 | % battery pack + main board are combined into cuboid (mass 18g) of 15 | % dimensions: 16 | % 17 | % width = 0.03m 18 | % depth = 0.03m 19 | % height = 0.012m 20 | % 21 | 22 | m = 0.030; % weight (in kg) with 5 vicon markers (each is about 0.25g) 23 | g = 9.81; % gravitational constant 24 | I = [1.43e-5, 0, 0; % inertial tensor in m^2 kg 25 | 0, 1.43e-5, 0; 26 | 0, 0, 2.89e-5]; 27 | L = 0.046; % arm length in m 28 | 29 | params.mass = m; 30 | params.I = I; 31 | params.invI = inv(I); 32 | params.grav = g; 33 | params.arm_length = L; 34 | 35 | params.maxangle = 40*pi/180; % you can specify the maximum commanded angle here 36 | params.maxF = 2.5*m*g; % left these untouched from the nano plus 37 | params.minF = 0.05*m*g; % left these untouched from the nano plus 38 | 39 | % You can add any fields you want in params 40 | % for example you can add your controller gains by 41 | % params.k = 0, and they will be passed into controller.m 42 | 43 | end 44 | -------------------------------------------------------------------------------- /traj_planning/generate_ts.m: -------------------------------------------------------------------------------- 1 | function [ts, total_time] = generate_ts(path) 2 | speed = 2; 3 | path_len = sum(sqrt(sum((path(2:end, :) - path(1:end-1,:)).^2,2))); 4 | total_time = path_len/speed; 5 | % ts = linspace(0, total_time, size(path,1)); 6 | path_seg_len = sqrt(sqrt(sum((path(2:end, :) - path(1:end-1,:)).^2,2))); 7 | ts = cumsum(path_seg_len); 8 | ts = ts/ts(end); 9 | ts = [0; ts]'; 10 | ts = ts*total_time; 11 | end -------------------------------------------------------------------------------- /traj_planning/init_script.m: -------------------------------------------------------------------------------- 1 | % Add additional initialization if you want to. 2 | % You can use this space to pre-compute the trajectory to avoid 3 | % repeatedly computing the same trajectory in every call of the 4 | % "trajectory_generator" function 5 | 6 | % Generate trajectory 7 | disp('Generating Trajectory ...'); 8 | trajectory_generator([], [], map, path); 9 | -------------------------------------------------------------------------------- /traj_planning/maps/map1.txt: -------------------------------------------------------------------------------- 1 | # map1 2 | 3 | boundary 0.0 -5.0 0.0 10.0 20.0 6.0 4 | 5 | block 0.0 2.0 0.0 10.0 2.5 1.5 255 0 0 6 | block 0.0 2.0 4.5 10.0 2.5 6.0 255 0 0 7 | 8 | block 0.0 2.0 1.5 3.0 2.5 4.5 255 0 0 9 | block 7.0 2.0 1.5 10.0 2.5 4.5 255 0 0 10 | 11 | block 3.0 0.0 2.4 7.0 0.5 4.5 255 0 0 12 | 13 | block 0.0 15.0 0.0 10.0 20.0 1.0 255 0 0 14 | block 0.0 15.0 1.0 10.0 16.0 3.5 255 0 0 15 | block 0.0 18.0 4.5 10.0 19.0 6.0 255 0 0 16 | -------------------------------------------------------------------------------- /traj_planning/maps/map2.txt: -------------------------------------------------------------------------------- 1 | # map2 2 | 3 | boundary 0.0 -5.0 0.0 10.0 30.0 5.0 4 | 5 | block 1.0 2.0 0.0 3.5 28.0 5.0 255 0 0 6 | block 6.5 2.0 0.0 9.0 28.0 5.0 255 0 0 7 | -------------------------------------------------------------------------------- /traj_planning/maps/map3.txt: -------------------------------------------------------------------------------- 1 | # map3 2 | 3 | block 3.100000 0.000000 2.100000 3.900000 5.000000 6.000000 255.000000 0.000000 0.000000 4 | block 9.100000 0.000000 2.100000 9.900000 5.000000 6.000000 255.000000 0.000000 0.000000 5 | block 15.100000 0.000000 2.100000 15.900000 5.000000 6.000000 255.000000 0.000000 0.000000 6 | block 0.100000 0.000000 0.000000 0.900000 5.000000 3.900000 0.000000 0.000000 255.000000 7 | boundary 0.000000 0.000000 0.000000 20.000000 5.000000 6.000000 8 | block 6.100000 0.000000 0.000000 6.900000 5.000000 3.900000 0.000000 0.000000 255.000000 9 | block 12.100000 0.000000 0.000000 12.900000 5.000000 3.900000 0.000000 0.000000 255.000000 10 | block 18.100000 0.000000 0.000000 18.900000 5.000000 3.900000 0.000000 0.000000 255.000000 11 | -------------------------------------------------------------------------------- /traj_planning/maps/map_empty.txt: -------------------------------------------------------------------------------- 1 | # An example environment 2 | # boundary xmin ymin zmin xmax ymax zmax 3 | # block xmin ymin zmin xmax ymax zmax r g b 4 | boundary 0 0 0 10 10 10 -------------------------------------------------------------------------------- /traj_planning/maps/map_one_block.txt: -------------------------------------------------------------------------------- 1 | # An example environment 2 | # boundary xmin ymin zmin xmax ymax zmax 3 | # block xmin ymin zmin xmax ymax zmax r g b 4 | boundary -5 -5 -5 15 15 15 5 | block 2.0 2.0 0.0 8.0 8.0 10.0 255 0 0 -------------------------------------------------------------------------------- /traj_planning/path_planning/collide.m: -------------------------------------------------------------------------------- 1 | function [C] = collide(map, points) 2 | % COLLIDE Test whether points collide with an obstacle in an environment. 3 | % C = collide(map, points). points is an M-by-3 matrix where each 4 | % row is an (x, y, z) point. C in an M-by-1 logical vector; 5 | % C(i) = 1 if M(i, :) touches an obstacle and is 0 otherwise. 6 | map3d = map.map3d_collision; 7 | sz = size(map3d); nx = sz(1); ny = sz(2); nz = sz(3); 8 | % vectorized 9 | points = points_to_idx(map, points); 10 | idx = (points(:,3)-1)*nx*ny + (points(:,2)-1)*nx + points(:,1); 11 | C = map3d(idx) ~= 255 | map3d(nx*ny*nz+idx) ~= 255 | map3d(nx*ny*nz*2+idx) ~= 255; 12 | end 13 | -------------------------------------------------------------------------------- /traj_planning/path_planning/dijkstra.m: -------------------------------------------------------------------------------- 1 | function [path, num_expanded] = dijkstra(map, start, goal, astar) 2 | % DIJKSTRA Find the shortest path from start to goal. 3 | % PATH = DIJKSTRA(map, start, goal) returns an M-by-3 matrix, where each row 4 | % consists of the (x, y, z) coordinates of a point on the path. The first 5 | % row is start and the last row is goal. If no path is found, PATH is a 6 | % 0-by-3 matrix. Consecutive points in PATH should not be farther apart than 7 | % neighboring cells in the map (e.g.., if 5 consecutive points in PATH are 8 | % co-linear, don't simplify PATH by removing the 3 intermediate points). 9 | % 10 | % PATH = DIJKSTRA(map, start, goal, astar) finds the path using euclidean 11 | % distance to goal as a heuristic if astar is true. 12 | % 13 | % [PATH, NUM_EXPANDED] = DIJKSTRA(...) returns the path as well as 14 | % the number of points that were visited while performing the search. 15 | if nargin < 4 16 | astar = false; 17 | end 18 | function cost = euclidean_heuristic(start_pos, goal_pos) 19 | cost = sqrt(sum((double(start_pos) - double(goal_pos)).^2)); 20 | end 21 | % astar = 1; 22 | path = []; 23 | num_expanded = 0; 24 | 25 | start_xyz = start; 26 | goal_xyz = goal; 27 | 28 | start = points_to_idx(map, start); 29 | goal = points_to_idx(map, goal); 30 | 31 | % init distance map 32 | dist = inf(size(map.occ_map)); % init as inf distance 33 | dist(start(1), start(2), start(3)) = 0; % set start dist as 0 34 | 35 | % init visited map 36 | % visited = map.occ_map; 37 | unvisited = ones(map.nx, map.ny, map.nz); % unvisited: 1, visited: inf 38 | % init back tracking map 39 | prev = zeros(size(map.occ_map,1), size(map.occ_map,2), size(map.occ_map,3), 3); 40 | 41 | % -------- inlined function calls for optimization --------- 42 | % function [i,j,k] = id2coord(map, id) 43 | % % convert a linear index of position into ijk coordinates 44 | % k = floor(id/(map.nx*map.ny))+1; 45 | % j = floor((id - (k-1)*map.nx*map.ny)/map.nx) + 1; 46 | % i = id - (k-1)*map.nx*map.ny - (j-1)*map.nx; 47 | % end 48 | % 49 | % function id = coord2id(map, i, j, k) 50 | % % convert coordinate (i,j,k) to linear id of the map 51 | % id = (k-1)*map.nx*map.ny + (j-1)*map.nx + i; 52 | % end 53 | 54 | % 6-connected 55 | dijk = [1,0,0;-1,0,0; 0,1,0; 0,-1,0; 0,0,1; 0,0,-1]; 56 | if astar == false 57 | % dijkstra algorithm 58 | while (~all(unvisited(:)==inf)) 59 | % find the min dist pos from unvisited node 60 | dist_unvisited = dist.*unvisited; 61 | [~, id] = min(dist_unvisited(:)); 62 | % mark visited 63 | unvisited(id) = inf; 64 | num_expanded = num_expanded + 1; 65 | % check 6-connected neighbors 66 | [i,j,k] = ind2sub([map.nx,map.ny,map.nz],id); 67 | for d = 1:size(dijk,1) 68 | nijk = bsxfun(@plus, int32([i,j,k]), int32(dijk(d,:))); 69 | % if neighbor (in the map) and (unvisited) and (unoccupied) 70 | if all(nijk > 0) & all(int32([map.nx, map.ny, map.nz]) >= int32(nijk)) ... 71 | & unvisited(nijk(1),nijk(2),nijk(3)) == 1 ... 72 | & map.occ_map(nijk(1),nijk(2),nijk(3)) ~= 1 73 | alt = dist(id) + sqrt(sum(dijk(d,:).^2)); 74 | % Got rid of function call for optimization 75 | nid = (nijk(3)-1)*map.nx*map.ny + (nijk(2)-1)*map.nx + nijk(1); 76 | if alt < dist(nid) 77 | dist(nid) = alt; 78 | prev(nijk(1), nijk(2), nijk(3),:) = [i,j,k]; 79 | end 80 | end 81 | end 82 | if id == (goal(3)-1)*map.nx*map.ny + (goal(2)-1)*map.nx + goal(1); 83 | break 84 | end 85 | end 86 | 87 | else 88 | % A* algorithm 89 | fscore = inf(size(map.occ_map)); % init as inf distance 90 | fscore(start(1), start(2), start(3)) = euclidean_heuristic(start, goal); % set start dist as 0 91 | openset = inf(size(map.occ_map)); 92 | openset(start(1), start(2), start(3)) = 1; 93 | 94 | while (~all(openset(:)==inf)) 95 | % find the min point with lowest fscore in the openset 96 | openfscore = fscore.*openset; 97 | [~, id] = min(openfscore(:)); 98 | 99 | if id == (goal(3)-1)*map.nx*map.ny + (goal(2)-1)*map.nx + goal(1); 100 | % if found the goal position, stop searching 101 | break 102 | end 103 | % remove current node from the open set 104 | openset(id) = inf; 105 | unvisited(id) = inf; 106 | num_expanded = num_expanded + 1; 107 | % check 6-connected neighbors 108 | [i,j,k] = ind2sub([map.nx,map.ny,map.nz],id); 109 | for d = 1:size(dijk,1) 110 | nijk = bsxfun(@plus, int32([i,j,k]), int32(dijk(d,:))); 111 | % if neighbor (in the map) and (unvisited) and (unoccupied) 112 | if all(nijk > 0) & all(int32([map.nx, map.ny, map.nz]) >= int32(nijk)) ... 113 | & unvisited(nijk(1),nijk(2),nijk(3)) == 1 ... 114 | & map.occ_map(nijk(1),nijk(2),nijk(3)) ~= 1 115 | % gscore is the distance from start to the neighbor via current 116 | % position 117 | nid = (nijk(3)-1)*map.nx*map.ny + (nijk(2)-1)*map.nx + nijk(1); 118 | gscore = dist(id) + sqrt(sum(dijk(d,:).^2)); 119 | if openset(nid) == inf 120 | openset(nid) = 1; 121 | elseif gscore >= dist(nid) 122 | % then it is a longer path 123 | continue 124 | end 125 | 126 | % then this is the best path so far 127 | prev(nijk(1), nijk(2), nijk(3),:) = [i,j,k]; 128 | dist(nid) = gscore; 129 | fscore(nid) = dist(nid) + euclidean_heuristic(nijk, goal); 130 | end 131 | end 132 | end 133 | 134 | end 135 | if dist(goal(1), goal(2), goal(3)) == inf 136 | path = []; 137 | else 138 | path = [goal(1), goal(2), goal(3)]; 139 | % back track to the start pos 140 | cur_p = reshape(prev(goal(1), goal(2), goal(3), :),[1,3]); 141 | while any(cur_p ~= start) 142 | path = [cur_p;path]; 143 | cur_p = reshape(prev(cur_p(1), cur_p(2), cur_p(3), :), [1,3]); 144 | end 145 | path = [cur_p;path]; 146 | end 147 | path = idx_to_points(map, path); 148 | if size(path, 1) > 0 149 | path = [start_xyz; path; goal_xyz]; 150 | else 151 | path = zeros(0, 3); 152 | end 153 | end -------------------------------------------------------------------------------- /traj_planning/path_planning/idx_to_points.m: -------------------------------------------------------------------------------- 1 | function xyz = idx_to_points(map, ijk) 2 | % convert indices in occupancy grid into 3d points xyz (Nx3) 3 | % @author: Yiren Lu 4 | % @date: 02/15/2017 5 | % @input: map map generated by load_map 6 | % ijk Nx3 indices in map's occupancy grid 7 | % @output: xyz Nx3 3d points in actual space 8 | xyz = ones(size(ijk, 1), 3); 9 | if size(ijk,1) > 0 10 | xyz(:,1) = map.boundary(1)+(ijk(:,1)-1)*map.xy_res + map.xy_res*0.5; 11 | xyz(:,2) = map.boundary(2)+(ijk(:,2)-1)*map.xy_res + map.xy_res*0.5; 12 | xyz(:,3) = map.boundary(3)+(ijk(:,3)-1)*map.z_res + map.z_res*0.5; 13 | else 14 | xyz = []; 15 | end 16 | end -------------------------------------------------------------------------------- /traj_planning/path_planning/load_map.m: -------------------------------------------------------------------------------- 1 | function map = load_map(filename, xy_res, z_res, margin) 2 | % LOAD_MAP Load a map from disk. 3 | % MAP = LOAD_MAP(filename, xy_res, z_res, margin). Creates an occupancy grid 4 | % map where a node is considered fill if it lies within 'margin' distance of 5 | % on abstacle. 6 | fid = fopen(filename); 7 | tline = fgets(fid); 8 | blocks = []; 9 | while ischar(tline) 10 | % skip empty line and comments 11 | if numel(tline) > 1 & tline(1)~='#' 12 | % convert char array to string 13 | if strncmpi(tline, 'boundary', 8) 14 | boundary = strread(tline(9:end)); 15 | end 16 | if strncmpi(tline, 'block', 5) 17 | block = strread(tline(6:end)); 18 | assert(size(block,2) == 9); 19 | blocks = [blocks; block]; 20 | end 21 | end 22 | tline = fgets(fid); 23 | end 24 | map.boundary = boundary; 25 | map.blocks = blocks; 26 | map.xy_res = xy_res; 27 | map.z_res = z_res; 28 | % map.margin = margin + 0.2; enforce the path finder to pick a safe path. 29 | map.margin = margin; 30 | n_x = ceil(abs(boundary(4) - boundary(1))/xy_res); 31 | n_y = ceil(abs(boundary(5) - boundary(2))/xy_res); 32 | n_z = ceil(abs(boundary(6) - boundary(3))/z_res); 33 | map.nx = n_x; 34 | map.ny = n_y; 35 | map.nz = n_z; 36 | 37 | map3d = ones(n_x, n_y, n_z, 3)*255; 38 | occ_map = logical(zeros(n_x, n_y, n_z)); 39 | for i = 1:size(blocks,1) 40 | block = blocks(i, :); 41 | xyzs = points_to_idx(map, [block(1) - map.margin, block(2) - map.margin, block(3) - map.margin]); 42 | xs = xyzs(1); ys = xyzs(2); zs = xyzs(3); 43 | xyze = points_to_idx(map, [block(4) + map.margin, block(5) + map.margin, block(6) + map.margin]); 44 | xe = xyze(1); ye = xyze(2); ze = xyze(3); 45 | % convert block to coordinates 46 | map3d(xs:xe,ys:ye,zs:ze,1) = uint8(block(7)); 47 | map3d(xs:xe,ys:ye,zs:ze,2) = uint8(block(8)); 48 | map3d(xs:xe,ys:ye,zs:ze,3) = uint8(block(9)); 49 | occ_map(xs:xe,ys:ye,zs:ze) = 1; 50 | end 51 | map.map3d = map3d; 52 | 53 | map3d_collision = ones(n_x, n_y, n_z, 3)*255; 54 | for i = 1:size(blocks,1) 55 | block = blocks(i, :); 56 | xyzs = points_to_idx(map, [block(1) - margin, block(2) - margin, block(3) - margin]); 57 | xs = xyzs(1); ys = xyzs(2); zs = xyzs(3); 58 | xyze = points_to_idx(map, [block(4) + margin, block(5) + margin, block(6) + margin]); 59 | xe = xyze(1); ye = xyze(2); ze = xyze(3); 60 | % convert block to coordinates 61 | map3d_collision(xs:xe,ys:ye,zs:ze,1) = uint8(block(7)); 62 | map3d_collision(xs:xe,ys:ye,zs:ze,2) = uint8(block(8)); 63 | map3d_collision(xs:xe,ys:ye,zs:ze,3) = uint8(block(9)); 64 | end 65 | map.map3d_collision = map3d_collision; 66 | 67 | map.occ_map = occ_map; 68 | end 69 | -------------------------------------------------------------------------------- /traj_planning/path_planning/plot_path.m: -------------------------------------------------------------------------------- 1 | function plot_path2(map, path) 2 | % PLOT_PATH Visualize a path through an environment 3 | % PLOT_PATH(map, path) creates a figure showing a path through the 4 | % environment. path is an N-by-3 matrix where each row corresponds to the 5 | % (x, y, z) coordinates of one point along the path. 6 | 7 | figure(1); 8 | % path = points_to_idx(map, path); 9 | 10 | 11 | % [x,y,z] = meshgrid(1:size(map.map3d,1),1:size(map.map3d,2),1:size(map.map3d,3)); 12 | % x = x(:); y = y(:); z = z(:); 13 | % 14 | % xyz = []; 15 | % for i = 1:numel(x) 16 | % if map.map3d(x(i),y(i),z(i),1)~= 255 | map.map3d(x(i),y(i),z(i),2)~= 255 | map.map3d(x(i),y(i),z(i),2)~= 255 17 | % xyz = [xyz; x(i),y(i),z(i)]; 18 | % end 19 | % end 20 | % rgb = zeros(size(xyz,1),3); 21 | % for i = 1:size(xyz,1) 22 | % rgb(i,:) = map.map3d(xyz(i,1),xyz(i,2),xyz(i,3),:); 23 | % end 24 | % xyz = idx_to_points(map, xyz); 25 | % if size(xyz,1) > 0 26 | % pcshow(xyz, rgb, 'MarkerSize', 20); 27 | % end 28 | % hold on; 29 | % if size(path,1) > 0 30 | % pcshow(path, [0,0,0],'MarkerSize', 20); 31 | % end 32 | % hold off; 33 | 34 | 35 | hold on; 36 | for i = 1:size(map.blocks,1) 37 | block = map.blocks(i, :); 38 | 39 | x = [ones(4,1) * block(1); ones(4,1) * block(4)]; 40 | y = [ones(2,1) * block(5); ones(2,1) * block(2); ones(2,1) * block(5); ones(2,1) * block(2)]; 41 | z = [block(3);block(6);block(3);block(6);block(3);block(6);block(3);block(6)]; 42 | 43 | 44 | vert = [x, y, z]; 45 | fac = [1 2 6 5;2 3 7 6;3 4 8 7;4 1 5 8;1 2 3 4;5 6 7 8]; 46 | c = block(7:9)/255; 47 | patch('Vertices',vert,'Faces',fac,... 48 | 'FaceVertexCData',hsv(6),'FaceColor',c); 49 | 50 | 51 | x = [ones(4,1) * block(1); ones(4,1) * block(4)]; 52 | y = [block(2);block(5);block(2);block(5);block(2);block(5);block(2);block(5)]; 53 | z = [ones(2,1) * block(3); ones(2,1) * block(6); ones(2,1) * block(3); ones(2,1) * block(6)]; 54 | 55 | vert = [x, y, z]; 56 | fac = [1 2 6 5;2 3 7 6;3 4 8 7;4 1 5 8;1 2 3 4;5 6 7 8]; 57 | c = block(7:9)/255; 58 | patch('Vertices',vert,'Faces',fac,... 59 | 'FaceVertexCData',hsv(6),'FaceColor',c); 60 | end 61 | 62 | if size(path,1) > 0 63 | pcshow(path, [0,0,0],'MarkerSize', 0.1); 64 | end 65 | % axis([map.boundary(1)-1, map.boundary(4)-1, map.boundary(2)-1,map.boundary(5)+1,map.boundary(3)+1,map.boundary(6)+1]) 66 | hold off; 67 | % view(3); 68 | end -------------------------------------------------------------------------------- /traj_planning/path_planning/plot_path1.m: -------------------------------------------------------------------------------- 1 | function plot_path(map, path) 2 | % PLOT_PATH Visualize a path through an environment 3 | % PLOT_PATH(map, path) creates a figure showing a path through the 4 | % environment. path is an N-by-3 matrix where each row corresponds to the 5 | % (x, y, z) coordinates of one point along the path. 6 | 7 | path = points_to_idx(map, path); 8 | 9 | [x,y,z] = meshgrid(1:size(map.map3d,1),1:size(map.map3d,2),1:size(map.map3d,3)); 10 | x = x(:); y = y(:); z = z(:); 11 | 12 | xyz = []; 13 | for i = 1:numel(x) 14 | if map.map3d(x(i),y(i),z(i),1)~= 255 | map.map3d(x(i),y(i),z(i),2)~= 255 | map.map3d(x(i),y(i),z(i),2)~= 255 15 | xyz = [xyz; x(i),y(i),z(i)]; 16 | end 17 | end 18 | rgb = zeros(size(xyz,1),3); 19 | for i = 1:size(xyz,1) 20 | rgb(i,:) = map.map3d(xyz(i,1),xyz(i,2),xyz(i,3),:); 21 | end 22 | if size(xyz,1) > 0 23 | pcshow(xyz, rgb, 'MarkerSize', 200); 24 | end 25 | hold on; 26 | if size(path,1) > 0 27 | pcshow(path, [0,0,0],'MarkerSize', 20); 28 | end 29 | hold off; 30 | end -------------------------------------------------------------------------------- /traj_planning/path_planning/plot_path3.m: -------------------------------------------------------------------------------- 1 | function plot_path(map, path) 2 | % PLOT_PATH Visualize a path through an environment 3 | % PLOT_PATH(map, path) creates a figure showing a path through the 4 | % environment. path is an N-by-3 matrix where each row corresponds to the 5 | % (x, y, z) coordinates of one point along the path. 6 | 7 | figure(1); 8 | % path = points_to_idx(map, path); 9 | 10 | [x,y,z] = meshgrid(1:size(map.map3d,1),1:size(map.map3d,2),1:size(map.map3d,3)); 11 | x = x(:); y = y(:); z = z(:); 12 | 13 | xyz = []; 14 | for i = 1:numel(x) 15 | if map.map3d(x(i),y(i),z(i),1)~= 255 | map.map3d(x(i),y(i),z(i),2)~= 255 | map.map3d(x(i),y(i),z(i),2)~= 255 16 | xyz = [xyz; x(i),y(i),z(i)]; 17 | end 18 | end 19 | rgb = zeros(size(xyz,1),3); 20 | for i = 1:size(xyz,1) 21 | rgb(i,:) = map.map3d(xyz(i,1),xyz(i,2),xyz(i,3),:); 22 | end 23 | xyz = idx_to_points(map, xyz); 24 | if size(xyz,1) > 0 25 | pcshow(xyz, rgb, 'MarkerSize', 20); 26 | end 27 | hold on; 28 | if size(path,1) > 0 29 | pcshow(path, [0,0,0],'MarkerSize', 20); 30 | end 31 | hold off; 32 | end -------------------------------------------------------------------------------- /traj_planning/path_planning/points_to_idx.m: -------------------------------------------------------------------------------- 1 | function ijk = points_to_idx(map, xyz) 2 | % convert 3d points xyz (Nx3) into indices in occupancy grid 3 | % @author: Yiren Lu 4 | % @date: 02/13/2017 5 | % @input: map map generated by load_map 6 | % xyz Nx3 3d points in actual space 7 | % @output: ijk Nx3 indices in map's occupancy grid 8 | ijk = ones(size(xyz,1),3); 9 | if size(xyz,1) > 0 10 | ijk(:,1) = min(max(floor((xyz(:,1) - map.boundary(1))/map.xy_res + 1),1), map.nx); 11 | ijk(:,2) = min(max(floor((xyz(:,2) - map.boundary(2))/map.xy_res + 1),1), map.ny); 12 | ijk(:,3) = min(max(floor((xyz(:,3) - map.boundary(3))/map.z_res + 1),1), map.nz); 13 | end 14 | end -------------------------------------------------------------------------------- /traj_planning/quadEOM.m: -------------------------------------------------------------------------------- 1 | function sdot = quadEOM(t, s, qn, controlhandle, trajhandle, params) 2 | % QUADEOM Wrapper function for solving quadrotor equation of motion 3 | % quadEOM takes in time, state vector, controller, trajectory generator 4 | % and parameters and output the derivative of the state vector, the 5 | % actual calcution is done in quadEOM_readonly. 6 | % 7 | % INPUTS: 8 | % t - 1 x 1, time 9 | % s - 13 x 1, state vector = [x, y, z, xd, yd, zd, qw, qx, qy, qz, p, q, r] 10 | % qn - quad number (used for multi-robot simulations) 11 | % controlhandle - function handle of your controller 12 | % trajhandle - function handle of your trajectory generator 13 | % params - struct, output from crazyflie() and whatever parameters you want to pass in 14 | % 15 | % OUTPUTS: 16 | % sdot - 13 x 1, derivative of state vector s 17 | % 18 | % NOTE: You should not modify this function 19 | % See Also: quadEOM_readonly, crazyflie 20 | 21 | % convert state to quad stuct for control 22 | qd{qn} = stateToQd(s); 23 | 24 | % Get desired_state 25 | desired_state = trajhandle(t, qn); 26 | 27 | % The desired_state is set in the trajectory generator 28 | qd{qn}.pos_des = desired_state.pos; 29 | qd{qn}.vel_des = desired_state.vel; 30 | qd{qn}.acc_des = desired_state.acc; 31 | qd{qn}.yaw_des = desired_state.yaw; 32 | qd{qn}.yawdot_des = desired_state.yawdot; 33 | 34 | % get control outputs 35 | [F, M, trpy, drpy] = controlhandle(qd, t, qn, params); 36 | 37 | % compute derivative 38 | sdot = quadEOM_readonly(t, s, F, M, params); 39 | 40 | end 41 | -------------------------------------------------------------------------------- /traj_planning/quadEOM_readonly.m: -------------------------------------------------------------------------------- 1 | function sdot = quadEOM_readonly(t, s, F, M, params) 2 | % QUADEOM_READONLY Solve quadrotor equation of motion 3 | % quadEOM_readonly calculate the derivative of the state vector 4 | % 5 | % INPUTS: 6 | % t - 1 x 1, time 7 | % s - 13 x 1, state vector = [x, y, z, xd, yd, zd, qw, qx, qy, qz, p, q, r] 8 | % F - 1 x 1, thrust output from controller (only used in simulation) 9 | % M - 3 x 1, moments output from controller (only used in simulation) 10 | % params - struct, output from crazyflie() and whatever parameters you want to pass in 11 | % 12 | % OUTPUTS: 13 | % sdot - 13 x 1, derivative of state vector s 14 | % 15 | % NOTE: You should not modify this function 16 | % See Also: quadEOM_readonly, crazyflie 17 | 18 | %************ EQUATIONS OF MOTION ************************ 19 | % Limit the force and moments due to actuator limits 20 | A = [0.25, 0, -0.5/params.arm_length; 21 | 0.25, 0.5/params.arm_length, 0; 22 | 0.25, 0, 0.5/params.arm_length; 23 | 0.25, -0.5/params.arm_length, 0]; 24 | 25 | prop_thrusts = A*[F;M(1:2)]; % Not using moment about Z-axis for limits 26 | prop_thrusts_clamped = max(min(prop_thrusts, params.maxF/4), params.minF/4); 27 | 28 | B = [ 1, 1, 1, 1; 29 | 0, params.arm_length, 0, -params.arm_length; 30 | -params.arm_length, 0, params.arm_length, 0]; 31 | F = B(1,:)*prop_thrusts_clamped; 32 | M = [B(2:3,:)*prop_thrusts_clamped; M(3)]; 33 | 34 | % Assign states 35 | x = s(1); 36 | y = s(2); 37 | z = s(3); 38 | xdot = s(4); 39 | ydot = s(5); 40 | zdot = s(6); 41 | qW = s(7); 42 | qX = s(8); 43 | qY = s(9); 44 | qZ = s(10); 45 | p = s(11); 46 | q = s(12); 47 | r = s(13); 48 | 49 | quat = [qW; qX; qY; qZ]; 50 | bRw = QuatToRot(quat); 51 | wRb = bRw'; 52 | 53 | % Acceleration 54 | accel = 1 / params.mass * (wRb * [0; 0; F] - [0; 0; params.mass * params.grav]); 55 | 56 | % Angular velocity 57 | K_quat = 2; %this enforces the magnitude 1 constraint for the quaternion 58 | quaterror = 1 - (qW^2 + qX^2 + qY^2 + qZ^2); 59 | qdot = -1/2*[0, -p, -q, -r;... 60 | p, 0, -r, q;... 61 | q, r, 0, -p;... 62 | r, -q, p, 0] * quat + K_quat*quaterror * quat; 63 | 64 | % Angular acceleration 65 | omega = [p;q;r]; 66 | pqrdot = params.invI * (M - cross(omega, params.I*omega)); 67 | 68 | % Assemble sdot 69 | sdot = zeros(13,1); 70 | sdot(1) = xdot; 71 | sdot(2) = ydot; 72 | sdot(3) = zdot; 73 | sdot(4) = accel(1); 74 | sdot(5) = accel(2); 75 | sdot(6) = accel(3); 76 | sdot(7) = qdot(1); 77 | sdot(8) = qdot(2); 78 | sdot(9) = qdot(3); 79 | sdot(10) = qdot(4); 80 | sdot(11) = pqrdot(1); 81 | sdot(12) = pqrdot(2); 82 | sdot(13) = pqrdot(3); 83 | 84 | end 85 | -------------------------------------------------------------------------------- /traj_planning/runsim.m: -------------------------------------------------------------------------------- 1 | close all; 2 | clear all; 3 | clc; 4 | addpath(genpath('./')); 5 | 6 | %% Plan path 1 7 | disp('Planning ...'); 8 | map = load_map('maps/map1.txt', 0.1, 2, 0.25); 9 | start = {[0.0 -4.9 0.2]}; 10 | stop = {[6.0 18.0-1 5.0]}; 11 | % stop = {[6.0 18.0-6 3.0]}; 12 | nquad = length(start); 13 | for qn = 1:nquad 14 | tic 15 | path{qn} = dijkstra(map, start{qn}, stop{qn}, true); 16 | toc 17 | end 18 | if nquad == 1 19 | plot_path(map, path{1}); 20 | else 21 | % you could modify your plot_path to handle cell input for multiple robots 22 | end 23 | 24 | 25 | %% Plan path 3 26 | disp('Planning ...'); 27 | map = load_map('maps/map3.txt', 0.2, 0.5, 0.25); 28 | start = {[0.0, 5, 5.0]}; 29 | stop = {[20, 5, 5]}; 30 | nquad = length(start); 31 | for qn = 1:nquad 32 | tic 33 | path{qn} = dijkstra(map, start{qn}, stop{qn}, true); 34 | toc 35 | end 36 | if nquad == 1 37 | plot_path(map, path{1}); 38 | else 39 | % you could modify your plot_path to handle cell input for multiple robots 40 | end 41 | 42 | 43 | %% Additional init script 44 | init_script; 45 | 46 | %% Run trajectory 47 | trajectory = test_trajectory(start, stop, map, path, true); % with visualization 48 | -------------------------------------------------------------------------------- /traj_planning/simplify_path.m: -------------------------------------------------------------------------------- 1 | function path1 = simplify_path(path) 2 | % path1 = [path(1,:);path(1:4:end, :);path(end,:)]; 3 | % return 4 | path0 = path; 5 | % remove duplicated points: 6 | rm_pts = logical(zeros(size(path,1),1)); 7 | for i = 1:size(path,1)-1 8 | if norm(path(i+1,:) - path(i,:)) < 1e-3 9 | rm_pts(i) = 1; 10 | end 11 | end 12 | path = path(~rm_pts,:); 13 | 14 | % handle diagional case 15 | path = path(1:2:end, :); 16 | 17 | % check collinear 18 | rm_pts = logical(zeros(size(path,1),1)); 19 | for i = 1:size(path,1) - 2 20 | if sum(sum((abs(bsxfun(@minus, path(i:i+2,:), mean(path(i:i+2,:))))<0.0001))) >= 6 21 | rm_pts(i+1) = 1; 22 | end 23 | end 24 | path = path(~rm_pts,:); 25 | 26 | % add start/goal points 27 | if any(path(1,:) ~= path0(1,:)) 28 | path = [path0(1,:); path]; 29 | end 30 | if any(path(end,:) ~= path0(end,:)) 31 | path = [path; path0(end,:)]; 32 | end 33 | path1 = path; 34 | % plot_path(map, path1); 35 | end -------------------------------------------------------------------------------- /traj_planning/simplify_path2.m: -------------------------------------------------------------------------------- 1 | function path1 = simplify_path2(map, path) 2 | path0 = path; 3 | if size(path,1) == 0 4 | path1 = path; 5 | return; 6 | end 7 | keep_pts = logical(zeros(size(path,1),1)); 8 | keep_pts(1) = 1; 9 | keep_pts(end) = 1; 10 | last_keep = path(1,:); 11 | 12 | function c = check_line_collision(map, start_pos, end_pos) 13 | % generate intermedia points 14 | lenxy = sqrt(sum((start_pos(1:2) - end_pos(1:2)).^2)); 15 | lenz = sqrt(sum((start_pos(3) - end_pos(3)).^2)); 16 | n_pts = int32(max(lenxy/map.xy_res, lenz/map.z_res)) + 2; 17 | pts = [linspace(start_pos(1), end_pos(1), n_pts); 18 | linspace(start_pos(2), end_pos(2), n_pts); 19 | linspace(start_pos(3), end_pos(3), n_pts)]'; 20 | c = collide(map, pts); 21 | c = any(c); 22 | end 23 | 24 | counter = 1; 25 | for i = 2:size(path,1)-1 26 | counter = counter + 1; 27 | if (~check_line_collision(map, last_keep, path(i,:)) & check_line_collision(map, last_keep, path(i+1,:))) | counter == 200000 28 | count = 1; 29 | last_keep = path(i,:); 30 | keep_pts(i) = 1; 31 | end 32 | end 33 | path = path(keep_pts,:); 34 | 35 | % add intermedia points to avoid running out of the map 36 | path1 = []; 37 | for i = 1:size(path,1)-1 38 | path1 = [path1; path(i,:)]; 39 | % path1 = [path1; path(i,:) + (path(i+1,:)-path(i,:))*0.1]; 40 | % path1 = [path1; path(i,:) + (path(i+1,:)-path(i,:))*0.2]; 41 | % path1 = [path1; path(i,:) + (path(i+1,:)-path(i,:))*0.5]; 42 | % path1 = [path1; path(i,:) + (path(i+1,:)-path(i,:))*0.7]; 43 | % path1 = [path1; path(i,:) + (path(i+1,:)-path(i,:))*0.9]; 44 | end 45 | path1 = [path1; path(end,:)]; 46 | end -------------------------------------------------------------------------------- /traj_planning/test_trajectory.m: -------------------------------------------------------------------------------- 1 | function [xtraj, ttraj, terminate_cond] = test_trajectory(start, stop, map, path, vis) 2 | % TEST_TRAJECTORY simulates the robot from START to STOP following a PATH 3 | % that's been planned for MAP. 4 | % start - a 3d vector or a cell contains multiple 3d vectors 5 | % stop - a 3d vector or a cell contains multiple 3d vectors 6 | % map - map generated by your load_map 7 | % path - n x 3 matrix path planned by your dijkstra algorithm 8 | % vis - true for displaying visualization 9 | 10 | %Controller and trajectory generator handles 11 | controlhandle = @controller; 12 | trajhandle = @trajectory_generator; 13 | 14 | % Make cell 15 | if ~iscell(start), start = {start}; end 16 | if ~iscell(stop), stop = {stop}; end 17 | if ~iscell(path), path = {path} ;end 18 | 19 | % Get nquad 20 | nquad = length(start); 21 | 22 | % Make column vector 23 | for qn = 1:nquad 24 | start{qn} = start{qn}(:); 25 | stop{qn} = stop{qn}(:); 26 | end 27 | 28 | % Quadrotor model 29 | params = crazyflie(); 30 | 31 | %% **************************** FIGURES ***************************** 32 | % Environment figure 33 | if nargin < 5 34 | vis = true; 35 | end 36 | 37 | fprintf('Initializing figures...\n') 38 | if vis 39 | h_fig = figure('Name', 'Environment'); 40 | else 41 | h_fig = figure('Name', 'Environment', 'Visible', 'Off'); 42 | end 43 | if nquad == 1 44 | plot_path(map, path{1}); 45 | else 46 | % you could modify your plot_path to handle cell input for multiple robots 47 | end 48 | h_3d = gca; 49 | drawnow; 50 | xlabel('x [m]'); ylabel('y [m]'); zlabel('z [m]') 51 | quadcolors = lines(nquad); 52 | set(gcf,'Renderer','OpenGL') 53 | 54 | %% *********************** INITIAL CONDITIONS *********************** 55 | fprintf('Setting initial conditions...\n') 56 | % Maximum time that the quadrotor is allowed to fly 57 | time_tol = 30; % maximum simulation time 58 | starttime = 0; % start of simulation in seconds 59 | tstep = 0.01; % this determines the time step at which the solution is given 60 | cstep = 0.05; % image capture time interval 61 | nstep = cstep/tstep; 62 | time = starttime; % current time 63 | max_iter = time_tol / cstep; % max iteration 64 | for qn = 1:nquad 65 | % Get start and stop position 66 | x0{qn} = init_state(start{qn}, 0); 67 | xtraj{qn} = zeros(max_iter*nstep, length(x0{qn})); 68 | ttraj{qn} = zeros(max_iter*nstep, 1); 69 | end 70 | 71 | % Maximum position error of the quadrotor at goal 72 | pos_tol = 0.05; % m 73 | % Maximum speed of the quadrotor at goal 74 | vel_tol = 0.05; % m/s 75 | 76 | x = x0; % state 77 | 78 | %% ************************* RUN SIMULATION ************************* 79 | OUTPUT_TO_VIDEO = 1; 80 | if OUTPUT_TO_VIDEO == 1 81 | v = VideoWriter('map1.avi'); 82 | open(v) 83 | end 84 | 85 | fprintf('Simulation Running....\n') 86 | for iter = 1:max_iter 87 | timeint = time:tstep:time+cstep; 88 | tic; 89 | % Iterate over each quad 90 | for qn = 1:nquad 91 | % Initialize quad plot 92 | if iter == 1 93 | QP{qn} = QuadPlot(qn, x0{qn}, 0.1, 0.04, quadcolors(qn,:), max_iter, h_3d); 94 | desired_state = trajhandle(time, qn); 95 | QP{qn}.UpdateQuadPlot(x{qn}, [desired_state.pos; desired_state.vel], time); 96 | h_title = title(sprintf('iteration: %d, time: %4.2f', iter, time)); 97 | end 98 | 99 | % Run simulation 100 | [tsave, xsave] = ode45(@(t,s) quadEOM(t, s, qn, controlhandle, trajhandle, params), timeint, x{qn}); 101 | x{qn} = xsave(end, :)'; 102 | % Save to traj 103 | xtraj{qn}((iter-1)*nstep+1:iter*nstep,:) = xsave(1:end-1,:); 104 | ttraj{qn}((iter-1)*nstep+1:iter*nstep) = tsave(1:end-1); 105 | 106 | % Update quad plot 107 | desired_state = trajhandle(time + cstep, qn); 108 | QP{qn}.UpdateQuadPlot(x{qn}, [desired_state.pos; desired_state.vel], time + cstep); 109 | if OUTPUT_TO_VIDEO == 1 110 | im = frame2im(getframe(gcf)); 111 | writeVideo(v,im); 112 | end 113 | end 114 | 115 | set(h_title, 'String', sprintf('iteration: %d, time: %4.2f', iter, time + cstep)) 116 | time = time + cstep; % Update simulation time 117 | t = toc; 118 | 119 | % Pause to make real-time 120 | if (t < cstep) 121 | pause(cstep - t); 122 | end 123 | 124 | % Check termination criteria 125 | terminate_cond = terminate_check(x, time, stop, pos_tol, vel_tol, time_tol); 126 | if terminate_cond 127 | break 128 | end 129 | 130 | end 131 | 132 | fprintf('Simulation Finished....\n') 133 | 134 | 135 | if OUTPUT_TO_VIDEO == 1 136 | close(v); 137 | end 138 | 139 | %% ************************* POST PROCESSING ************************* 140 | % Truncate xtraj and ttraj 141 | for qn = 1:nquad 142 | xtraj{qn} = xtraj{qn}(1:iter*nstep,:); 143 | ttraj{qn} = ttraj{qn}(1:iter*nstep); 144 | end 145 | 146 | % Plot the saved position and velocity of each robot 147 | if vis 148 | for qn = 1:nquad 149 | % Truncate saved variables 150 | QP{qn}.TruncateHist(); 151 | % Plot position for each quad 152 | h_pos{qn} = figure('Name', ['Quad ' num2str(qn) ' : position']); 153 | plot_state(h_pos{qn}, QP{qn}.state_hist(1:3,:), QP{qn}.time_hist, 'pos', 'vic'); 154 | plot_state(h_pos{qn}, QP{qn}.state_des_hist(1:3,:), QP{qn}.time_hist, 'pos', 'des'); 155 | % Plot velocity for each quad 156 | h_vel{qn} = figure('Name', ['Quad ' num2str(qn) ' : velocity']); 157 | plot_state(h_vel{qn}, QP{qn}.state_hist(4:6,:), QP{qn}.time_hist, 'vel', 'vic'); 158 | plot_state(h_vel{qn}, QP{qn}.state_des_hist(4:6,:), QP{qn}.time_hist, 'vel', 'des'); 159 | end 160 | end 161 | 162 | end 163 | -------------------------------------------------------------------------------- /traj_planning/traj_opt3.m: -------------------------------------------------------------------------------- 1 | function X = traj_opt3(path, total_time, ts) 2 | % 5th order trajectory optimization (minimum acceleration) 3 | % by solving 4m linear equations. Assumes constant speed 4 | % @author Yiren Lu 5 | % @email luyiren [at] seas [dot] upenn [dot] edu 6 | % 7 | % @input: path (m+1) by 3 planning trajectory 8 | % total_time total time 9 | % @output X 4mx3 solution vector, where 10 | % X(4*(k-1)+1:4*k,d) is the coefficients 11 | % of x_k(t), i.e., 12 | % x_k(t)=X(4*(k-1)+1:4*k,d)'*[t^3;t^2,t,1] 13 | % where d \in [1,2,3] to indicate dimension xyz 14 | path0 = path; 15 | % generate the trajectory here, and decide the total_time 16 | [m,n] = size(path0); % n == 3 17 | % there we set m = m - 1 for convenience 18 | m = m-1; 19 | % there are now in total m+1 points in the path, which seperate the path into 20 | % m subpaths. 21 | 22 | % ts(k) = t_{k+1}, e.g. ts(1) = t_0, 23 | % time planning according to the segment length 24 | 25 | % ts(m+1) = t_m 26 | % In 3rd order trajectory optimization, for each subpath there are 4 27 | % parameters 28 | % x_k(t) = c_{k,3}*t^3 + c_{k,2}*t^2 + c_{k,1}*t^1 + c_{k,0} 29 | % for k = 1..(m-1) 30 | 31 | % X contains the parameters 32 | X = zeros(4*m,n); 33 | A = zeros(4*m, 4*m, n); 34 | Y = zeros(4*m,n); 35 | 36 | for i = 1:n 37 | A(:,:,i) = eye(4*m)*eps; 38 | % constraint 1: x_k(t_k) = x_{k+1}(t_k) = p_k, where p_k is a 39 | % waypoint 40 | % x_k(t) = c_{k,3}*t^3 + c_{k,2}*t^2 + c_{k,1}*t^1 + c_{k,0} 41 | % for k = 1..(m-1) 42 | % e.g. x_1(t_1) = x_2(t_1) = p_1; 43 | % there are in total 2*(m-1) constraints 44 | idx = 1; % constraint counter 45 | for k = 1:(m-1) 46 | A(idx, 4*(k-1)+1:4*k, i) = [ts(k+1)^3, ts(k+1)^2, ts(k+1), 1]; 47 | Y(idx,i) = path0(k+1,i); 48 | idx = idx + 1; 49 | A(idx, 4*(k)+1:4*(k+1), i) = [ts(k+1)^3, ts(k+1)^2, ts(k+1), 1]; 50 | Y(idx,i) = path0(k+1,i); 51 | idx = idx + 1; 52 | end 53 | % constraint 2: \dot{x}_k(t_k) = \dot{x}_{k+1}(t_k) 54 | % \dot{x}_k(t) = 3*c_{k,3}*t^2 + 2*c_{k,2}*t + c_{k,1} 55 | % e.g. \dot{x}_1(t_1) = \dot{x}_2(t_1) 56 | % there are in total m-1 constraints 57 | for k = 1:(m-1) 58 | A(idx, 4*(k-1)+1:4*k, i) = [3*ts(k+1)^2, 2*ts(k+1), 1, 0]; 59 | A(idx, 4*(k)+1:4*(k+1), i) = -[3*ts(k+1)^2, 2*ts(k+1), 1, 0]; 60 | Y(idx,i) = 0; 61 | idx = idx + 1; 62 | end 63 | 64 | % constraint 3: \ddot{x}_k(t_k) = \ddot{x}_{k+1}(t_k) 65 | % \ddot{x}_k(t) =6*c_{k,3}*t + 2*c_{k,2} 66 | % e.g. \ddot{x}_1(t_1) = \ddot{x}_2(t_1) 67 | % there are in total m-1 constraints 68 | for k = 1:(m-1) 69 | A(idx, 4*(k-1)+1:4*k, i) = [6*ts(k+1), 2, 0, 0]; 70 | A(idx, 4*(k)+1:4*(k+1), i) = -[6*ts(k+1), 2, 0, 0]; 71 | Y(idx,i) = 0; 72 | idx = idx + 1; 73 | end 74 | 75 | % so far there are 2*(m-1) + (m-1) + (m-1) = 4m - 4 constraints 76 | % there are 4 left: 77 | % x_1(t_0) = p_0 78 | % x_T(t_T) = p_T 79 | % \dot{x}_0(t_0) = 0 80 | % \dot{x}_T(t_T) = 0 81 | k = 1; 82 | A(idx, 4*(k-1)+1:4*k, i) = [ts(k)^3, ts(k)^2, ts(k), 1]; 83 | Y(idx,i) = path0(k,i); 84 | idx = idx + 1; 85 | A(idx, 4*(k-1)+1:4*k, i) = [3*ts(k)^2, 2*ts(k), 1, 0]; 86 | Y(idx,i) = 0; 87 | idx = idx + 1; 88 | k = m; 89 | A(idx, 4*(k-1)+1:4*k, i) = [ts(k+1)^3, ts(k+1)^2, ts(k+1), 1]; 90 | Y(idx,i) = path0(k+1,i); 91 | idx = idx + 1; 92 | A(idx, 4*(k-1)+1:4*k, i) = [3*ts(k+1)^2, 2*ts(k+1), 1, 0]; 93 | Y(idx,i) = 0; 94 | idx = idx + 1; 95 | % add regulerization to prevent singularity 96 | A(:,:,i) = A(:,:,i)+eye(4*m)*eps; 97 | X(:,i) = A(:,:,i)\Y(:,i); 98 | end 99 | 100 | end -------------------------------------------------------------------------------- /traj_planning/traj_opt3c.m: -------------------------------------------------------------------------------- 1 | function X = traj_opt3c(path, total_time, ts) 2 | % 3rd order trajectory optimization (minimum acceleration) 3 | % by solving 4m linear equations. Assumes constant speed 4 | % @author Yiren Lu 5 | % @email luyiren [at] seas [dot] upenn [dot] edu 6 | % 7 | % @input: path (m+1) by 3 planning trajectory 8 | % total_time total time 9 | % @output X 4mx3 solution vector, where 10 | % X(4*(k-1)+1:4*k,d) is the coefficients 11 | % of x_k(t), i.e., 12 | % x_k(t)=X(4*(k-1)+1:4*k,d)'*[t^3;t^2,t,1] 13 | % where d \in [1,2,3] to indicate dimension xyz 14 | path0 = path; 15 | % generate the trajectory here, and decide the total_time 16 | [m,n] = size(path0); % n == 3 17 | % there we set m = m - 1 for convenience 18 | m = m-1; 19 | % there are now in total m+1 points in the path, which seperate the path into 20 | % m subpaths. 21 | 22 | % ts(k) = t_{k+1}, e.g. ts(1) = t_0, 23 | % time planning according to the segment length 24 | 25 | % ts(m+1) = t_m 26 | % In 3rd order trajectory optimization, for each subpath there are 4 27 | % parameters 28 | % x_k(t) = c_{k,3}*t^3 + c_{k,2}*t^2 + c_{k,1}*t^1 + c_{k,0} 29 | % for k = 1..(m-1) 30 | 31 | % X contains the parameters 32 | X = zeros(4*m,n); 33 | A = zeros(4*m, 4*m, n); 34 | Y = zeros(4*m,n); 35 | 36 | for i = 3 37 | A(:,:,i) = eye(4*m)*eps; 38 | % constraint 1: x_k(t_k) = x_{k+1}(t_k) = p_k, where p_k is a 39 | % waypoint 40 | % x_k(t) = c_{k,3}*t^3 + c_{k,2}*t^2 + c_{k,1}*t^1 + c_{k,0} 41 | % for k = 1..(m-1) 42 | % e.g. x_1(t_1) = x_2(t_1) = p_1; 43 | % there are in total 2*(m-1) constraints 44 | idx = 1; % constraint counter 45 | for k = 1:(m-1) 46 | A(idx, 4*(k-1)+1:4*k, i) = [ts(k+1)^3, ts(k+1)^2, ts(k+1), 1]; 47 | Y(idx,i) = path0(k+1,i); 48 | idx = idx + 1; 49 | A(idx, 4*(k)+1:4*(k+1), i) = [ts(k+1)^3, ts(k+1)^2, ts(k+1), 1]; 50 | Y(idx,i) = path0(k+1,i); 51 | idx = idx + 1; 52 | end 53 | % constraint 2: \dot{x}_k(t_k) = \dot{x}_{k+1}(t_k) 54 | % \dot{x}_k(t) = 3*c_{k,3}*t^2 + 2*c_{k,2}*t + c_{k,1} 55 | % e.g. \dot{x}_1(t_1) = \dot{x}_2(t_1) 56 | % there are in total m-1 constraints 57 | for k = 1:(m-1) 58 | A(idx, 4*(k-1)+1:4*k, i) = [3*ts(k+1)^2, 2*ts(k+1), 1, 0]; 59 | % A(idx, 4*(k)+1:4*(k+1), i) = -[3*ts(k+1)^2, 2*ts(k+1), 1, 0]; 60 | Y(idx,i) = 0; 61 | idx = idx + 1; 62 | A(idx, 4*(k)+1:4*(k+1), i) = [3*ts(k+1)^2, 2*ts(k+1), 1, 0]; 63 | Y(idx,i) = 0; 64 | idx = idx + 1; 65 | end 66 | 67 | % % constraint 3: \ddot{x}_k(t_k) = \ddot{x}_{k+1}(t_k) 68 | % % \ddot{x}_k(t) =6*c_{k,3}*t + 2*c_{k,2} 69 | % % e.g. \ddot{x}_1(t_1) = \ddot{x}_2(t_1) 70 | % % there are in total m-1 constraints 71 | % for k = 1:(m-1) 72 | % A(idx, 4*(k-1)+1:4*k, i) = [6*ts(k+1), 2, 0, 0]; 73 | % A(idx, 4*(k)+1:4*(k+1), i) = -[6*ts(k+1), 2, 0, 0]; 74 | % Y(idx,i) = 0; 75 | % idx = idx + 1; 76 | % end 77 | 78 | % so far there are 2*(m-1) + (m-1) + (m-1) = 4m - 4 constraints 79 | % there are 4 left: 80 | % x_1(t_0) = p_0 81 | % x_T(t_T) = p_T 82 | % \dot{x}_0(t_0) = 0 83 | % \dot{x}_T(t_T) = 0 84 | k = 1; 85 | A(idx, 4*(k-1)+1:4*k, i) = [ts(k)^3, ts(k)^2, ts(k), 1]; 86 | Y(idx,i) = path0(k,i); 87 | idx = idx + 1; 88 | A(idx, 4*(k-1)+1:4*k, i) = [3*ts(k)^2, 2*ts(k), 1, 0]; 89 | Y(idx,i) = 0; 90 | idx = idx + 1; 91 | k = m; 92 | A(idx, 4*(k-1)+1:4*k, i) = [ts(k+1)^3, ts(k+1)^2, ts(k+1), 1]; 93 | Y(idx,i) = path0(k+1,i); 94 | idx = idx + 1; 95 | A(idx, 4*(k-1)+1:4*k, i) = [3*ts(k+1)^2, 2*ts(k+1), 1, 0]; 96 | Y(idx,i) = 0; 97 | idx = idx + 1; 98 | % assert(rank(A(:,:,i))==4*m); 99 | X(:,i) = A(:,:,i)\Y(:,i); 100 | end 101 | % for i = 1:n 102 | % A(:,:,i) = eye(4*m)*eps; 103 | % % constraint 1: x_k(t_k) = x_{k+1}(t_k) = p_k, where p_k is a 104 | % % waypoint 105 | % % x_k(t) = c_{k,3}*t^3 + c_{k,2}*t^2 + c_{k,1}*t^1 + c_{k,0} 106 | % % for k = 1..(m-1) 107 | % % e.g. x_1(t_1) = x_2(t_1) = p_1; 108 | % % there are in total 2*(m-1) constraints 109 | % idx = 1; % constraint counter 110 | % for k = 1:(m-1) 111 | % A(idx, 4*(k-1)+1:4*k, i) = [ts(k+1)^3, ts(k+1)^2, ts(k+1), 1]; 112 | % Y(idx,i) = path0(k+1,i); 113 | % idx = idx + 1; 114 | % A(idx, 4*(k)+1:4*(k+1), i) = [ts(k+1)^3, ts(k+1)^2, ts(k+1), 1]; 115 | % Y(idx,i) = path0(k+1,i); 116 | % idx = idx + 1; 117 | % end 118 | % % constraint 2: \dot{x}_k(t_k) = \dot{x}_{k+1}(t_k) 119 | % % \dot{x}_k(t) = 3*c_{k,3}*t^2 + 2*c_{k,2}*t + c_{k,1} 120 | % % e.g. \dot{x}_1(t_1) = \dot{x}_2(t_1) 121 | % % there are in total m-1 constraints 122 | % for k = 1:(m-1) 123 | % A(idx, 4*(k-1)+1:4*k, i) = [3*ts(k+1)^2, 2*ts(k+1), 1, 0]; 124 | % A(idx, 4*(k)+1:4*(k+1), i) = -[3*ts(k+1)^2, 2*ts(k+1), 1, 0]; 125 | % Y(idx,i) = 0; 126 | % idx = idx + 1; 127 | % end 128 | % 129 | % % constraint 3: \ddot{x}_k(t_k) = \ddot{x}_{k+1}(t_k) 130 | % % \ddot{x}_k(t) =6*c_{k,3}*t + 2*c_{k,2} 131 | % % e.g. \ddot{x}_1(t_1) = \ddot{x}_2(t_1) 132 | % % there are in total m-1 constraints 133 | % for k = 1:(m-1) 134 | % A(idx, 4*(k-1)+1:4*k, i) = [6*ts(k+1), 2, 0, 0]; 135 | % A(idx, 4*(k)+1:4*(k+1), i) = -[6*ts(k+1), 2, 0, 0]; 136 | % Y(idx,i) = 0; 137 | % idx = idx + 1; 138 | % end 139 | % 140 | % % so far there are 2*(m-1) + (m-1) + (m-1) = 4m - 4 constraints 141 | % % there are 4 left: 142 | % % x_1(t_0) = p_0 143 | % % x_T(t_T) = p_T 144 | % % \dot{x}_0(t_0) = 0 145 | % % \dot{x}_T(t_T) = 0 146 | % k = 1; 147 | % A(idx, 4*(k-1)+1:4*k, i) = [ts(k)^3, ts(k)^2, ts(k), 1]; 148 | % Y(idx,i) = path0(k,i); 149 | % idx = idx + 1; 150 | % A(idx, 4*(k-1)+1:4*k, i) = [3*ts(k)^2, 2*ts(k), 1, 0]; 151 | % Y(idx,i) = 0; 152 | % idx = idx + 1; 153 | % k = m; 154 | % A(idx, 4*(k-1)+1:4*k, i) = [ts(k+1)^3, ts(k+1)^2, ts(k+1), 1]; 155 | % Y(idx,i) = path0(k+1,i); 156 | % idx = idx + 1; 157 | % A(idx, 4*(k-1)+1:4*k, i) = [3*ts(k+1)^2, 2*ts(k+1), 1, 0]; 158 | % Y(idx,i) = 0; 159 | % idx = idx + 1; 160 | % % assert(rank(A(:,:,i))==4*m); 161 | % X(:,i) = A(:,:,i)\Y(:,i); 162 | % end 163 | 164 | end -------------------------------------------------------------------------------- /traj_planning/traj_opt7.m: -------------------------------------------------------------------------------- 1 | function X = traj_opt7(path, total_time, ts) 2 | % Minimum snap trajectory optimization 3 | % by solving 8m linear equations. Assumes constant speed 4 | % @author Yiren Lu 5 | % @email luyiren [at] seas [dot] upenn [dot] edu 6 | % 7 | % @input: path (m+1) by 3 planning trajectory 8 | % total_time total time 9 | % @output X 8mx3 solution vector, where 10 | % X(8*(k-1)+1:8*k,d) is the coefficients 11 | % of x_k(t), i.e., 12 | % x_k(t)=X(8*(k-1)+1:8*k,d)'*[t^3;t^2,t,1] 13 | % where d \in [1,2,3] to indicate dimension xyz 14 | path0 = path; 15 | % generate the trajectory here, and decide the total_time 16 | [m,n] = size(path0); % n == 3 17 | % there we set m = m - 1 for convenience 18 | m = m-1; 19 | % there are now in total m+1 points in the path, which seperate the path into 20 | % m subpaths. 21 | 22 | % ts(k) = t_{k+1}, e.g. ts(1) = t_0, 23 | % time planning according to the segment length 24 | 25 | % ts(m+1) = t_m 26 | % In 7rd order (minimum snap) trajectory optimization, for each subpath there are 27 | % 8 parameters 28 | % x_k(t) = sum_{i=1}^8 c_{k,i-1}*t^{i-1} 29 | % for k = 1..(m-1) 30 | 31 | % X contains the parameters 32 | X = zeros(8*m,n); 33 | A = zeros(8*m, 8*m, n); 34 | Y = zeros(8*m,n); 35 | 36 | for i = 1:n 37 | A(:,:,i) = eye(8*m)*eps; 38 | % constraint 1: x_k(t_k) = x_{k+1}(t_k) = p_k, where p_k is a 39 | % waypoint 40 | % x_k(t) = sum_{i=1}^8 c_{k,i-1}*t^{i-1} 41 | % for k = 1..(m-1) 42 | % e.g. x_1(t_1) = x_2(t_1) = p_1; 43 | % there are in total 2*(m-1) constraints 44 | idx = 1; % constraint counter 45 | for k = 1:(m-1) 46 | A(idx, 8*(k-1)+1:8*k, i) = [ts(k+1)^7, ts(k+1)^6, ts(k+1)^5, ts(k+1)^4, ts(k+1)^3, ts(k+1)^2, ts(k+1), 1]; 47 | Y(idx,i) = path0(k+1,i); 48 | idx = idx + 1; 49 | A(idx, 8*(k)+1:8*(k+1), i) = [ts(k+1)^7, ts(k+1)^6, ts(k+1)^5, ts(k+1)^4, ts(k+1)^3, ts(k+1)^2, ts(k+1), 1]; 50 | Y(idx,i) = path0(k+1,i); 51 | idx = idx + 1; 52 | end 53 | % constraint 2: \dot{x}_k(t_k) = \dot{x}_{k+1}(t_k) 54 | % e.g. \dot{x}_1(t_1) = \dot{x}_2(t_1) 55 | % there are in total m-1 constraints 56 | for k = 1:(m-1) 57 | A(idx, 8*(k-1)+1:8*k, i) = [7*ts(k+1)^6, 6*ts(k+1)^5, 5*ts(k+1)^4, 4*ts(k+1)^3, 3*ts(k+1)^2, 2*ts(k+1), 1, 0]; 58 | A(idx, 8*(k)+1:8*(k+1), i) = -[7*ts(k+1)^6, 6*ts(k+1)^5, 5*ts(k+1)^4, 4*ts(k+1)^3, 3*ts(k+1)^2, 2*ts(k+1), 1, 0]; 59 | Y(idx,i) = 0; 60 | idx = idx + 1; 61 | end 62 | 63 | % constraint 3: \ddot{x}_k(t_k) = \ddot{x}_{k+1}(t_k) 64 | % e.g. \ddot{x}_1(t_1) = \ddot{x}_2(t_1) 65 | % there are in total m-1 constraints 66 | for k = 1:(m-1) 67 | A(idx, 8*(k-1)+1:8*k, i) = [42*ts(k+1)^5, 30*ts(k+1)^4, 20*ts(k+1)^3, 12*ts(k+1)^2, 6*ts(k+1), 2, 0, 0]; 68 | A(idx, 8*(k)+1:8*(k+1), i) = -[42*ts(k+1)^5, 30*ts(k+1)^4, 20*ts(k+1)^3, 12*ts(k+1)^2, 6*ts(k+1), 2, 0, 0]; 69 | Y(idx,i) = 0; 70 | idx = idx + 1; 71 | end 72 | 73 | % constraint 4: x^(3)_k(t_k) = x^(3)_{k+1}(t_k) 74 | % e.g. x^(3)_1(t_1) = x^(3)_2(t_1) 75 | % there are in total m-1 constraints 76 | for k = 1:(m-1) 77 | A(idx, 8*(k-1)+1:8*k, i) = [210*ts(k+1)^4, 120*ts(k+1)^3, 60*ts(k+1)^2, 24*ts(k+1), 6, 0, 0, 0]; 78 | A(idx, 8*(k)+1:8*(k+1), i) = -[210*ts(k+1)^4, 120*ts(k+1)^3, 60*ts(k+1)^2, 24*ts(k+1), 6, 0, 0, 0]; 79 | Y(idx,i) = 0; 80 | idx = idx + 1; 81 | end 82 | 83 | % constraint 5: x^(4)_k(t_k) = x^(4)_{k+1}(t_k) 84 | % e.g. x^(4)_1(t_1) = x^(4)_2(t_1) 85 | % there are in total m-1 constraints 86 | for k = 1:(m-1) 87 | A(idx, 8*(k-1)+1:8*k, i) = [840*ts(k+1)^3, 360*ts(k+1)^2, 120*ts(k+1), 24, 0, 0, 0, 0]; 88 | A(idx, 8*(k)+1:8*(k+1), i) = -[840*ts(k+1)^3, 360*ts(k+1)^2, 120*ts(k+1), 24, 0, 0, 0, 0]; 89 | Y(idx,i) = 0; 90 | idx = idx + 1; 91 | end 92 | 93 | % constraint 6: x^(5)_k(t_k) = x^(5)_{k+1}(t_k) 94 | % e.g. x^(5)_1(t_1) = x^(5)_2(t_1) 95 | % there are in total m-1 constraints 96 | for k = 1:(m-1) 97 | A(idx, 8*(k-1)+1:8*k, i) = [2520*ts(k+1)^2, 720*ts(k+1), 120, 0, 0, 0, 0, 0]; 98 | A(idx, 8*(k)+1:8*(k+1), i) = -[2520*ts(k+1)^2, 720*ts(k+1), 120, 0, 0, 0, 0, 0]; 99 | Y(idx,i) = 0; 100 | idx = idx + 1; 101 | end 102 | 103 | % constraint 7: x^(6)_k(t_k) = x^(6)_{k+1}(t_k) 104 | % e.g. x^(6)_1(t_1) = x^(6)_2(t_1) 105 | % there are in total m-1 constraints 106 | for k = 1:(m-1) 107 | A(idx, 8*(k-1)+1:8*k, i) = [5040*ts(k+1), 720, 0, 0, 0, 0, 0, 0]; 108 | A(idx, 8*(k)+1:8*(k+1), i) = -[5040*ts(k+1), 720, 0, 0, 0, 0, 0, 0]; 109 | Y(idx,i) = 0; 110 | idx = idx + 1; 111 | end 112 | 113 | % so far there are 8(m-1) constraints 114 | % there are 8 left: 115 | % x_1(t_0) = p_0 116 | % x^(1)_0(t_0) = 0 117 | % x^(2)_0(t_0) = 0 118 | % x^(3)_0(t_0) = 0 119 | % x_T(t_T) = p_T 120 | % x^(1)_T(t_T) = 0 121 | % x^(2)_T(t_T) = 0 122 | % x^(3)_T(t_T) = 0 123 | 124 | k = 1; 125 | A(idx, 8*(k-1)+1:8*k, i) = [ts(k)^7, ts(k)^6, ts(k)^5, ts(k)^4, ts(k)^3, ts(k)^2, ts(k), 1]; 126 | Y(idx,i) = path0(k,i); 127 | idx = idx + 1; 128 | A(idx, 8*(k-1)+1:8*k, i) = [7*ts(k)^6, 6*ts(k)^5, 5*ts(k)^4, 4*ts(k)^3, 3*ts(k)^2, 2*ts(k), 1, 0]; 129 | Y(idx,i) = 0; 130 | idx = idx + 1; 131 | A(idx, 8*(k-1)+1:8*k, i) = [42*ts(k)^5, 30*ts(k)^4, 20*ts(k)^3, 12*ts(k)^2, 6*ts(k), 2, 0, 0]; 132 | Y(idx,i) = 0; 133 | idx = idx + 1; 134 | A(idx, 8*(k-1)+1:8*k, i) = [210*ts(k)^4, 120*ts(k)^3, 60*ts(k)^2, 24*ts(k), 6, 0, 0, 0]; 135 | Y(idx,i) = 0; 136 | idx = idx + 1; 137 | 138 | k = m; 139 | A(idx, 8*(k-1)+1:8*k, i) = [ts(k+1)^7, ts(k+1)^6, ts(k+1)^5, ts(k+1)^4, ts(k+1)^3, ts(k+1)^2, ts(k+1), 1]; 140 | Y(idx,i) = path0(k+1,i); 141 | idx = idx + 1; 142 | A(idx, 8*(k-1)+1:8*k, i) = [7*ts(k+1)^6, 6*ts(k+1)^5, 5*ts(k+1)^4, 4*ts(k+1)^3, 3*ts(k+1)^2, 2*ts(k+1), 1, 0]; 143 | Y(idx,i) = 0; 144 | idx = idx + 1; 145 | A(idx, 8*(k-1)+1:8*k, i) = [42*ts(k+1)^5, 30*ts(k+1)^4, 20*ts(k+1)^3, 12*ts(k+1)^2, 6*ts(k+1), 2, 0, 0]; 146 | Y(idx,i) = 0; 147 | idx = idx + 1; 148 | A(idx, 8*(k-1)+1:8*k, i) = [210*ts(k+1)^4, 120*ts(k+1)^3, 60*ts(k+1)^2, 24*ts(k+1), 6, 0, 0, 0]; 149 | Y(idx,i) = 0; 150 | idx = idx + 1; 151 | % A(:,:,i) = A(:,:,i) + eye(8*m)*eps; 152 | X(:,i) = A(:,:,i)\Y(:,i); 153 | end 154 | -------------------------------------------------------------------------------- /traj_planning/traj_opt7c.m: -------------------------------------------------------------------------------- 1 | function X = traj_opt7c(path, total_time, ts) 2 | % Minimum snap trajectory optimization 3 | % by solving 8m linear equations. Assumes constant speed 4 | % @author Yiren Lu 5 | % @email luyiren [at] seas [dot] upenn [dot] edu 6 | % 7 | % @input: path (m+1) by 3 planning trajectory 8 | % total_time total time 9 | % @output X 8mx3 solution vector, where 10 | % X(8*(k-1)+1:8*k,d) is the coefficients 11 | % of x_k(t), i.e., 12 | % x_k(t)=X(8*(k-1)+1:8*k,d)'*[t^3;t^2,t,1] 13 | % where d \in [1,2,3] to indicate dimension xyz 14 | path0 = path; 15 | % generate the trajectory here, and decide the total_time 16 | [m,n] = size(path0); % n == 3 17 | % there we set m = m - 1 for convenience 18 | m = m-1; 19 | % there are now in total m+1 points in the path, which seperate the path into 20 | % m subpaths. 21 | 22 | % ts(k) = t_{k+1}, e.g. ts(1) = t_0, 23 | % time planning according to the segment length 24 | 25 | % ts(m+1) = t_m 26 | % In 7rd order (minimum snap) trajectory optimization, for each subpath there are 27 | % 8 parameters 28 | % x_k(t) = sum_{i=1}^8 c_{k,i-1}*t^{i-1} 29 | % for k = 1..(m-1) 30 | 31 | % X contains the parameters 32 | X = zeros(8*m,n); 33 | A = zeros(8*m, 8*m, n); 34 | Y = zeros(8*m,n); 35 | 36 | for i = 1:n 37 | A(:,:,i) = eye(8*m)*eps; 38 | % constraint 1: x_k(t_k) = x_{k+1}(t_k) = p_k, where p_k is a 39 | % waypoint 40 | % x_k(t) = sum_{i=1}^8 c_{k,i-1}*t^{i-1} 41 | % for k = 1..(m-1) 42 | % e.g. x_1(t_1) = x_2(t_1) = p_1; 43 | % there are in total 2*(m-1) constraints 44 | idx = 1; % constraint counter 45 | for k = 1:(m-1) 46 | A(idx, 8*(k-1)+1:8*k, i) = [ts(k+1)^7, ts(k+1)^6, ts(k+1)^5, ts(k+1)^4, ts(k+1)^3, ts(k+1)^2, ts(k+1), 1]; 47 | Y(idx,i) = path0(k+1,i); 48 | idx = idx + 1; 49 | A(idx, 8*(k)+1:8*(k+1), i) = [ts(k+1)^7, ts(k+1)^6, ts(k+1)^5, ts(k+1)^4, ts(k+1)^3, ts(k+1)^2, ts(k+1), 1]; 50 | Y(idx,i) = path0(k+1,i); 51 | idx = idx + 1; 52 | end 53 | % constraint 2: \dot{x}_k(t_k) = \dot{x}_{k+1}(t_k) 54 | % e.g. \dot{x}_1(t_1) = \dot{x}_2(t_1) 55 | % there are in total m-1 constraints 56 | for k = 1:(m-1) 57 | A(idx, 8*(k-1)+1:8*k, i) = [7*ts(k+1)^6, 6*ts(k+1)^5, 5*ts(k+1)^4, 4*ts(k+1)^3, 3*ts(k+1)^2, 2*ts(k+1), 1, 0]; 58 | % A(idx, 8*(k)+1:8*(k+1), i) = -[7*ts(k+1)^6, 6*ts(k+1)^5, 5*ts(k+1)^4, 4*ts(k+1)^3, 3*ts(k+1)^2, 2*ts(k+1), 1, 0]; 59 | Y(idx,i) = 0; 60 | idx = idx + 1; 61 | A(idx, 8*(k)+1:8*(k+1), i) = [7*ts(k+1)^6, 6*ts(k+1)^5, 5*ts(k+1)^4, 4*ts(k+1)^3, 3*ts(k+1)^2, 2*ts(k+1), 1, 0]; 62 | Y(idx,i) = 0; 63 | idx = idx + 1; 64 | end 65 | 66 | % constraint 3: \ddot{x}_k(t_k) = \ddot{x}_{k+1}(t_k) 67 | % e.g. \ddot{x}_1(t_1) = \ddot{x}_2(t_1) 68 | % there are in total m-1 constraints 69 | for k = 1:(m-1) 70 | A(idx, 8*(k-1)+1:8*k, i) = [42*ts(k+1)^5, 30*ts(k+1)^4, 20*ts(k+1)^3, 12*ts(k+1)^2, 6*ts(k+1), 2, 0, 0]; 71 | % A(idx, 8*(k)+1:8*(k+1), i) = -[42*ts(k+1)^5, 30*ts(k+1)^4, 20*ts(k+1)^3, 12*ts(k+1)^2, 6*ts(k+1), 2, 0, 0]; 72 | Y(idx,i) = 0; 73 | idx = idx + 1; 74 | A(idx, 8*(k)+1:8*(k+1), i) = [42*ts(k+1)^5, 30*ts(k+1)^4, 20*ts(k+1)^3, 12*ts(k+1)^2, 6*ts(k+1), 2, 0, 0]; 75 | Y(idx,i) = 0; 76 | idx = idx + 1; 77 | end 78 | 79 | % constraint 4: x^(3)_k(t_k) = x^(3)_{k+1}(t_k) 80 | % e.g. x^(3)_1(t_1) = x^(3)_2(t_1) 81 | % there are in total m-1 constraints 82 | for k = 1:(m-1) 83 | A(idx, 8*(k-1)+1:8*k, i) = [210*ts(k+1)^4, 120*ts(k+1)^3, 60*ts(k+1)^2, 24*ts(k+1), 6, 0, 0, 0]; 84 | % A(idx, 8*(k)+1:8*(k+1), i) = -[210*ts(k+1)^4, 120*ts(k+1)^3, 60*ts(k+1)^2, 24*ts(k+1), 6, 0, 0, 0]; 85 | Y(idx,i) = 0; 86 | idx = idx + 1; 87 | A(idx, 8*(k)+1:8*(k+1), i) = [210*ts(k+1)^4, 120*ts(k+1)^3, 60*ts(k+1)^2, 24*ts(k+1), 6, 0, 0, 0]; 88 | Y(idx,i) = 0; 89 | idx = idx + 1; 90 | end 91 | 92 | % % constraint 5: x^(4)_k(t_k) = x^(4)_{k+1}(t_k) 93 | % % e.g. x^(4)_1(t_1) = x^(4)_2(t_1) 94 | % % there are in total m-1 constraints 95 | % for k = 1:(m-1) 96 | % A(idx, 8*(k-1)+1:8*k, i) = [840*ts(k+1)^3, 360*ts(k+1)^2, 120*ts(k+1), 24, 0, 0, 0, 0]; 97 | % A(idx, 8*(k)+1:8*(k+1), i) = -[840*ts(k+1)^3, 360*ts(k+1)^2, 120*ts(k+1), 24, 0, 0, 0, 0]; 98 | % Y(idx,i) = 0; 99 | % idx = idx + 1; 100 | % end 101 | % 102 | % % constraint 6: x^(5)_k(t_k) = x^(5)_{k+1}(t_k) 103 | % % e.g. x^(5)_1(t_1) = x^(5)_2(t_1) 104 | % % there are in total m-1 constraints 105 | % for k = 1:(m-1) 106 | % A(idx, 8*(k-1)+1:8*k, i) = [2520*ts(k+1)^2, 720*ts(k+1), 120, 0, 0, 0, 0, 0]; 107 | % A(idx, 8*(k)+1:8*(k+1), i) = -[2520*ts(k+1)^2, 720*ts(k+1), 120, 0, 0, 0, 0, 0]; 108 | % Y(idx,i) = 0; 109 | % idx = idx + 1; 110 | % end 111 | % 112 | % % constraint 7: x^(6)_k(t_k) = x^(6)_{k+1}(t_k) 113 | % % e.g. x^(6)_1(t_1) = x^(6)_2(t_1) 114 | % % there are in total m-1 constraints 115 | % for k = 1:(m-1) 116 | % A(idx, 8*(k-1)+1:8*k, i) = [5040*ts(k+1), 720, 0, 0, 0, 0, 0, 0]; 117 | % A(idx, 8*(k)+1:8*(k+1), i) = -[5040*ts(k+1), 720, 0, 0, 0, 0, 0, 0]; 118 | % Y(idx,i) = 0; 119 | % idx = idx + 1; 120 | % end 121 | 122 | % so far there are 8(m-1) constraints 123 | % there are 8 left: 124 | % x_1(t_0) = p_0 125 | % x^(1)_0(t_0) = 0 126 | % x^(2)_0(t_0) = 0 127 | % x^(3)_0(t_0) = 0 128 | % x_T(t_T) = p_T 129 | % x^(1)_T(t_T) = 0 130 | % x^(2)_T(t_T) = 0 131 | % x^(3)_T(t_T) = 0 132 | 133 | k = 1; 134 | A(idx, 8*(k-1)+1:8*k, i) = [ts(k)^7, ts(k)^6, ts(k)^5, ts(k)^4, ts(k)^3, ts(k)^2, ts(k), 1]; 135 | Y(idx,i) = path0(k,i); 136 | idx = idx + 1; 137 | A(idx, 8*(k-1)+1:8*k, i) = [7*ts(k)^6, 6*ts(k)^5, 5*ts(k)^4, 4*ts(k)^3, 3*ts(k)^2, 2*ts(k), 1, 0]; 138 | Y(idx,i) = 0; 139 | idx = idx + 1; 140 | A(idx, 8*(k-1)+1:8*k, i) = [42*ts(k)^5, 30*ts(k)^4, 20*ts(k)^3, 12*ts(k)^2, 6*ts(k), 2, 0, 0]; 141 | Y(idx,i) = 0; 142 | idx = idx + 1; 143 | A(idx, 8*(k-1)+1:8*k, i) = [210*ts(k)^4, 120*ts(k)^3, 60*ts(k)^2, 24*ts(k), 6, 0, 0, 0]; 144 | Y(idx,i) = 0; 145 | idx = idx + 1; 146 | 147 | k = m; 148 | A(idx, 8*(k-1)+1:8*k, i) = [ts(k+1)^7, ts(k+1)^6, ts(k+1)^5, ts(k+1)^4, ts(k+1)^3, ts(k+1)^2, ts(k+1), 1]; 149 | Y(idx,i) = path0(k+1,i); 150 | idx = idx + 1; 151 | A(idx, 8*(k-1)+1:8*k, i) = [7*ts(k+1)^6, 6*ts(k+1)^5, 5*ts(k+1)^4, 4*ts(k+1)^3, 3*ts(k+1)^2, 2*ts(k+1), 1, 0]; 152 | Y(idx,i) = 0; 153 | idx = idx + 1; 154 | A(idx, 8*(k-1)+1:8*k, i) = [42*ts(k+1)^5, 30*ts(k+1)^4, 20*ts(k+1)^3, 12*ts(k+1)^2, 6*ts(k+1), 2, 0, 0]; 155 | Y(idx,i) = 0; 156 | idx = idx + 1; 157 | A(idx, 8*(k-1)+1:8*k, i) = [210*ts(k+1)^4, 120*ts(k+1)^3, 60*ts(k+1)^2, 24*ts(k+1), 6, 0, 0, 0]; 158 | Y(idx,i) = 0; 159 | idx = idx + 1; 160 | A(:,:,i) = A(:,:,i) + eye(8*m)*eps; 161 | X(:,i) = A(:,:,i)\Y(:,i); 162 | end 163 | -------------------------------------------------------------------------------- /traj_planning/trajectory_generator.m: -------------------------------------------------------------------------------- 1 | function [ desired_state ] = trajectory_generator(t, qn, map, path) 2 | % TRAJECTORY_GENERATOR: Turn a Dijkstra or A* path into a trajectory 3 | % 4 | % NOTE: This function would be called with variable number of input 5 | % arguments. In init_script, it will be called with arguments 6 | % trajectory_generator([], [], map, path) and later, in test_trajectory, 7 | % it will be called with only t and qn as arguments, so your code should 8 | % be able to handle that. This can be done by checking the number of 9 | % arguments to the function using the "nargin" variable, check the 10 | % MATLAB documentation for more information. 11 | % 12 | % map: The map structure returned by your load_map function 13 | % path: This is the path returned by your planner (dijkstra function) 14 | % 15 | % desired_state: Contains all the information that is passed to the 16 | % controller, as in phase 2 17 | % 18 | % It is suggested to use "persistent" variables to store map and path 19 | % during the initialization call of trajectory_generator, e.g. 20 | % persistent map0 path0 21 | % map0 = map; 22 | % path0 = path; 23 | persistent map0 path0 total_time X ts; 24 | if numel(t) == 0 | numel(qn) == 0 25 | map0 = map; 26 | path{1} = simplify_path2(map, path{1}); 27 | path0 = path; 28 | [ts, total_time] = generate_ts(path0{1}); 29 | % X = traj_opt(path0{1}, total_time,ts); 30 | X = traj_opt7(path0{1}, total_time,ts); 31 | return 32 | end 33 | 34 | if nargin < 4 35 | map = map0; 36 | path = path0; 37 | end 38 | 39 | p = path{qn}; 40 | if t >= total_time 41 | pos = p(end,:); 42 | vel = [0;0;0]; 43 | acc = [0;0;0]; 44 | else 45 | % 46 | % 3rd order trajectory planning 47 | % k = find(ts<=t); 48 | % k = k(end); 49 | % pos = [t^3, t^2, t, 1]*X(4*(k-1)+1:4*k,:); 50 | % vel = [3*t^2, 2*t, 1, 0]*X(4*(k-1)+1:4*k,:); 51 | % acc = [6*t, 2, 0, 0]*X(4*(k-1)+1:4*k,:); 52 | 53 | % % 7th order minimum snap trajectory 54 | k = find(ts<=t); 55 | k = k(end); 56 | pos = [t^7, t^6, t^5, t^4, t^3, t^2, t, 1]*X(8*(k-1)+1:8*k,:); 57 | vel = [7*t^6, 6*t^5, 5*t^4, 4*t^3, 3*t^2, 2*t, 1, 0]*X(8*(k-1)+1:8*k,:); 58 | acc = [42*t^5, 30*t^4, 20*t^3, 12*t^2, 6*t, 2, 0, 0]*X(8*(k-1)+1:8*k,:); 59 | end 60 | 61 | yaw = 0; 62 | yawdot = 0; 63 | 64 | % =================== Your code ends here =================== 65 | 66 | desired_state.pos = pos(:); 67 | desired_state.vel = vel(:); 68 | desired_state.acc = acc(:); 69 | desired_state.yaw = yaw; 70 | desired_state.yawdot = yawdot; 71 | -------------------------------------------------------------------------------- /traj_planning/utils/QuadPlot.m: -------------------------------------------------------------------------------- 1 | classdef QuadPlot < handle 2 | %QUADPLOT Visualization class for quad 3 | 4 | properties (SetAccess = public) 5 | k = 0; 6 | qn; % quad number 7 | time = 0; % time 8 | state; % state 9 | des_state; % desried state [x; y; z; xdot; ydot; zdot]; 10 | rot; % rotation matrix body to world 11 | 12 | color; % color of quad 13 | wingspan; % wingspan 14 | height; % height of quad 15 | motor; % motor position 16 | 17 | state_hist % position history 18 | state_des_hist; % desired position history 19 | time_hist; % time history 20 | max_iter; % max iteration 21 | end 22 | 23 | properties (SetAccess = private) 24 | h_3d 25 | h_m13; % motor 1 and 3 handle 26 | h_m24; % motor 2 and 4 handle 27 | h_qz; % z axis of quad handle 28 | h_qn; % quad number handle 29 | h_pos_hist; % position history handle 30 | h_pos_des_hist; % desired position history handle 31 | text_dist; % distance of quad number to quad 32 | end 33 | 34 | methods 35 | % Constructor 36 | function Q = QuadPlot(qn, state, wingspan, height, color, max_iter, h_3d) 37 | Q.qn = qn; 38 | Q.state = state; 39 | Q.wingspan = wingspan; 40 | Q.color = color; 41 | Q.height = height; 42 | Q.rot = QuatToRot(Q.state(7:10)); 43 | Q.motor = quad_pos(Q.state(1:3), Q.rot, Q.wingspan, Q.height); 44 | Q.text_dist = Q.wingspan / 3; 45 | Q.des_state = Q.state(1:6); 46 | 47 | Q.max_iter = max_iter; 48 | Q.state_hist = zeros(6, max_iter); 49 | Q.state_des_hist = zeros(6, max_iter); 50 | Q.time_hist = zeros(1, max_iter); 51 | 52 | % Initialize plot handle 53 | if nargin < 7, h_3d = gca; end 54 | Q.h_3d = h_3d; 55 | hold(Q.h_3d, 'on') 56 | Q.h_pos_hist = plot3(Q.h_3d, Q.state(1), Q.state(2), Q.state(3), 'r.'); 57 | Q.h_pos_des_hist = plot3(Q.h_3d, Q.des_state(1), Q.des_state(2), Q.des_state(3), 'b.'); 58 | Q.h_m13 = plot3(Q.h_3d, ... 59 | Q.motor(1,[1 3]), ... 60 | Q.motor(2,[1 3]), ... 61 | Q.motor(3,[1 3]), ... 62 | '-ko', 'MarkerFaceColor', Q.color, 'MarkerSize', 5); 63 | Q.h_m24 = plot3(Q.h_3d, ... 64 | Q.motor(1,[2 4]), ... 65 | Q.motor(2,[2 4]), ... 66 | Q.motor(3,[2 4]), ... 67 | '-ko', 'MarkerFaceColor', Q.color, 'MarkerSize', 5); 68 | Q.h_qz = plot3(Q.h_3d, ... 69 | Q.motor(1,[5 6]), ... 70 | Q.motor(2,[5 6]), ... 71 | Q.motor(3,[5 6]), ... 72 | 'Color', Q.color, 'LineWidth', 2); 73 | Q.h_qn = text(... 74 | Q.motor(1,5) + Q.text_dist, ... 75 | Q.motor(2,5) + Q.text_dist, ... 76 | Q.motor(3,5) + Q.text_dist, num2str(qn)); 77 | hold(Q.h_3d, 'off') 78 | end 79 | 80 | % Update quad state 81 | function UpdateQuadState(Q, state, time) 82 | Q.state = state; 83 | Q.time = time; 84 | Q.rot = QuatToRot(state(7:10))'; % Q.rot needs to be body-to-world 85 | end 86 | 87 | % Update desired quad state 88 | function UpdateDesiredQuadState(Q, des_state) 89 | Q.des_state = des_state; 90 | end 91 | 92 | % Update quad history 93 | function UpdateQuadHist(Q) 94 | Q.k = Q.k + 1; 95 | Q.time_hist(Q.k) = Q.time; 96 | Q.state_hist(:,Q.k) = Q.state(1:6); 97 | Q.state_des_hist(:,Q.k) = Q.des_state(1:6); 98 | end 99 | 100 | % Update motor position 101 | function UpdateMotorPos(Q) 102 | Q.motor = quad_pos(Q.state(1:3), Q.rot, Q.wingspan, Q.height); 103 | end 104 | 105 | % Truncate history 106 | function TruncateHist(Q) 107 | Q.time_hist = Q.time_hist(1:Q.k); 108 | Q.state_hist = Q.state_hist(:, 1:Q.k); 109 | Q.state_des_hist = Q.state_des_hist(:, 1:Q.k); 110 | end 111 | 112 | % Update quad plot 113 | function UpdateQuadPlot(Q, state, des_state, time) 114 | Q.UpdateQuadState(state, time); 115 | Q.UpdateDesiredQuadState(des_state); 116 | Q.UpdateQuadHist(); 117 | Q.UpdateMotorPos(); 118 | set(Q.h_m13, ... 119 | 'XData', Q.motor(1,[1 3]), ... 120 | 'YData', Q.motor(2,[1 3]), ... 121 | 'ZData', Q.motor(3,[1 3])); 122 | set(Q.h_m24, ... 123 | 'XData', Q.motor(1,[2 4]), ... 124 | 'YData', Q.motor(2,[2 4]), ... 125 | 'ZData', Q.motor(3,[2 4])); 126 | set(Q.h_qz, ... 127 | 'XData', Q.motor(1,[5 6]), ... 128 | 'YData', Q.motor(2,[5 6]), ... 129 | 'ZData', Q.motor(3,[5 6])) 130 | set(Q.h_qn, 'Position', ... 131 | [Q.motor(1,5) + Q.text_dist, ... 132 | Q.motor(2,5) + Q.text_dist, ... 133 | Q.motor(3,5) + Q.text_dist]); 134 | set(Q.h_pos_hist, ... 135 | 'XData', Q.state_hist(1,1:Q.k), ... 136 | 'YData', Q.state_hist(2,1:Q.k), ... 137 | 'ZData', Q.state_hist(3,1:Q.k)); 138 | set(Q.h_pos_des_hist, ... 139 | 'XData', Q.state_des_hist(1,1:Q.k), ... 140 | 'YData', Q.state_des_hist(2,1:Q.k), ... 141 | 'ZData', Q.state_des_hist(3,1:Q.k)); 142 | drawnow; 143 | end 144 | end 145 | 146 | end 147 | -------------------------------------------------------------------------------- /traj_planning/utils/QuatToRot.m: -------------------------------------------------------------------------------- 1 | function R = QuatToRot(q) 2 | %QuatToRot Converts a Quaternion to Rotation matrix 3 | % written by Daniel Mellinger 4 | 5 | % normalize q 6 | q = q./sqrt(sum(q.^2)); 7 | 8 | qahat(1,2) = -q(4); 9 | qahat(1,3) = q(3); 10 | qahat(2,3) = -q(2); 11 | qahat(2,1) = q(4); 12 | qahat(3,1) = -q(3); 13 | qahat(3,2) = q(2); 14 | 15 | R = eye(3) + 2*qahat*qahat + 2*q(1)*qahat; 16 | 17 | end -------------------------------------------------------------------------------- /traj_planning/utils/RPYtoRot_ZXY.m: -------------------------------------------------------------------------------- 1 | function R = RPYtoRot_ZXY(phi,theta,psi) 2 | %RPYtoRot_ZXY Converts roll, pitch, yaw to a body-to-world Rotation matrix 3 | % The rotation matrix in this function is world to body [bRw] you will 4 | % need to transpose this matrix to get the body to world [wRb] such that 5 | % [wP] = [wRb] * [bP], where [bP] is a point in the body frame and [wP] 6 | % is a point in the world frame 7 | % written by Daniel Mellinger 8 | % 9 | 10 | R = [cos(psi)*cos(theta) - sin(phi)*sin(psi)*sin(theta), ... 11 | cos(theta)*sin(psi) + cos(psi)*sin(phi)*sin(theta), ... 12 | -cos(phi)*sin(theta); ... 13 | -cos(phi)*sin(psi),... 14 | cos(phi)*cos(psi), ... 15 | sin(phi);... 16 | cos(psi)*sin(theta) + cos(theta)*sin(phi)*sin(psi),... 17 | sin(psi)*sin(theta) - cos(psi)*cos(theta)*sin(phi),... 18 | cos(phi)*cos(theta)]; 19 | 20 | end 21 | -------------------------------------------------------------------------------- /traj_planning/utils/RotToQuat.m: -------------------------------------------------------------------------------- 1 | function q = RotToQuat(R) 2 | %ROTTOQUAT Converts a Rotation matrix into a Quaternion 3 | % written by Daniel Mellinger 4 | % from the following website, deals with the case when tr<0 5 | % http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm 6 | 7 | %takes in W_R_B rotation matrix 8 | 9 | tr = R(1,1) + R(2,2) + R(3,3); 10 | 11 | if (tr > 0) 12 | S = sqrt(tr+1.0) * 2; % S=4*qw 13 | qw = 0.25 * S; 14 | qx = (R(3,2) - R(2,3)) / S; 15 | qy = (R(1,3) - R(3,1)) / S; 16 | qz = (R(2,1) - R(1,2)) / S; 17 | elseif ((R(1,1) > R(2,2)) && (R(1,1) > R(3,3))) 18 | S = sqrt(1.0 + R(1,1) - R(2,2) - R(3,3)) * 2; % S=4*qx 19 | qw = (R(3,2) - R(2,3)) / S; 20 | qx = 0.25 * S; 21 | qy = (R(1,2) + R(2,1)) / S; 22 | qz = (R(1,3) + R(3,1)) / S; 23 | elseif (R(2,2) > R(3,3)) 24 | S = sqrt(1.0 + R(2,2) - R(1,1) - R(3,3)) * 2; % S=4*qy 25 | qw = (R(1,3) - R(3,1)) / S; 26 | qx = (R(1,2) + R(2,1)) / S; 27 | qy = 0.25 * S; 28 | qz = (R(2,3) + R(3,2)) / S; 29 | else 30 | S = sqrt(1.0 + R(3,3) - R(1,1) - R(2,2)) * 2; % S=4*qz 31 | qw = (R(2,1) - R(1,2)) / S; 32 | qx = (R(1,3) + R(3,1)) / S; 33 | qy = (R(2,3) + R(3,2)) / S; 34 | qz = 0.25 * S; 35 | end 36 | 37 | q = [qw;qx;qy;qz]; 38 | q = q*sign(qw); 39 | 40 | end 41 | -------------------------------------------------------------------------------- /traj_planning/utils/RotToRPY_ZXY.m: -------------------------------------------------------------------------------- 1 | function [phi,theta,psi] = RotToRPY_ZXY(R) 2 | %RotToRPY_ZXY Extract Roll, Pitch, Yaw from a world-to-body Rotation Matrix 3 | % The rotation matrix in this function is world to body [bRw] you will 4 | % need to transpose the matrix if you have a body to world [wRb] such 5 | % that [wP] = [wRb] * [bP], where [bP] is a point in the body frame and 6 | % [wP] is a point in the world frame 7 | % written by Daniel Mellinger 8 | % bRw = [ cos(psi)*cos(theta) - sin(phi)*sin(psi)*sin(theta), 9 | % cos(theta)*sin(psi) + cos(psi)*sin(phi)*sin(theta), 10 | % -cos(phi)*sin(theta)] 11 | % [-cos(phi)*sin(psi), cos(phi)*cos(psi), sin(phi)] 12 | % [ cos(psi)*sin(theta) + cos(theta)*sin(phi)*sin(psi), 13 | % sin(psi)*sin(theta) - cos(psi)*cos(theta)*sin(phi), 14 | % cos(phi)*cos(theta)] 15 | 16 | phi = asin(R(2,3)); 17 | psi = atan2(-R(2,1)/cos(phi),R(2,2)/cos(phi)); 18 | theta = atan2(-R(1,3)/cos(phi),R(3,3)/cos(phi)); 19 | 20 | end 21 | -------------------------------------------------------------------------------- /traj_planning/utils/collision_check.m: -------------------------------------------------------------------------------- 1 | function collide = collision_check(p, margin) 2 | collide = 0; 3 | if(size(p,1) <= 1) 4 | return; 5 | end 6 | p(:,3) = p(:,3)/3; % scale z-axis by 3 to make it ellipsoid 7 | dis = pdist(p); 8 | if min(dis) < 2*margin 9 | collide = 1; 10 | end 11 | -------------------------------------------------------------------------------- /traj_planning/utils/init_state.m: -------------------------------------------------------------------------------- 1 | function [ s ] = init_state( start, yaw ) 2 | %INIT_STATE Initialize 13 x 1 state vector 3 | 4 | s = zeros(13,1); 5 | phi0 = 0.0; 6 | theta0 = 0.0; 7 | psi0 = yaw; 8 | Rot0 = RPYtoRot_ZXY(phi0, theta0, psi0); 9 | Quat0 = RotToQuat(Rot0); 10 | s(1) = start(1); %x 11 | s(2) = start(2); %y 12 | s(3) = start(3); %z 13 | s(4) = 0; %xdot 14 | s(5) = 0; %ydot 15 | s(6) = 0; %zdot 16 | s(7) = Quat0(1); %qw 17 | s(8) = Quat0(2); %qx 18 | s(9) = Quat0(3); %qy 19 | s(10) = Quat0(4); %qz 20 | s(11) = 0; %p 21 | s(12) = 0; %q 22 | s(13) = 0; %r 23 | 24 | end -------------------------------------------------------------------------------- /traj_planning/utils/plot_state.m: -------------------------------------------------------------------------------- 1 | function [ h_fig ] = plot_state( h_fig, state, time, name, type, view ) 2 | %PLOT_STATE visualize state data 3 | 4 | if nargin < 6, view = 'sep'; end 5 | if nargin < 5, type = 'vic'; end 6 | if nargin < 4, name = 'pos'; end 7 | if isempty(h_fig), h_fig = figure(); end 8 | line_width = 2; 9 | 10 | switch type 11 | case 'vic' 12 | line_color = 'r'; 13 | case 'des' 14 | line_color = 'b'; 15 | case 'est' 16 | line_color = 'g'; 17 | end 18 | 19 | switch name 20 | case 'pos' 21 | labels = {'x [m]', 'y [m]', 'z [m]'}; 22 | case 'vel' 23 | labels = {'xdot [m/s]', 'ydot [m/s]', 'zdot [m/s]'}; 24 | case 'euler' 25 | labels = {'roll [rad]', 'pitch [rad]', 'yaw [rad]'}; 26 | end 27 | 28 | figure(h_fig) 29 | if strcmp(view, 'sep') 30 | % Plot seperate 31 | 32 | for i = 1:3 33 | subplot(3, 1, i) 34 | hold on 35 | plot(time, state(i,:), line_color, 'LineWidth', line_width); 36 | hold off 37 | xlim([time(1), time(end)]) 38 | grid on 39 | xlabel('time [s]') 40 | ylabel(labels{i}) 41 | end 42 | elseif strcmp(view, '3d') 43 | % Plot 3d 44 | hold on 45 | plot3(state(1,:), state(2,:), state(3,:), line_color, 'LineWidth', line_width) 46 | hold off 47 | grid on 48 | xlabel(labels{1}); 49 | ylabel(labels{2}); 50 | zlabel(labels{3}); 51 | end 52 | 53 | end 54 | -------------------------------------------------------------------------------- /traj_planning/utils/qdToState.m: -------------------------------------------------------------------------------- 1 | function [x] = qdToState(qd) 2 | % Converts state vector for simulation to qd struct used in hardware. 3 | % x is 1 x 13 vector of state variables [pos vel quat omega] 4 | % qd is a struct including the fields pos, vel, euler, and omega 5 | 6 | x = zeros(1,13); %initialize dimensions 7 | 8 | x(1:3) = qd.pos; 9 | x(4:6) = qd.vel; 10 | 11 | Rot = RPYtoRot_ZXY(qd.euler(1), qd.euler(2), qd.euler(3)); 12 | quat = RotToQuat(Rot); 13 | 14 | x(7:10) = quat; 15 | x(11:13) = qd.omega; 16 | 17 | end 18 | -------------------------------------------------------------------------------- /traj_planning/utils/quad_pos.m: -------------------------------------------------------------------------------- 1 | function [ quad ] = quad_pos( pos, rot, L, H ) 2 | %QUAD_POS Calculates coordinates of quadrotor's position in world frame 3 | % pos 3x1 position vector [x; y; z]; 4 | % rot 3x3 body-to-world rotation matrix 5 | % L 1x1 length of the quad 6 | 7 | if nargin < 4; H = 0.05; end 8 | 9 | % wRb = RPYtoRot_ZXY(euler(1), euler(2), euler(3))'; 10 | wHb = [rot pos(:); 0 0 0 1]; % homogeneous transformation from body to world 11 | 12 | quadBodyFrame = [L 0 0 1; 0 L 0 1; -L 0 0 1; 0 -L 0 1; 0 0 0 1; 0 0 H 1]'; 13 | quadWorldFrame = wHb * quadBodyFrame; 14 | quad = quadWorldFrame(1:3, :); 15 | 16 | end 17 | -------------------------------------------------------------------------------- /traj_planning/utils/stateToQd.m: -------------------------------------------------------------------------------- 1 | function [qd] = stateToQd(x) 2 | %Converts qd struct used in hardware to x vector used in simulation 3 | % x is 1 x 13 vector of state variables [pos vel quat omega] 4 | % qd is a struct including the fields pos, vel, euler, and omega 5 | 6 | %current state 7 | qd.pos = x(1:3); 8 | qd.vel = x(4:6); 9 | 10 | Rot = QuatToRot(x(7:10)'); 11 | [phi, theta, yaw] = RotToRPY_ZXY(Rot); 12 | 13 | qd.euler = [phi; theta; yaw]; 14 | qd.omega = x(11:13); 15 | 16 | end 17 | -------------------------------------------------------------------------------- /traj_planning/utils/terminate_check.m: -------------------------------------------------------------------------------- 1 | function [ terminate_cond ] = terminate_check( x, time, stop, pos_tol, vel_tol, time_tol) 2 | %TERMINATE_CHECK Check termination criteria, including position, velocity and time 3 | nquad = length(stop); 4 | 5 | % Initialize 6 | pos_check = true; 7 | vel_check = true; 8 | pos_col_check = zeros(nquad, 3); 9 | 10 | % Check position and velocity and still time for each quad 11 | for qn = 1:nquad 12 | pos_check = pos_check && (norm(x{qn}(1:3) - stop{qn}) < pos_tol); 13 | vel_check = vel_check && (norm(x{qn}(4:6)) < vel_tol); 14 | pos_col_check(qn,:) = x{qn}(1:3)'; 15 | end 16 | 17 | % Check total simulation time 18 | time_check = time > time_tol; 19 | % Check collision criteria 20 | col_check = collision_check(pos_col_check, 0.3); 21 | 22 | if (pos_check && vel_check) 23 | terminate_cond = 1; % Robot reaches goal and stops, successful 24 | elseif time_check 25 | terminate_cond = 2; % Robot doesn't reach goal within given time, not complete 26 | elseif col_check 27 | terminate_cond = 3; % Robot collide with each other 28 | else 29 | terminate_cond = 0; 30 | end 31 | 32 | end --------------------------------------------------------------------------------