5R-5 Bar, Closed Loop, Linkage

A relatively simple mechanism, with the very useful feature of sharing the load between the two powered joints: ‘A’ and ‘E’.

It may look like two 2R-2 Bar linkages back-to-back, but it works in a completely different way. Though the math is very similar.

Overview

‘A’ through ‘E’ are revolute joints, hence the 5R part of the name, but only the bottom two ‘A’ and ‘E’ are powered. Joints ‘B’, ‘C’, and ‘D’ are all free floating, whose location and angles are determined purely by the positioning of the links attached to ‘A’ and ‘E’. The linkages, labled ‘L₁‘ through ‘L₅‘ connect the joints together in a closed loop (which you may have guessed from the name). Link ‘L₅‘ is special in as much as it is grounded and doesn’t move. It connects together joints ‘A’ and ‘E’ which are the two driven joints whose locations also do not change. The origin for the coordinate system understood by the mechanism can be set anywhere, however it is usually chosen to make the centre of ‘L₅‘ be the origin of the mechanism’s coordinate system.

Due to the fact that linkages ‘L₂‘ and ‘L₃‘ being connected together via joint ‘C’ (the end effector), as ‘the powered ‘L₁‘ and ‘L₄‘ effectively squash joint ‘C’, via ‘L₂‘ and ‘L₃‘, into position. It’s a simple mechanism, but it works rather well as long as the elbows are not allowed to invert, which can cause a lockup under certain conditions.

Just like it’s sibling, the 2R-2 Bar linkage, the math for computing the inverse and forward kinematics is relatively straight forward as long as you are comfortable with both Pythagoras and the law of cosines.

Inverse Kinematics

This is the working backwards from where we want to be, to how we are going to get there. It is likely to be the first problem that is needed to be solved to get a working robot arm/leg.

The solution is remarkably similar to that for the 2R-2 Bar mechanism. In fact we can solve the left and right linkage sets separately, but identically. First reactions may be to assume that means twice as many calculations as the 2R sibling. Surprisingly, perhaps, there are not significantly more calculations needed as the knee/elbow joint angles and positions are not required to be known.

Just like before, we connect imaginary lines between ‘C’ and ‘A’, and ‘C’ and ‘E’. We then drop a perpendicular line down to the ground from ‘C’. The result looks something like this:

and the math that goes with that diagram looks something like this (it’s the same for both sides):

\[ \delta_y = C_y – {anchor}_y \]

\[ \delta_x = {anchor}_x – C_x \]

\[ h^2 = \delta_y^2 + \delta_x^2 \]

\[ \alpha = \arctan\left(\frac{\delta_y}{\delta_x}\right) \]

\[ \beta = \arccos\left(\frac{femur^2 + h^2 – tibia^2}{2*femur*h} \right) \]

\[ \theta = \alpha + \beta \]

struct AnchorAngles {
    float a;
    float e;
};

struct Point2D {
    float x;
    float y;
};

// Links (bones)
const float L1 = 40.0f; // Femur (or Humerus) depending on use
const float L2 = 60.0f; // Tibia (or Radius)depending upon use
const float L3 = 60.0f; // Tibia (or Radius) depending upon use
const float L4 = 40.0f; // Femur (or Humerus) depending on use
const float L5 = 30.0f; // Fixed hip (or shoulders) depending on use

// Driving joint coordinates - Origin at center of L5
const Point2D A { -(L5 / 2.0f), 0.0f }; // Left of origin
const Point2D E { (L5 / 2.0f), 0.0f };  // Right of origin

/** @return angle in degrees (measured anti-clockwise from base) */
float calcInvKinematicsHalf(Point2D anchor, Point2D target, float femur, float tibia, bool leftSide) {
    float dy = (target.y - anchor.y);
    float dx = (target.x - anchor.x);
    float distSqrd = dy * dy + dx * dx;
    float dist = std::sqrtf(distSqrd);
    float femurSqrd = femur * femur; // C/C++ has no squared operator
    float tibiaSqrd = tibia * tibia;

    if (dist > (femur + tibia) || dist < std::fabsf(femur - tibia)) {
        return 0.0f; // Target unreachable
    }

    // Use atan2 because it takes into account the quadrant of angle
    float alpha = std::atan2(dy, dx); // Radians
    float cos_beta = (femurSqrd + distSqrd - tibiaSqrd) / (2.0f * femur * dist); // Radians
    float beta = std::acos(std::max(-1.0f, std::min(1.0f, cos_beta))); // Radians
    float theta = leftSide ? (alpha + beta) : (alpha - beta); // Radians

    return theta * 180.0f / M_PIf; // Return as degrees
}

