![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1756054/image.png)
How to apply orientation constraints between two rigid bodies
59 views (last 30 days)
Show older comments
Aiden Morcombe
on 19 Aug 2024 at 19:48
Commented: Aiden Morcombe
on 22 Aug 2024 at 20:46
I am trying to do an inverse kinematics analysis on a rigid body tree that has 3 rigid bodies, all with revolute joints, operating in a 2D plane. One of the contraints of the system that this is based off is that the first and third body must be parallel. How would I apply a constraint to ensure that this happens? I am attempting to mimic the physical motion of the model below, where the base frame is the hole near the top of the 3D model.
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1755794/image.jpeg)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1755799/image.jpeg)
0 Comments
Accepted Answer
Sahas
on 20 Aug 2024 at 8:35
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);
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1756054/image.png)
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 at 6:52
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
Find more on Passivity and Sector Bounds in Help Center and File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!