Main Content

Solve Inverse Kinematics for Closed Loop Linkages

Closed loop linkages are widely used in automobiles, construction and manufacturing machines, and in robot manipulation. Although you cannot directly model closed-loop linkages with the rigidBodyTreeImportInfo object in Robotics System Toolbox™, you can still study the kinematics of closed-loop systems by combining a rigid body tree with constraints that mimic loop-closing joints. The constraintRevoluteJoint, constraintPrismaticJoint, and constraintFixedJoint constraint objects enable you to model loop-closing revolute, prismatic, and fixed joints, respectively. To kinematically model closed-loop linkages, use the constraints with a generalizedInverseKinematics solver to constrain the solutions to act as desired.

This example shows how to model a four-bar linkage, a widely used closed-loop linkage, using the rigidBodyTree and constraintRevoluteJoint objects, and the generalizedInverseKinematics System object™.

Four-Bar Linkage as Rigid Body Tree with Revolute Joint Constraint

This figure shows a four-bar linkage. The highlighted joint j4 is a loop-closing revolute joint.

Annotated four-bar linkage

You can create a rigid body tree corresponding to the four-bar linkage, which consists of joints j1, j2, and j3, and is constrained with a revolute joint constraint that acts as a fourth joint, j4. This figure shows the reference frames of the links of the rigid body tree in an unconstrained joint configuration. The frames of the links are located at their respective joints. The frame corresponding to the base body of the rigid body tree, link0, is represented by the black arrows.

Four bar linkage with disconnected third link showing the revolute joint constraint

Visualize the rigid body tree in an unconstrained configuration.

fourBarLinkageTree =  exampleHelperFourBarLinkageTree();
figure(Name="Rigid Body Tree Unconstrained As Four-Bar Linkage",Visible="on")
unconstrainedConfig = [pi/4 -pi/4 -110*pi/180];
ax = show(fourBarLinkageTree, ...
    unconstrainedConfig, ...
    Collisions="on", ...
    Frames="on", ...
    PreservePlot=true, ...
    FastUpdate=false);
title("Rigid Body Tree Unconstrained as Four-Bar Linkage")
view([0 0 pi/2])

Connectivity Graph of Four-Bar Linkage

One way to approach the modeling of closed-loop linkages is by using connectivity graphs. This approach models the kinematic linkage using a graph whose vertices are the links of the linkage, and an edge between links represents the joint between them.

Using this representation, the goal of modeling the closed-loop linkage as a rigid body tree with joint constraints is to remove edges from the graph so that the graph becomes a tree. A tree is a graph with no loops, which instead uses constraints to enforce the removed edges.

In the connectivity graph, the difference between the four-bar linkage and its equivalent rigid body tree representation is that the rigid body tree needs a loop-closing joint constraint from link0 to link3.

Linkage relationships as diagrams

Set Up Inverse Kinematics Solver

The four-bar linkage is a one degree-of-freedom linkage, and is driven by the crank link, link1. Given a crank position specified by the value of joint j1, the generalized inverse kinematics solver outputs the joint positions corresponding to joints j2 and j3.

Create an instance of the generalized inverse kinematics solver. For this task, the solver requires two constraint objects, constraintJointBounds and constraintRevoluteJoint.

solverFourBarLinkage = generalizedInverseKinematics(...
    RigidBodyTree=fourBarLinkageTree, ...
    ConstraintInputs={'jointbounds','revolutejoint'});

% Disable random restarts for repeatable IK solutions
solverFourBarLinkage.SolverParameters.AllowRandomRestart = false;
solverFourBarLinkage.SolverParameters.StepTolerance = 1e-14;

Define the revolute joint constraint between link3 and link0.

% Revolute joint constraint between "link3" and "link0"
cRevolute = constraintRevoluteJoint("link3","link0");

To ensure that the crank position is fixed with respect to the solution, you must implement a joint bounds constraint. The elements of the Weights property of the joint bounds constraint corresponds to a joint position in the joint configuration of the rigid body tree. In this case, only joint j1 has a non-zero weight, as it is the joint that we want to constrain.

% Joint bounds constraint to fix the crank position.
activeJointConstraint = constraintJointBounds(fourBarLinkageTree);
activeJointConstraint.Weights = [1 0 0];

Intermediate Frames of Constraints

Revolute joint constraints constrain two intermediate frames. Each frame corresponds to the predecessor and the successor body. This figure below shows which intermediate frames to constrain to mimic the loop-closing joint.

Showing predecessor and successor intermediate frames

In the unconstrained configuration, the predecessor intermediate frame is located at along the x-axis of the base (link0) frame at [1 0 0], shown as the black arrows. Similarly, the successor intermediate frame is located along the x-axis of the link3 frame at [1 0 0].

cRevolute.PredecessorTransform = trvec2tform([1 0 0]);
cRevolute.SuccessorTransform = trvec2tform([1 0 0]);
vis = exampleHelperFrameVisual(fourBarLinkageTree,cRevolute,ax);
vis.update(unconstrainedConfig);
view([0 0 pi/2])

If you constrain the intermediate frames, they co-locate. This figure shows a constrained configuration that corresponds to the unconstrained configuration.

Four-bar linkage with reference frames attached

Visualize Constrained Motion

The joint position, theta, of the crank drives the linkage. For a discrete set of crank positions in the interval[π2,2π+π2], the solver solves for joint positions in the rigid body tree such that it behaves like a four-bar linkage. This code visualizes the constrained rigid body tree for every crank position, theta.

For the initial crank position, π2, guess an initial constrained configuration for the solver. The visualization loop overwrites this value.

constrainedConfig = [pi/2 0 pi/3];

Initialize the figure for visualization.

figure(Name="Rigid Body Tree Constrained As Four-Bar Linkage",Visible="on")
ax = show(fourBarLinkageTree,constrainedConfig, ...
    PreservePlot=false, ...
    FastUpdate=true, ...
    Collisions="on");
title("Rigid Body Tree Constrained as Four-Bar Linkage")
vis = exampleHelperFrameVisual(fourBarLinkageTree,cRevolute,ax);

Visualize the four-bar linkage for a specified crank position.

for theta = linspace(pi/2,5*pi/2,101)
    % Fix the crank position to "theta" in the solver-generated constrained
    % config. This is done by setting identical upper and lower values for
    % the joint bounds constraint.
    config = [theta constrainedConfig(2) constrainedConfig(3)];
    activeJointConstraint.Bounds(1,:) = [theta theta];
    constrainedConfig = solverFourBarLinkage(config,activeJointConstraint,cRevolute);
    
    % Visualize the constrained rigid body tree and the
    % predecessor and successor intermediate frames of the constraint.
    show(fourBarLinkageTree,constrainedConfig, ...
        PreservePlot=false, ...
        FastUpdate=true, ...
        Collisions="on");
    vis.update(constrainedConfig);
    view([0 0 pi/2]);
    drawnow;
end