├── dcm_trajectories.png ├── compute_rvrp_given_dcm.m ├── get_xi_d.m ├── get_xi_vel_d.m ├── get_trajectory_length.m ├── get_DS_end_time.m ├── get_DS_start_time.m ├── get_t_exp_start.m ├── get_DS_start_time.asv ├── get_xi_DS_poly.m ├── get_xi_vel_DS_poly.m ├── get_polynomial_matrix.m ├── get_xi_ref_d.m ├── get_xi_vel_ref_d.m ├── which_step_index.m ├── README.md ├── which_hybrid_step_index.m ├── get_xi_ref_hybrid_d.m ├── get_xi_vel_ref_hybrid_d.m ├── dcm_test.asv └── dcm_test.m /dcm_trajectories.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stevenjj/dcm_matlab/HEAD/dcm_trajectories.png -------------------------------------------------------------------------------- /compute_rvrp_given_dcm.m: -------------------------------------------------------------------------------- 1 | function r_vrp_traj = compute_rvrp_given_dcm(b, xi, xi_vel) 2 | r_vrp_traj = xi - b*xi_vel; 3 | end -------------------------------------------------------------------------------- /get_xi_d.m: -------------------------------------------------------------------------------- 1 | function xi_d = get_xi_d(t, t_step, b, r_vrp, xi_eos) 2 | xi_d = r_vrp + exp((1/b)*(t-t_step)).*(xi_eos - r_vrp); 3 | end 4 | -------------------------------------------------------------------------------- /get_xi_vel_d.m: -------------------------------------------------------------------------------- 1 | function xi_vel_d = get_xi_vel_d(t, t_step, b, r_vrp, xi_eos) 2 | xi_vel_d = (1/b)*exp((1/b)*(t-t_step)).*(xi_eos - r_vrp); 3 | end 4 | -------------------------------------------------------------------------------- /get_trajectory_length.m: -------------------------------------------------------------------------------- 1 | function t_traj_length = get_trajectory_length(t_step) 2 | t_traj_length = 0.0; 3 | for(i = 1:size(t_step,2)) 4 | t_traj_length = t_traj_length + t_step(i); 5 | end -------------------------------------------------------------------------------- /get_DS_end_time.m: -------------------------------------------------------------------------------- 1 | function t_ds_end = get_DS_end_time(step_index, t_step, t_ds_vec, alpha) 2 | t_ds_start = get_DS_start_time(step_index, t_step, t_ds_vec, alpha); 3 | t_ds_end = t_ds_start + t_ds_vec(step_index); 4 | end -------------------------------------------------------------------------------- /get_DS_start_time.m: -------------------------------------------------------------------------------- 1 | function t_ds_start = get_DS_start_time(step_index, t_step, t_ds_vec, alpha) 2 | t_ds_start = get_t_exp_start(step_index, t_step); 3 | if (step_index > 1) 4 | t_ds_start = t_ds_start - alpha*t_ds_vec(step_index); 5 | end 6 | end -------------------------------------------------------------------------------- /get_t_exp_start.m: -------------------------------------------------------------------------------- 1 | function t_local_start = get_t_exp_start(step_index, t_step) 2 | t_local_start = 0.0; 3 | for(i = 1:step_index) 4 | if (i < step_index) 5 | t_local_start = t_local_start + t_step(i); 6 | else 7 | break 8 | end 9 | end 10 | end -------------------------------------------------------------------------------- /get_DS_start_time.asv: -------------------------------------------------------------------------------- 1 | function t_ds_start = get_DS_start_time(step_index, t_step, t_ds_vec, alpha) 2 | t_ds_start = get_t_exp_start(step_index, t_step); 3 | if (step_index == size(t_step,2)) 4 | elseif (step_index == 1) 5 | t_ds_start = t_ds_start; 6 | else 7 | t_ds_start = t_ds_start - alpha*t_ds_vec(step_index); 8 | end 9 | end -------------------------------------------------------------------------------- /get_xi_DS_poly.m: -------------------------------------------------------------------------------- 1 | function xi_DS_poly = get_xi_DS_poly(t, Ts, P_mat) 2 | xi_DS_poly = zeros(3, size(t,2)); 3 | 4 | % Clamp input to boundary Ts 5 | for (i = 1:size(t,2)) 6 | t_input = max(min(t(i), Ts), 0.0); 7 | time_vec = [t_input^3, t_input^2, t_input, 1]; 8 | xi_DS_poly(:,i) = time_vec*P_mat; 9 | end 10 | end -------------------------------------------------------------------------------- /get_xi_vel_DS_poly.m: -------------------------------------------------------------------------------- 1 | function xi_vel_DS_poly = get_xi_vel_DS_poly(t, Ts, P_mat) 2 | xi_vel_DS_poly = zeros(3, size(t,2)); 3 | % Clamp input to boundary Ts 4 | for (i = 1:size(t,2)) 5 | t_input = max(min(t(i), Ts), 0.0); 6 | time_vec = [3*(t_input^2), 2*(t_input), 1, 0]; 7 | xi_vel_DS_poly(:,i) = time_vec*P_mat; 8 | end 9 | end -------------------------------------------------------------------------------- /get_polynomial_matrix.m: -------------------------------------------------------------------------------- 1 | function P = get_polynomial_matrix(Ts, xi_ini, xi_ini_vel, xi_end, xi_end_vel) 2 | Pmat = zeros(4,4); 3 | boundMat = [xi_ini'; xi_ini_vel'; xi_end'; xi_end_vel']; 4 | Pmat(1, :) = [2.0/(Ts^3), 1/(Ts^2), -2/(Ts^3), 1/(Ts^2) ]; 5 | Pmat(2, :) = [-3.0/(Ts^2), -2.0/(Ts), 3/(Ts^2), -1/(Ts) ]; 6 | Pmat(3, :) = [0, 1, 0, 0]; 7 | Pmat(4, :) = [1, 0, 0, 0]; 8 | P = Pmat*boundMat; 9 | end -------------------------------------------------------------------------------- /get_xi_ref_d.m: -------------------------------------------------------------------------------- 1 | function xi_ref_d = get_xi_ref_d(t, t_step, b, r_vrp, xi_eos) 2 | xi_ref_d = zeros(3, size(t,2)); 3 | t_local_start = 0.0; 4 | for(i = 1:size(xi_ref_d,2)) 5 | step_index = which_step_index(t(i), t_step); 6 | t_local_start = get_t_exp_start(step_index, t_step); 7 | xi_ref_d(:, i) = get_xi_d(t(i) - t_local_start, t_step(step_index), b, r_vrp(:, step_index), xi_eos(:, step_index)); 8 | end 9 | end 10 | -------------------------------------------------------------------------------- /get_xi_vel_ref_d.m: -------------------------------------------------------------------------------- 1 | function xi_vel_ref_d = get_xi_vel_ref_d(t, t_step, b, r_vrp, xi_eos) 2 | xi_vel_ref_d = zeros(3, size(t,2)); 3 | t_local_start = 0.0; 4 | for(i = 1:size(xi_vel_ref_d,2)) 5 | step_index = which_step_index(t(i), t_step); 6 | t_local_start = get_t_exp_start(step_index, t_step); 7 | xi_vel_ref_d(:, i) = get_xi_vel_d(t(i) - t_local_start, t_step(step_index), b, r_vrp(:, step_index), xi_eos(:, step_index)); 8 | end 9 | end 10 | -------------------------------------------------------------------------------- /which_step_index.m: -------------------------------------------------------------------------------- 1 | function step_index = which_step_index(t_input, t_step) 2 | % Get trajectory length and clamp t_input within the trajectory bounds 3 | t_trajectory_length = get_trajectory_length(t_step); 4 | t_input = max(min(t_input, t_trajectory_length), 0.0); 5 | 6 | t_step_start = 0.0; 7 | step_index = 1; 8 | for(i = 1:size(t_step,2)) 9 | t_step_end = t_step_start + t_step(i); 10 | if (t_step_start <= t_input) && (t_input < t_step_end) 11 | step_index = i; 12 | return 13 | end 14 | t_step_start = t_step_end; 15 | end 16 | 17 | step_index = size(t_step, 2); -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # dcm_matlab 2 | This matlab code implements the discontinuous and continuous double support DCM trajectories as described in the following paper 3 | ``` 4 | Englsberger, Johannes, Christian Ott, and Alin Albu-Schäffer. 5 | "Three-dimensional bipedal walking control based on divergent component of motion." 6 | IEEE Transactions on Robotics 31.2 (2015): 355-368. 7 | ``` 8 | 9 | In matlab, run the file dcm_test.m to visualize the DCM trajectories 10 | 11 | The dcm_test.m MATLAB script produces the following plots: 12 | ![DCM trajectories](https://raw.githubusercontent.com/stevenjj/dcm_matlab/master/dcm_trajectories.png) 13 | -------------------------------------------------------------------------------- /which_hybrid_step_index.m: -------------------------------------------------------------------------------- 1 | function step_index = which_hybrid_step_index(t_input, t_step, t_ds_vec, alpha) 2 | % Get trajectory length and clamp t_input within the trajectory bounds 3 | t_trajectory_length = get_trajectory_length(t_step); 4 | t_input = max(min(t_input, t_trajectory_length), 0.0); 5 | 6 | % Get which hybrid step index to use 7 | for(i = 1:size(t_step,2)) 8 | if (i < size(t_step,2)) 9 | t_ds_step_start = get_DS_start_time(i, t_step, t_ds_vec, alpha); 10 | t_step_end = get_DS_start_time(i+1, t_step, t_ds_vec, alpha); 11 | 12 | % Use a unique ending condition for the first step 13 | if (i == 1) 14 | t_step_end = get_DS_end_time(i, t_step, t_ds_vec, alpha); 15 | end 16 | 17 | if (t_ds_step_start <= t_input) && (t_input < t_step_end) 18 | step_index = i; 19 | return 20 | end 21 | else 22 | break 23 | end 24 | end 25 | 26 | step_index = size(t_step, 2); -------------------------------------------------------------------------------- /get_xi_ref_hybrid_d.m: -------------------------------------------------------------------------------- 1 | function xi_ref_hybrid_d = get_xi_ref_hybrid_d(t, t_step, b, r_vrp, xi_eos, t_ds_vec, alpha, P_mats) 2 | xi_ref_hybrid_d = zeros(3, size(t,2)); 3 | t_local_start = 0.0; 4 | for(i = 1:size(xi_ref_hybrid_d,2)) 5 | % Identify which step index to use 6 | step_index = which_hybrid_step_index(t(i), t_step, t_ds_vec, alpha); 7 | % Use Polynomial Interpolation 8 | if (t(i) <= get_DS_end_time(step_index, t_step, t_ds_vec, alpha)) 9 | t_local_start = t(i) - get_DS_start_time(step_index, t_step, t_ds_vec, alpha); 10 | xi_ref_hybrid_d(:, i) = get_xi_DS_poly(t_local_start, t_ds_vec(step_index), P_mats(:,:, step_index)); 11 | % Use exponential interpolation 12 | else 13 | t_local_start = t(i) - get_t_exp_start(step_index, t_step); 14 | xi_ref_hybrid_d(:, i) = get_xi_d(t_local_start, t_step(step_index), b, r_vrp(:, step_index), xi_eos(:, step_index)); 15 | end 16 | end 17 | end 18 | -------------------------------------------------------------------------------- /get_xi_vel_ref_hybrid_d.m: -------------------------------------------------------------------------------- 1 | function xi_vel_ref_hybrid_d = get_xi_vel_ref_hybrid_d(t, t_step, b, r_vrp, xi_eos, t_ds_vec, alpha, P_mats) 2 | xi_vel_ref_hybrid_d = zeros(3, size(t,2)); 3 | t_local_start = 0.0; 4 | for(i = 1:size(xi_vel_ref_hybrid_d,2)) 5 | % Identify which step index to use 6 | step_index = which_hybrid_step_index(t(i), t_step, t_ds_vec, alpha); 7 | % Use Polynomial Interpolation 8 | if (t(i) <= get_DS_end_time(step_index, t_step, t_ds_vec, alpha)) 9 | t_local_start = t(i) - get_DS_start_time(step_index, t_step, t_ds_vec, alpha); 10 | xi_vel_ref_hybrid_d(:, i) = get_xi_vel_DS_poly(t_local_start, t_ds_vec(step_index), P_mats(:,:, step_index)); 11 | % Use exponential interpolation 12 | else 13 | t_local_start = t(i) - get_t_exp_start(step_index, t_step); 14 | xi_vel_ref_hybrid_d(:, i) = get_xi_vel_d(t_local_start, t_step(step_index), b, r_vrp(:, step_index), xi_eos(:, step_index)); 15 | end 16 | end 17 | end 18 | -------------------------------------------------------------------------------- /dcm_test.asv: -------------------------------------------------------------------------------- 1 | clc; clear; 2 | % Set Walking Params 3 | g = 9.81; 4 | z = 0.75; 5 | b = sqrt(z/g); 6 | 7 | % Temporal Parameters 8 | % Commented temporal params are for the DRACO biped 9 | t_transfer = 0.5 %0.1; 10 | t_ss = 1.0; %0.3; 11 | t_ds = 0.25; %0.05; 12 | alpha = 0.5; 13 | t_ds_ini = alpha*t_ds; 14 | t_ds_end = (1-alpha)*t_ds; 15 | 16 | % Initialize foot conditions 17 | rfoot_y = -0.33; 18 | lfoot_y = 0.0; 19 | midfeet_y = (lfoot_y +rfoot_y)/2.0; 20 | 21 | % Initial dcm_state 22 | xi_init_state = [0.0; midfeet_y; z]; 23 | xi_vel_init_state = [0.0; 0.0; 0.0]; 24 | 25 | % Initialize DCM containers 26 | r_vrp = zeros(3,4); 27 | n = size(r_vrp, 2); 28 | t_step = zeros(1, n); 29 | xi_ini = zeros(3, n); 30 | xi_eos = zeros(3, n); 31 | 32 | % Initialize r_vrp 33 | r_vrp(:,1) = [0, midfeet_y, z]; 34 | r_vrp(:,2) = [0, lfoot_y, z]; 35 | r_vrp(:,3) = [1, rfoot_y, z]; 36 | r_vrp(:,4) = [1, midfeet_y, z]; 37 | % Initialize t_step 38 | t_step(:,1) = t_transfer + t_ds; 39 | t_step(:,2) = t_ss + t_ds; 40 | t_step(:,3) = t_ss + t_ds; 41 | t_step(:,4) = (1-alpha)*t_ds; 42 | 43 | % Polynomial Vectors 44 | xi_ini_DS = zeros(3, n); 45 | xi_ini_vel_DS = zeros(3, n); 46 | xi_end_DS = zeros(3, n); 47 | xi_end_vel_DS = zeros(3, n); 48 | t_ds_vec = ones(1,n)*t_ds; 49 | t_ds_vec(1) = t_transfer + t_ds + (1-alpha)*t_ds; 50 | P_mats = zeros(4,3,n); 51 | 52 | % Recursively find xi boundary conditions 53 | xi_eos(:,end) = r_vrp(:,end); 54 | for(j = 1:n) 55 | i = n-j+1; % Reverse counter 56 | xi_ini(:,i) = r_vrp(:,i) + exp( -(1/b)*t_step(i) ) * (xi_eos(:,i) - r_vrp(:,i)); 57 | if (i > 1) 58 | xi_eos(:,i-1) = xi_ini(:,i); 59 | end 60 | end 61 | 62 | % Define double support boundary conditions 63 | % Initial Double Support Conditions 64 | xi_ini_DS(:,1) = xi_init_state; 65 | xi_ini_vel_DS(:,1) = xi_vel_init_state; 66 | for(j = 1:n) 67 | i = n-j+1; % Reverse counter 68 | if (i > 1) 69 | xi_ini_DS(:,i) = r_vrp(:,i-1) + exp((-1/b)*t_ds_ini)*(xi_ini(:,i) - r_vrp(:,i-1)); 70 | xi_ini_vel_DS(:,i) = (1/b)*exp((-1/b)*t_ds_ini)*(xi_ini(:,i) - r_vrp(:,i-1)); 71 | end 72 | end 73 | 74 | % Ending Double Support conditions 75 | for(i = 1:n) 76 | xi_end_DS(:,i) = r_vrp(:,i) + exp((1/b)*t_ds_end)*(xi_ini(:,i) - r_vrp(:,i)); 77 | xi_end_vel_DS(:,i) = (1/b)*exp((1/b)*t_ds_end)*(xi_ini(:,i) - r_vrp(:,i)); 78 | end 79 | xi_end_DS(:,1) = xi_end_DS(:,2); 80 | xi_end_vel_DS(:,1) = xi_end_vel_DS(:,2); 81 | 82 | % Compute Polynomial Matrices 83 | for (i = 1:n) 84 | P_mats(:,:,i) = get_polynomial_matrix(t_ds_vec(i), xi_ini_DS(:,i), xi_ini_vel_DS(:,i), xi_end_DS(:,i), xi_end_vel_DS(:,i)); 85 | end 86 | 87 | % Get DCM references ------------------------------------------------------ 88 | t_additional = 0.5; 89 | t = [0:0.01:get_trajectory_length(t_step) + t_additional]; 90 | % Exponential DCM references 91 | xi_ref_d = get_xi_ref_d(t, t_step, b, r_vrp, xi_eos); 92 | xi_vel_ref_d = get_xi_vel_ref_d(t, t_step, b, r_vrp, xi_eos); 93 | 94 | % Exponential and Polynomial DCM references 95 | xi_ref_hybrid_d = get_xi_ref_hybrid_d(t, t_step, b, r_vrp, xi_eos, t_ds_vec, alpha, P_mats); 96 | xi_vel_ref_hybrid_d = get_xi_vel_ref_hybrid_d(t, t_step, b, r_vrp, xi_eos, t_ds_vec, alpha, P_mats); 97 | 98 | r_vrp_disc = compute_rvrp_given_dcm(b, xi_ref_d, xi_vel_ref_d); 99 | r_vrp_cont = compute_rvrp_given_dcm(b, xi_ref_hybrid_d, xi_vel_ref_hybrid_d); 100 | 101 | % Set index to plot 102 | figure(1) 103 | hold on 104 | plot(t, xi_ref_hybrid_d(1,:),'s-'); 105 | plot(t, xi_vel_ref_hybrid_d(1,:),'s-'); 106 | scatter(t, xi_ref_d(1,:)) 107 | scatter(t, xi_vel_ref_d(1,:)); 108 | t_start = 0.0; 109 | for(i = 1:size(t_step,2)) 110 | xline(t_start); 111 | t_start = t_start + t_step(i); 112 | end 113 | xlabel('time(s)') 114 | ylabel('dcm') 115 | legend({'DCM poly x','DCM poly x vel', 'DCM x','DCM x vel'},'Location','northeast') 116 | 117 | figure(2) 118 | hold on 119 | plot(t, xi_ref_hybrid_d(2,:),'s-'); 120 | plot(t, xi_vel_ref_hybrid_d(2,:)); 121 | scatter(t, xi_ref_d(2,:)) 122 | scatter(t, xi_vel_ref_d(2,:)); 123 | t_start = 0.0; 124 | for(i = 1:size(t_step,2)) 125 | xline(t_start); 126 | t_start = t_start + t_step(i); 127 | end 128 | xlabel('time(s)') 129 | ylabel('dcm') 130 | legend({'DCM poly y','DCM poly y vel', 'DCM y','DCM y vel'},'Location','southeast') 131 | 132 | figure(3) 133 | hold on 134 | plot(t, xi_ref_hybrid_d(3,:)); 135 | plot(t, xi_vel_ref_hybrid_d(3,:)); 136 | scatter(t, xi_ref_d(3,:)) 137 | scatter(t, xi_vel_ref_d(3,:)); 138 | t_start = 0.0; 139 | for(i = 1:size(t_step,2)) 140 | xline(t_start); 141 | t_start = t_start + t_step(i); 142 | end 143 | xlabel('time(s)') 144 | ylabel('dcm') 145 | legend({'DCM poly z','DCM poly z vel', 'DCM z','DCM z vel'},'Location','east') 146 | 147 | figure(4) 148 | hold on 149 | scatter(r_vrp_disc(1,:), r_vrp_disc(2,:)) 150 | plot(r_vrp_cont(1,:), r_vrp_cont(2,:)) 151 | plot(xi_ref_hybrid_d(1,:), xi_ref_hybrid_d(2,:), 's-') 152 | xlabel('x (m)') 153 | ylabel('y (m)') 154 | legend({'r ecmp xy disc','r ecmp xy cont', 'dcm cont'},'Location','northeast') 155 | 156 | figure(5) 157 | hold on 158 | scatter(t, r_vrp_disc(1,:)) 159 | plot(t, r_vrp_cont(1,:)) 160 | xlabel('time(s)') 161 | ylabel('r ecmp x (m)') 162 | legend({'discontinuous','continuous'},'Location','northeast') 163 | 164 | figure(6) 165 | hold on 166 | scatter(t, r_vrp_disc(2,:)) 167 | plot(t, r_vrp_cont(2,:)) 168 | xlabel('time(s)') 169 | ylabel('r ecmp y (m)') 170 | legend({'discontinuous','continuous'},'Location','northeast') 171 | 172 | 173 | %--- 174 | %figure(4) 175 | %index_to_try = n; 176 | %xi_vel_ds_poly_test = get_xi_vel_DS_poly(t, t_ds_vec(index_to_try), P_mats(:,:, index_to_try)); 177 | %plot(t, xi_vel_ds_poly_test(2,:)) 178 | -------------------------------------------------------------------------------- /dcm_test.m: -------------------------------------------------------------------------------- 1 | clc; clear; 2 | % Set Walking Params 3 | g = 9.81; 4 | z = 0.75; 5 | b = sqrt(z/g); 6 | 7 | % Temporal Parameters 8 | % Commented temporal params are for the DRACO biped 9 | t_transfer = 0.75; %0.1; 10 | t_ss = 1.0; %0.4; 11 | t_ds = 0.25; %0.05; 12 | alpha = 0.5; 13 | t_ds_ini = alpha*t_ds; 14 | t_ds_end = (1-alpha)*t_ds; 15 | 16 | % Set exponential settle time (so that when CoM is computed, it converges to 17 | % the final DCM state) 18 | percentage_settle = 0.99; 19 | t_settle = -b*log(1.0 - percentage_settle); 20 | 21 | % Initialize foot conditions 22 | rfoot_y = -0.33; 23 | lfoot_y = 0.0; 24 | midfeet_y = (lfoot_y +rfoot_y)/2.0; 25 | 26 | % Initial dcm_state 27 | xi_init_state = [0.0; midfeet_y; z]; 28 | xi_vel_init_state = [0.0; 0.0; 0.0]; 29 | 30 | % Initialize DCM containers 31 | r_vrp = zeros(3,4); 32 | n = size(r_vrp, 2); 33 | t_step = zeros(1, n); 34 | xi_ini = zeros(3, n); 35 | xi_eos = zeros(3, n); 36 | 37 | % Initialize r_vrp 38 | r_vrp(:,1) = [0, midfeet_y, z]; 39 | r_vrp(:,2) = [0, lfoot_y, z]; 40 | r_vrp(:,3) = [1, rfoot_y, z]; 41 | r_vrp(:,4) = [1, midfeet_y, z]; 42 | % Initialize t_step 43 | t_step(:,1) = t_transfer + t_ds; 44 | t_step(:,2) = t_ss + t_ds; 45 | t_step(:,3) = t_ss + t_ds; 46 | t_step(:,4) = (1-alpha)*t_ds; 47 | 48 | % Polynomial Vectors 49 | xi_ini_DS = zeros(3, n); 50 | xi_ini_vel_DS = zeros(3, n); 51 | xi_end_DS = zeros(3, n); 52 | xi_end_vel_DS = zeros(3, n); 53 | t_ds_vec = ones(1,n)*t_ds; 54 | t_ds_vec(1) = t_transfer + t_ds + (1-alpha)*t_ds; 55 | P_mats = zeros(4,3,n); 56 | 57 | % Recursively find xi boundary conditions 58 | xi_eos(:,end) = r_vrp(:,end); 59 | for(j = 1:n) 60 | i = n-j+1; % Reverse counter 61 | xi_ini(:,i) = r_vrp(:,i) + exp( -(1/b)*t_step(i) ) * (xi_eos(:,i) - r_vrp(:,i)); 62 | if (i > 1) 63 | xi_eos(:,i-1) = xi_ini(:,i); 64 | end 65 | end 66 | 67 | % Define double support boundary conditions 68 | % Initial Double Support Conditions 69 | xi_ini_DS(:,1) = xi_init_state; 70 | xi_ini_vel_DS(:,1) = xi_vel_init_state; 71 | for(j = 1:n) 72 | i = n-j+1; % Reverse counter 73 | if (i > 1) 74 | xi_ini_DS(:,i) = r_vrp(:,i-1) + exp((-1/b)*t_ds_ini)*(xi_ini(:,i) - r_vrp(:,i-1)); 75 | xi_ini_vel_DS(:,i) = (1/b)*exp((-1/b)*t_ds_ini)*(xi_ini(:,i) - r_vrp(:,i-1)); 76 | end 77 | end 78 | 79 | % Ending Double Support conditions 80 | for(i = 1:n) 81 | xi_end_DS(:,i) = r_vrp(:,i) + exp((1/b)*t_ds_end)*(xi_ini(:,i) - r_vrp(:,i)); 82 | xi_end_vel_DS(:,i) = (1/b)*exp((1/b)*t_ds_end)*(xi_ini(:,i) - r_vrp(:,i)); 83 | end 84 | xi_end_DS(:,1) = xi_end_DS(:,2); 85 | xi_end_vel_DS(:,1) = xi_end_vel_DS(:,2); 86 | 87 | % Compute Polynomial Matrices 88 | for (i = 1:n) 89 | P_mats(:,:,i) = get_polynomial_matrix(t_ds_vec(i), xi_ini_DS(:,i), xi_ini_vel_DS(:,i), xi_end_DS(:,i), xi_end_vel_DS(:,i)); 90 | end 91 | 92 | % Get DCM references ------------------------------------------------------ 93 | t = [0:0.01:get_trajectory_length(t_step) + t_settle]; 94 | % Exponential DCM references 95 | xi_ref_d = get_xi_ref_d(t, t_step, b, r_vrp, xi_eos); 96 | xi_vel_ref_d = get_xi_vel_ref_d(t, t_step, b, r_vrp, xi_eos); 97 | 98 | % Exponential and Polynomial DCM references 99 | xi_ref_hybrid_d = get_xi_ref_hybrid_d(t, t_step, b, r_vrp, xi_eos, t_ds_vec, alpha, P_mats); 100 | xi_vel_ref_hybrid_d = get_xi_vel_ref_hybrid_d(t, t_step, b, r_vrp, xi_eos, t_ds_vec, alpha, P_mats); 101 | 102 | r_vrp_disc = compute_rvrp_given_dcm(b, xi_ref_d, xi_vel_ref_d); 103 | r_vrp_cont = compute_rvrp_given_dcm(b, xi_ref_hybrid_d, xi_vel_ref_hybrid_d); 104 | 105 | % Set index to plot 106 | figure(1) 107 | subplot(2,3,1) 108 | hold on 109 | plot(t, xi_ref_hybrid_d(1,:),'s-'); 110 | plot(t, xi_vel_ref_hybrid_d(1,:),'s-'); 111 | scatter(t, xi_ref_d(1,:)) 112 | scatter(t, xi_vel_ref_d(1,:)); 113 | t_start = 0.0; 114 | for(i = 1:size(t_step,2)) 115 | xline(t_start); 116 | t_start = t_start + t_step(i); 117 | end 118 | xlabel('time(s)') 119 | ylabel('dcm') 120 | legend({'DCM poly x','DCM poly x vel', 'DCM x','DCM x vel'},'Location','northeast') 121 | 122 | %figure(2) 123 | subplot(2,3,2) 124 | hold on 125 | plot(t, xi_ref_hybrid_d(2,:),'s-'); 126 | plot(t, xi_vel_ref_hybrid_d(2,:),'s-'); 127 | scatter(t, xi_ref_d(2,:)) 128 | scatter(t, xi_vel_ref_d(2,:)); 129 | t_start = 0.0; 130 | for(i = 1:size(t_step,2)) 131 | xline(t_start); 132 | t_start = t_start + t_step(i); 133 | end 134 | xlabel('time(s)') 135 | ylabel('dcm') 136 | legend({'DCM poly y','DCM poly y vel', 'DCM y','DCM y vel'},'Location','southeast') 137 | 138 | %figure(3) 139 | subplot(2,3,3) 140 | hold on 141 | plot(t, xi_ref_hybrid_d(3,:),'s-'); 142 | plot(t, xi_vel_ref_hybrid_d(3,:),'s-'); 143 | scatter(t, xi_ref_d(3,:)) 144 | scatter(t, xi_vel_ref_d(3,:)); 145 | t_start = 0.0; 146 | for(i = 1:size(t_step,2)) 147 | xline(t_start); 148 | t_start = t_start + t_step(i); 149 | end 150 | xlabel('time(s)') 151 | ylabel('dcm') 152 | legend({'DCM poly z','DCM poly z vel', 'DCM z','DCM z vel'},'Location','east') 153 | 154 | %figure(4) 155 | subplot(2,3,4) 156 | hold on 157 | scatter(r_vrp_disc(1,:), r_vrp_disc(2,:)) 158 | plot(r_vrp_cont(1,:), r_vrp_cont(2,:)) 159 | plot(xi_ref_hybrid_d(1,:), xi_ref_hybrid_d(2,:), 's-') 160 | xlabel('x (m)') 161 | ylabel('y (m)') 162 | legend({'r ecmp xy disc','r ecmp xy cont', 'dcm cont'},'Location','northeast') 163 | 164 | %figure(5) 165 | subplot(2,3,5) 166 | hold on 167 | scatter(t, r_vrp_disc(1,:)) 168 | plot(t, r_vrp_cont(1,:)) 169 | xlabel('time(s)') 170 | ylabel('r ecmp x (m)') 171 | legend({'discontinuous','continuous'},'Location','northeast') 172 | 173 | %figure(6) 174 | subplot(2,3,6) 175 | hold on 176 | scatter(t, r_vrp_disc(2,:)) 177 | plot(t, r_vrp_cont(2,:)) 178 | xlabel('time(s)') 179 | ylabel('r ecmp y (m)') 180 | legend({'discontinuous','continuous'},'Location','northeast') 181 | 182 | 183 | %--- 184 | %figure(4) 185 | %index_to_try = n; 186 | %xi_vel_ds_poly_test = get_xi_vel_DS_poly(t, t_ds_vec(index_to_try), P_mats(:,:, index_to_try)); 187 | %plot(t, xi_vel_ds_poly_test(2,:)) 188 | --------------------------------------------------------------------------------