力螺旋轨迹

% Parameters
r_max = 1; % Maximum radius of the spiral
omega = 2 * pi; % Angular velocity (rad/s)
t_end = 10; % End time (s)
dt = 0.001; % Time step (s)
F_rz0 = 20; % Constant force in the z direction

% Time vector
t = 0:dt:t_end;

% Spiral trajectory equations
r = linspace(0, r_max, length(t)); % Radius increases linearly over time
f_rx = -r .* omega .* sin(omega .* t);
f_ry = r .* omega .* cos(omega .* t);
f_rz = F_rz0 * ones(size(t)); % Constant value along z-axis

% Plotting the force spiral
figure;
plot3(f_rx, f_ry, f_rz, 'b-', 'LineWidth', 1.5);
grid on;
xlabel('f_r_x (N)');
ylabel('f_r_y (N)');
zlabel('f_r_z (N)');
zlim([0 30])
title('Force Spiral Trajectory');
axis equal;

% Optional: Add markers to indicate direction
hold on;
scatter3(f_rx(1), f_ry(1), f_rz(1), 'ro', 'filled'); % Start point
scatter3(f_rx(end), f_ry(end), f_rz(end), 'go', 'filled'); % End point
legend('Spiral Trajectory', 'Start', 'End');



%%
% Parameters
r_max = 1; % Maximum radius of the spiral
omega = 2 * pi; % Angular velocity (rad/s)
t_end = 10; % End time (s)
dt = 0.001; % Time step (s)
z_increment = 0.5; % Increment of z per second

% Time vector
t = 0:dt:t_end;

% Spiral trajectory equations
r = linspace(0, r_max, length(t)); % Radius increases linearly over time
f_rx = -r .* omega .* sin(omega .* t);
f_ry = r .* omega .* cos(omega .* t);
f_rz = z_increment * t; % z increases linearly over time

% Plotting the force spiral
figure;
plot3(f_rx, f_ry, f_rz, 'b-', 'LineWidth', 1.5);
grid on;
xlabel('X (N)');
ylabel('Y (N)');
zlabel('Z (N)');
title('Spiral Trajectory');
axis equal;