function Optimize_robot_arm % Solve the robotic arm problem using fmincon. It visualizes the position % of the robotic arm at each step of the minimization problem % Define the lengths of the robot arm segments l1 = 1; % Length of the first arm segment l2 = 1; % Length of the second arm segment l3 = 1; % Length of the third arm segment % Define the desired position of the end effector desiredPosition = [1; 2]; % Objective function to minimize (distance from the desired position) objectiveFunction = @(theta) norm([... l1*cos(theta(1)) + l2*cos(theta(1) + theta(2)) + l3*cos(theta(1) + theta(2) + theta(3)); l1*sin(theta(1)) + l2*sin(theta(1) + theta(2)) + l3*sin(theta(1) + theta(2) + theta(3)) ] - desiredPosition, 2); % Initial guess for the joint angles initialGuess = [0; 0; 0]; % Set up optimization options options = optimoptions('fmincon','Display', 'iter', 'Algorithm', 'sqp', ... 'OutputFcn', @outfun); % Prepare the figure hFig = figure; clf; hAx = axes('Parent', hFig); title(hAx, 'Robot Arm Optimization Process'); xlabel(hAx, 'X'); ylabel(hAx, 'Y'); hold(hAx, 'on'); grid(hAx, 'on'); axis(hAx, 'equal'); % Set fixed axes limits axisLimits = [-3, 3, -3, 3]; xlim(hAx, axisLimits(1:2)); ylim(hAx, axisLimits(3:4)); % Plot the desired position as a green square plot(hAx, desiredPosition(1), desiredPosition(2), 'gs', 'MarkerSize', 10, 'MarkerFaceColor', 'g'); % Call fmincon to minimize the objective [thetaSolution, fval, exitflag, output] = fmincon(... objectiveFunction, ... initialGuess, ... [], [], ... % Linear inequality constraints [], [], ... % Linear equality constraints [], [], ... % Bounds [], ... % Nonlinear constraints options); % Display the solution disp('Joint angles solution (in radians):'); disp(thetaSolution); function stop = outfun(theta, optimValues, state) stop = false; switch state case 'iter' % Update the plot with the current robot arm position plot_robot_arm(hAx, l1, l2, l3, theta, axisLimits,desiredPosition); drawnow; pause(0.5); % Pause for half a second end end end function plot_robot_arm(ax, l1, l2, l3, theta, axisLimits,desiredPosition) % Calculate joint positions p0 = [0; 0]; p1 = p0 + l1 * [cos(theta(1)); sin(theta(1))]; p2 = p1 + l2 * [cos(theta(1) + theta(2)); sin(theta(1) + theta(2))]; p3 = p2 + l3 * [cos(theta(1) + theta(2) + theta(3)); sin(theta(1) + theta(2) + theta(3))]; % Clear existing robot arm plot cla(ax); % Plot the robot arm plot(ax, [p0(1), p1(1), p2(1), p3(1)], [p0(2), p1(2), p2(2), p3(2)], ... 'bo-', 'MarkerFaceColor', 'b'); % Re-plot the desired position as a green square plot(ax, desiredPosition(1), desiredPosition(2), 'gs', 'MarkerSize', 10, 'MarkerFaceColor', 'g'); % Set the axes limits to the specified values xlim(ax, axisLimits(1:2)); ylim(ax, axisLimits(3:4)); end