AnchorAngles calcInvKinematics5r(Point2D target) {
    // Calc for left half (prefer elbow to left)
    float a_angle = calcInvKinematicsHalf(A, target, L1, L2, true);

    // Calc for right half (prefer elbow to right)
    float e_angle = calcInvKinematicsHalf(E, target, L4, L3, false);

    return AnchorAngles{ a_angle, e_angle };
}

from math import acos, atan2, sqrt, pi
from dataclasses import dataclass

@dataclass
class Point2D:
    x: float
    y: float

L1: float = 40.0
L2: float = 60.0
L3: float = 60.0
L4: float = 40.0
L5: float = 30.0

A = Point2D(-(L5 / 2.0), 0.0)
E = Point2D(L5 / 2.0, 0.0)

def calc_inverse_kinematics_half(
    anchor: Point2D, target: Point2D, femur: float, tibia: float, leftSide: bool
) -> float:

    # Can arget be reached?
    dy: float = target.y - anchor.y
    dx: float = target.x - anchor.x
    dist: float = sqrt(dy**2 + dx**2)

    if dist > femur + tibia or dist < abs(femur - tibia):
        return 0.0  # Target unreachable

    # use atan2 because it takes into account the quadrant of the angle
    alpha: float = atan2(dy, dx) # Radians
    beta: float=acos(
        (femur**2 + dist**2 - tibia**2) / (2.0 * femur * dist)
    ) # Radians

    # Start by assuming right hand side of mechanism
    theta: float = alpha - beta
    if leftSide:
        theta = alpha + beta

    return theta * 180.0 / pi  # Return as degrees

def calc_inverse_kinematics_5r(target: Point2D) -> tuple[float, float]:
    # Calc for left half (prefer elbow to left)
    a_angle: float = calc_inverse_kinematics_half(A, target, L1, L2, True)

    # Calc for right half (prefer elbow to right)
    e_angle: float = calc_inverse_kinematics_half(E, target, L4, L3, False)

    return (a_angle, e_angle)

Forward Kinematics

This is working forwards from setting the angles of joints ‘A’ and ‘B’ to working out where the end effector ‘C’ will be. The need for this calculation is relatively rare in a hobbyist scenario as it requires that the mechanism is fitted with position feedback sensors on the joints. Also, it is more often associated with a human manually moving the arm to train the robot on new movement patterns. However, it is included here for completeness of understanding. It’s also a useful check to make sure that the calculation for inverse kinematics has been programmed correctly.

The math is slightly more complicated than for the 2R-2 Bar model, because we don’t know the angles for the knee/elbow joints. The situation is further complicated by the fact that for any angle of driving joint, the associated knee/elbow at the end of it’s link can have it’s other link pointing either up or down.

Even so, it still just requires a combination Pythagoras and the law of cosines to solve the calculation, typically in robotics the elbow-up position is preferred.

Start by dropping an imaginary line down from ‘B’ to ground, and then connecting back to ‘A’. this forms a right angled triangle that will be used to calculate the ‘X’ and ‘Y’ coordinates of ‘B’. the same is done for joint ‘D’ on the other side.

Now to be able to calculate the position of joint ‘C’ (the end effector), we start by connecting ‘B’ and ‘D’ with an imaginary line which will form the hypotenuse of a right-angled triangle. The base of that triangle is formed by connecting an imaginary horizontal line from ‘B’ to intersect a vertical line down (or up) from ‘D’. Next we need to drop an imaginary line down from ‘C’ to intersect the base of the triangle just imagined between ‘B’ and ‘D’. That’s it, we’re all set to do the math 🙂

In the diagram below illustrating this, the relative linkage lengths shown, while perfectly legitimate, are highly unlikely. In most practical situations, the left and right sides of the mechanism are mirror images of each other. It is drawn this way purely to clarify the math calculations involved.

