As per my understanding, you are doing inverse kinematics analysis on the three rigid bodies with revolute joints and would like to add a constraint to keep the first and third body always parallel to each other.
I took some assumptions like knowing the final position of the “end-effector”, included “forward kinematics” and implemented the required constraints in the code below.
To ensure body1 and body3 are in parallel, their orientation angles must be equal when measure from the same reference. Thus, equate the orientation angles of body3 and body1.
function [jointAngles] = solveInverseKinematics(initialCoordinates, targetPosition, L1, L2, L3)
%Objective function for minimizing the error
function f = objectiveFunction(angles)
theta1 = angles(1);
theta2 = angles(2);
theta3 = angles(3);
[x, y] = forwardKinematics(theta1, theta2, theta3, L1, L2, L3);
f = norm([targetPosition(1) - x, targetPosition(2) - y]);
end
%Nonlinear constraint for enforcing constraint: theta3 = theta1
%To know more about this function, refer to the documentation links provided
function [c, ceq] = nonlinearConstraints(angles)
c = []; %No inequality constraints
ceq = angles(1) - angles(3); %For theta1 - theta3 ~ 0
end
%Pre-req steps for using "fmincon" function
options = optimoptions('fmincon', 'Display', 'iter', 'Algorithm', 'sqp');
%Solving the optimization problem
jointAngles = fmincon(@objectiveFunction, initialCoordinates, [], [], [], [], [], [], ...
@nonlinearConstraints, options);
end
function [x, y] = forwardKinematics(theta1, theta2, theta3, L1, L2, L3)
%Calculate positions of links
x = L1 * cos(theta1) + L2 * cos(theta1 + theta2) + L3 * cos(theta3);
y = L1 * sin(theta1) + L2 * sin(theta1 + theta2) + L3 * sin(theta3);
end
%Plot function
function plotRigidBodySystem(jointAngles, L1, L2, L3)
%Angles
theta1 = jointAngles(1);
theta2 = jointAngles(2);
theta3 = jointAngles(3);
%Calculate joint positions in cartesian system
x0 = 0; y0 = 0;
x1 = L1 * cos(theta1);
y1 = L1 * sin(theta1);
x2 = x1 + L2 * cos(theta1 + theta2);
y2 = y1 + L2 * sin(theta1 + theta2);
x3 = x2 + L3 * cos(theta3);
y3 = y2 + L3 * sin(theta3);
%Plot links
figure;
hold on;
plot([x0, x1], [y0, y1], 'r-o', 'LineWidth', 2);
plot([x1, x2], [y1, y2], 'g-o', 'LineWidth', 2);
plot([x2, x3], [y2, y3], 'b-o', 'LineWidth', 2);
axis equal;
xlim([-sum([L1, L2, L3]), sum([L1, L2, L3])]);
ylim([-sum([L1, L2, L3]), sum([L1, L2, L3])]);
xlabel('X');
ylabel('Y');
title('2D Rigid Body System with Parallel Constraint');
grid on;
hold off;
end
%Main function
initialCoordinates = [0, 0, 0];
targetPosition = [1, 1.5]; %Final position of the end effector
%Lengths of the links
L1 = 1;
L2 = 0.5;
L3 = 1;
%Inverse Kinematics Analysis
jointAngles = solveInverseKinematics(initialCoordinates, targetPosition, L1, L2, L3);
disp('Solved Joint Angles:');
disp(jointAngles);
%Plotting
plotRigidBodySystem(jointAngles, L1, L2, L3);
You can see that the first and third bodies are parallel for the set “target positions”.
I used the “fmincon” function here to solve the constrained non-linear optimization problem. Alternatively, you can also use MATLAB’s “fsolve” function to solve the system on non-linear equations.
To know more about the usage of “fmincon” refer to the following MathWorks documentation links:
I hope this is beneficial!
