Hi Bhagya,
I understand that you want the spline to follow the trajectory of a polygon and pass around the waypoints. The code you provided uses a simple spline interpolation which doesn’t consider the polygon trajectory. To achieve this, you can try modifying the way point generation and spline interpolation process. Here’s a rough idea on how you can approach this:
1) Polygon Trajectory: Define your polygon trajectory. This could be a set of points that define the vertices of the polygon.
2) Waypoint Generation: Generate waypoints along this polygon trajectory. One way to do this is to interpolate points along the edges of the polygon.
3) Spline Interpolation: Now, instead of directly interpolating between your waypoints, you would interpolate between these points along the polygon edges. This will ensure that your spline follows the polygon trajectory.
4) Waypoint Navigation: When navigating, if your current waypoint is close enough (within some defined tolerance), you move on to the next waypoint.
Try running this sample code to get more idea:
clc;
clear;
close all;
% Define the polygon vertices as a set of (x, y) coordinates
polygonVertices = [
1, 1;
2, 3;
4, 4;
6, 2;
4.5,-0.5;
3, 0;
1, 1;
];
% Generate waypoints along the polygon edges
numWaypoints = size(polygonVertices, 1) * 10; % Increase for smoother path
waypoints = interp1(1:size(polygonVertices, 1), polygonVertices, linspace(1, size(polygonVertices, 1), numWaypoints), 'linear');
% Initialize the robot's current position
currentPosition = waypoints(1, :);
% Define a tolerance radius for reaching waypoints
toleranceRadius = 0.01;
% Create a figure for plotting the robot's movement
figure;
hold on;
title('Polygon-Based Waypoint Navigation');
xlabel('X-East');
ylabel('Y-North');
axis([0 8 -2 5]);
grid on;
% Plot the polygon vertices
plot(polygonVertices(:, 1), polygonVertices(:, 2), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r');
% Loop through each waypoint
for i = 1:numWaypoints
waypoint = waypoints(i, :);
% Display the current waypoint
fprintf('Navigating to waypoint (%.2f, %.2f)...\n', waypoint(1), waypoint(2));
% Perform waypoint navigation
while norm(currentPosition - waypoint) > toleranceRadius
% Calculate the desired heading (angle) towards the waypoint
desiredHeading = atan2(waypoint(2) - currentPosition(2), waypoint(1) - currentPosition(1));
% Implement control to move the robot towards the desired heading
stepSize = 0.02; % Adjust this value based on your robot's speed
currentPosition = currentPosition + stepSize * [cos(desiredHeading), sin(desiredHeading)];
% Update the robot's position on the plot
plot(currentPosition(1), currentPosition(2), 'bo', 'MarkerSize', 5, 'MarkerFaceColor', 'b');
% Add a delay to simulate real-time movement (adjust as needed)
pause(0.01);
end
% Display a message when the waypoint is reached
fprintf('Waypoint (%.2f, %.2f) reached!\n', waypoint(1), waypoint(2));
end
fprintf('Navigation complete!\n');
% Remove hold on the plot to allow further interaction
hold off;
Hope this helps!