\[ B_x = -\frac{L_5}{2} + L_1 * \cos(\theta_1) \]

\[ B_y = L_1 * \sin(\theta_1) \]

\[ D_x = \frac{L_5}{2} + L_4 * \cos(\theta_2) \]

\[ D_y = L_4 * \sin(\theta_2) \]

\[ d = \sqrt{(D_x – B_x)^2 + (D_y – B_y)^2} \]

\[ \phi = arctan\left(\frac{D_y – B_y}{D_x – B_x}\right) \]

\[ \alpha = \arccos\left(\frac{d^2 + L_1^2 – L_3^2}{2 * d * L_2}\right) \]

\[ C_x = B_x + L_2 * \cos(\phi \pm \alpha) \]

\[ C_y = B_y + L_2 * \sin(\phi \pm \alpha) \]

Note that the \(\pm\) in the final two equations reflects the fact that the mechanism often has two possible positions of ‘C’ for a given angle of ‘B’ and ‘D’ (elbow up and elbow down). In most robotics situations the “elbow up” (+) position is the preferred outcome.

struct Point2D {
    float x;
    float y;
};

// Links (bones)
const float L1 = 40.0f; // Femur (or Humerus) depending on use
const float L2 = 60.0f; // Tibia (or Radius)depending upon use
const float L3 = 60.0f; // Tibia (or Radius) depending upon use
const float L4 = 40.0f; // Femur (or Humerus) depending on use
const float L5 = 30.0f; // Fixed hip (or shoulders) depending on use

// Driving joint coordinates - Origin at center of L5
const Point2D A { -(L5 / 2.0f), 0.0f }; // Left of origin
const Point2D E { (L5 / 2.0f), 0.0f }; // Right of origin

bool calcFwdKinematics5R(float theta1Deg, float theta2Deg, Point2D& C, bool elbowUp = true) {
    // Translate input degrees into radians
    float theta1 = theta1Deg * M_PIf / 180.0f;
    float theta2 = theta2Deg * M_PIf / 180.0f;

    // Coordinates for knees/elbows
    float Bx = -L5 / 2.0f + L1 * std::cos(theta1);
    float By = L1 * std::sinf(theta1);
    float Dx = L5 / 2.0f + L4 * std::cos(theta2);
    float Dy = L4 * std::sinf(theta2);

    // Distance & angle between knees/elbows
    float dx = Dx - Bx;
    float dy = Dy - By;
    float d = std::sqrtf(dx * dx + dy * dy);

    // Sanity check - is distance achievable with link lengths we have?
    if (d > (L2 + L3) || d < std::fabsf(L2 - L3) || d == 0.0f) {
        return false; // Out of range
    }

    // Calc top two triangles
    float phi = std::atan2f(dy, dx);
    float alpha = std::acosf((d * d + L2 * L2 - L3 * L3) / (2 * d * L2));

    // Now we can calculate the coordinates for C
    if (elbowUp) {
        // usually preferred solution
        C.x = Bx + L2 * std::cosf(phi + alpha);
        C.y = By + L2 * std::sinf(phi + alpha);
    }
    else{
        C.x = Bx + L2 * std::cosf(phi - alpha);
        C.y = By + L2 * std::sinf(phi - alpha);
    }

    return true; // If we got here everything must be OK
}

from math import cos, sin, acos, atan2, sqrt, pi
from dataclasses import dataclass

@dataclass
class Point2D:
    x: float
    y: float

L1: float = 40.0
L2: float = 60.0
L3: float = 60.0
L4: float = 40.0
L5: float = 30.0

A = Point2D(-(L5 / 2.0), 0.0)
E = Point2D(L5 / 2.0, 0.0)

