# How to apply orientation constraints between two rigid bodies

6 views (last 30 days)

Show older comments

##### 0 Comments

### Accepted Answer

Sahas
on 20 Aug 2024

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!

### More Answers (1)

David Goodmanson
on 20 Aug 2024

Hi Aiden,

because of the parallelism there are only two degrees of freedom here (assuming that one pivot point is the hole near the top that you mentioned). Referring to the attachment, the eqns for point E are

Ex = [OB] cos(th1) + [BD] cos(th1+th2) + [DE] cos(th1)

Ey = [OB] sin(th1) + [BD] sin(th1+th2) + [DE] sin(th1)

which is the same as

Ex = ([OB]+[DE])cos(th1) + [BD]cos(th1+th2)

Ey = ([OB]+[DE])sin(th1) + [BD]sin(th1+th2)

which is the lower diagram with

([OB]+[DE]) = L1 [BD] = L2

so you really just have a standard problem with two bars.

##### 0 Comments

### See Also

### Categories

### Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!