![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1756054/image.png)
How to apply orientation constraints between two rigid bodies
58 visualizaciones (últimos 30 días)
Mostrar comentarios más antiguos
Aiden Morcombe
el 19 de Ag. de 2024 a las 19:48
Comentada: Aiden Morcombe
el 22 de Ag. de 2024 a las 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 comentarios
Respuesta aceptada
Sahas
el 20 de Ag. de 2024 a las 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!
Más respuestas (1)
David Goodmanson
el 20 de Ag. de 2024 a las 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 comentarios
Ver también
Categorías
Más información sobre Passivity and Sector Bounds en Help Center y File Exchange.
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!