How to apply orientation constraints between two rigid bodies

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.

Answer 1

[Sahas]

Hi @Aiden Morcombe,

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:

https://www.mathworks.com/help/optim/ug/fmincon

https://in.mathworks.com/help/optim/ug/fmincon/nonlcon

I hope this is beneficial!

[Aiden Morcombe]

Thank you Sahas, this is exactly what I was looking for!

Answer 2

[David Goodmanson]

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.