function generate_spiral_traj(r, x_origin, y_origin, z_from, z_to, Ts, ...
completion_time, f)
% Generate spiral trajectory with center at [x_origin y_origin] with radius
% r, spiralling from z_from to z_to.
% The trajectory is generated with sampling period Ts and with duration of
% completion_time.
t = 0:Ts:completion_time;
r_space = linspace (0, 1, numel (t));
z = linspace (z_from, z_to, numel (t));
omega = 2*pi*f;
output = zeros(length(r_space), 3);
for ii=1:length(output)
output(ii, :) = [r*sin(omega*t(ii))+x_origin r*cos(omega*t(ii))+y_origin z(ii)];
end
plot3(output(:,1), output(:,2), output(:,3));
dlmwrite('traj_spiral.dat', output, '\t');
调用方法
generate_spiral_traj(0.025, 0,0,0.25, 0.4, 0.001, 5*pi)