In the given code, waypoints is a 3×3 matrix. The error comes in line 8.
persistent waypoints0 traj_time d0 if nargin > 2 d = waypoints(:,2:end) - waypoints(:,1:end-1); d0 = 2 * sqrt(d(1,:).^2 + d(2,:).^2 + d(3,:).^2); traj_time = [0, cumsum(d0)]; waypoints0 = waypoints; else if t >= traj_time(end) t = traj_time(end); end t_index = find(traj_time > t,1) - 1; if (t_index == 0) t_index = 1; end if(t == 0) desired_state.pos = waypoints0(:,1); else scale = (t-traj_time(t_index)) / d0(t_index); index = [(t_index-1)*8+1:t_index*8]; end end
Best Answer