def calc_forward_kinematics_5r(
    theta1_deg: float, theta2_deg: float, elbow_up: bool=True
) -> tuple[bool, Point2D]:

    foot=Point2D(0.0, 0.0)

    # Translate input degrees into radians
    theta1: float = theta1_deg * pi / 180.0
    theta2: float = theta2_deg * pi / 180.0

    # Coordinates for knees/elbows
    B = Point2D(-L5 / 2.0 + L1 * cos(theta1), L1 * sin(theta1))
    D = Point2D(L5 / 2.0 + L4 * cos(theta2), L4 * sin(theta2))

    # Distance & angle between elbows
    dx: float = D.x - B.x
    dy: float = D.y - B.y
    d: float = sqrt(dx**2 + dy**2)

    # Is distance within range of links?
    if d > (L2 + L3) or d < abs(L2 - L3) or d == 0.0:
        return False, foot# Out of range

    # Calculate top two triangles
    phi: float = atan2(dy, dx)
    alpha: float = acos((d**2 + L2**2 - L3**2) / (2 * d * L2))

    # Now we can calculate the coordinates for the foot
    if elbow_up:
        # Usually the preferred solution
        foot.x = B.x + L2 * cos(phi + alpha)
        foot.y = B.y + L2 * sin(phi + alpha)
    else:
        foot.x = B.x + L2 * cos(phi - alpha)
        foot.y = B.y + L2 * sin(phi - alpha)

    return True, foot# If we got here everything is OK

Adaptation for 3D Space

Like the previously investigated 2R-2 Bar mechanism, the 5R-5 Bar mechanism also operates purely in 2D space. However, most real world use cases require it to operate in a limited 3D space. One of the most frequent use cases for this mechanism for the robotics hobbyist, is as a leg mechanism for a quadruped. Fortunately there are a couple of alternative simple modifications that can be made to accommodate that need.

1. The whole mechanism can be rotated about the X-axis. Think of a leg doing a side step. Rotating the mechanism in this way allows the leg to work in a horizontal cylinder of 3D space (actually it’s closer to the shape of a doughnut, but the usable area is effectively a hollow cylinder with very thick walls). It is very simple to implement, and is quite adequate for many situations.
2. The whole mechanism can be rotated about the Y-axis. Think of steering a leg like you steer a car. Again very simple to implement, though less often used than option 1 (probably because it looks less life like on a quadruped).

The illustrations below have been drawn upright to keep the labeling in line with all the calculations shown above. Though, obviously, if it were a leg it would be inverted. Both options presented have very similar calculation requirements and solutions. Assuming that a Cartesian coordinate system is employed, they both need:

• To find the angle of rotation (\(\theta_3\)) that gives the required ‘Z’ offset.
• To find the new, longer, ‘X’ or ‘Y’ coordinate (Y’ or X’).

Side-Step Modification

.

\[ \theta_3 = \tan\left(\frac{z}{y}\right) \]

\[ y’ = \sqrt{y^2 + z^2} \]

Steering Modification

\[ \theta_3 = \tan\left(\frac{z}{x}\right) \]

\[ x’ = \sqrt{x^2 + z^2} \]

Derivations of the Mechanism

There are a number of adaptations of this mechanism to meet varying practical needs. I don’t intend to mention them all here, though there is one adaptation that is commonly enough encountered to be worth mentioning.

The classic mechanism, as it has been detailed above, works well for relative small robots. However, once the size of the robot body starts to increase, so too does the need for ground clearance. A leg, using the classic design, that is long enough to give that clearance gets rather bulky. The solution implemented by many designers is to keep the mechanism as it is, and relatively small, but to add an extension to either ‘L2’ or ‘L3’ beyond joint ‘C’. Then move the foot to the end of that extension.

The advantage of this minor alteration to the mechanism is to keep the leg slim, but still be long enough to give the required ground clearance. The mechanism is then often mounted at a slight angle on the body to reduce the effective rake of the leg. Here is a schematic of what that might look like.

There are a few things to note about this configuration:

• The base (\(L_5\)), is fixed at an angle to allow the leg to be close to straight in the neutral position;
• \(L_1\) and \(L_2\) are now of asymmetrical lengths because the lift and drive is mostly separated allowing for a more customized length of movement;
• \(L_6\) is not hinged as such, but is a continuation of \(L_2\) to get the extra leg length;
• Joint ‘C’ is no longer the foot. It is jointed midway down the \(L_2\)/\(L_6\) leg;
• \(L_1\) now acts primarily in lifting the foot off of the ground;
• \(L_4\) now acts primarily to drive the leg forwards and backwards;
• The force from the foot is not evenly distributed across ‘A’ and ‘E’ as in the classical configuration.

The corresponding alteration to the kinematics for this modified design is relatively straight forward, but will not be discussed further here as this article is already long enough. Though I may revisit that decision later.