Let \( \mathbf{q}\in\mathbb{R}^n \) denote joint coordinates and \( \mathbf{x}\in\mathbb{R}^m \) denote task-space coordinates (pose of the end-effector or moving platform). For a serial manipulator, the configuration map

\[ \mathbf{x} = f_{\mathrm{ser}}(\mathbf{q}),\qquad f_{\mathrm{ser}} : \mathcal{Q}\subset\mathbb{R}^n \to \mathcal{X}\subset\mathbb{R}^m \]

is explicitly constructed as a product of homogeneous transforms or exponentials. FK simply evaluates \( f_{\mathrm{ser}} \); IK inverts it:

\[ \text{Given } \mathbf{x}_d\in\mathcal{X},\ \text{find } \mathbf{q}\in\mathcal{Q} \ \text{s.t.}\ f_{\mathrm{ser}}(\mathbf{q})=\mathbf{x}_d. \]

For a parallel manipulator, the situation is different. The end-effector is connected to the base by several independent kinematic chains (legs) forming one or more kinematic loops. Instead of an explicit mapping \( \mathbf{x}=f(\mathbf{q}) \), we typically have holonomic constraints

\[ \boldsymbol{\Phi}(\mathbf{x},\mathbf{q}) = \mathbf{0}, \qquad \boldsymbol{\Phi} : \mathcal{X}\times\mathcal{Q}\to\mathbb{R}^{n_c}, \]

which encode loop closure and leg geometry. The forward kinematics (FK) of a parallel robot is:

\[ \text{Given actuated joints } \mathbf{q}_a,\ \text{solve for } \mathbf{x} \ \text{s.t.}\ \boldsymbol{\Phi}(\mathbf{x},\mathbf{q}_a)=\mathbf{0}. \]

This is typically a nonlinear polynomial system with several isolated solutions (assembly modes). In contrast, for many architectures the inverse kinematics (IK) is leg-wise geometric:

\[ \text{Given } \mathbf{x}_d,\ \text{compute } \mathbf{q}_a \ \text{from simple distance or angle relations on each leg.} \]

The following diagram summarizes this inversion of difficulty between serial and parallel manipulators.

flowchart TB
  S1["Serial FK: x = f_ser(q) \n(closed form)"]
  S2["Serial IK: solve f_ser(q) = x_d \n(nonlinear, many solutions)"]
  P1["Parallel IK: q_a from leg geometry \n(often explicit)"]
  P2["Parallel FK: solve Phi(x, q_a) = 0 \n(polynomial, multi-solution)"]
  S1 --> S2
  P1 --> P2
        

In subsequent lessons we will differentiate the constraint equations \( \boldsymbol{\Phi}(\mathbf{x},\mathbf{q})=\mathbf{0} \) to obtain Jacobians relating platform and joint velocities, and eventually incorporate these constraints into the dynamic model.

Consider a parallel manipulator with a moving platform of \( m \) degrees of freedom. Let the generalized platform coordinates be \( \mathbf{x}\in\mathbb{R}^m \) (e.g. planar pose \( \mathbf{x}=[x\ y\ \varphi]^\top \) or spatial pose in \( \mathrm{SE}(3) \)). The legs provide \( n_a \) actuated joints and possibly additional passive joints.

A convenient modeling strategy is to treat platform coordinates and all leg joint variables as independent, and to enforce loop-closure constraints:

\[ \boldsymbol{\Phi}(\mathbf{x},\mathbf{q}_a,\mathbf{q}_p) = \mathbf{0}, \]

where \( \mathbf{q}_a \) are actuated joints and \( \mathbf{q}_p \) are passive joints. For many common architectures (RPR, RRR, UPS legs, etc.), each leg contributes one scalar constraint relating the distance or orientation of two anchor points attached to base and platform. For a schematic leg “length” equation,

\[ \Phi_i(\mathbf{x},q_{ai}) = \big\|\mathbf{p}_i(\mathbf{x})\big\| - q_{ai} = 0, \qquad i=1,\dots,n_a, \]

where \( \mathbf{p}_i(\mathbf{x}) \) is the vector from the base anchor point of leg \( i \) to the corresponding platform anchor point, expressed in the base frame. This structure already suggests why IK in parallel robots is easier: given \( \mathbf{x} \), IK simply evaluates \( q_{ai}=\|\mathbf{p}_i(\mathbf{x})\| \) (plus possibly an offset), whereas FK must invert these relations simultaneously.

Differentially, the constraint equations imply

\[ \frac{\partial\boldsymbol{\Phi}}{\partial \mathbf{x}}\dot{\mathbf{x}} + \frac{\partial\boldsymbol{\Phi}}{\partial \mathbf{q}_a}\dot{\mathbf{q}}_a = \mathbf{0}, \]

which defines the task-space and joint-space Jacobians for parallel mechanisms. The structure and singularities of these Jacobians will be analyzed formally in Lesson 2.

As a concrete example we consider a planar 3-RPR (Revolute-Prismatic-Revolute) parallel manipulator. The base carries three revolute joints with fixed anchor points \( B_i\in\mathbb{R}^2 \). The moving platform carries three anchor points \( P_i\in\mathbb{R}^2 \) expressed in the platform frame. Each leg consists of a passive revolute joint at the base, an actuated prismatic joint of length \( L_i \), and a passive revolute joint on the platform.

The planar pose of the platform is \( \mathbf{x}=[x\ y\ \varphi]^\top \), where \( x,y \) are the coordinates of a chosen platform reference point (e.g. its geometric center), and \( \varphi \) is the rotation angle with respect to the base frame. The rotation matrix is

\[ \mathbf{R}(\varphi)= \begin{bmatrix} \cos\varphi & -\sin\varphi \\ \sin\varphi & \cos\varphi \end{bmatrix}. \]

The position of the platform anchor point \( P_i \) in the base frame is

\[ \mathbf{c}_i(\mathbf{x}) = \begin{bmatrix} x \\ y \end{bmatrix} + \mathbf{R}(\varphi) \begin{bmatrix} P_{ix} \\ P_{iy} \end{bmatrix}. \]

The vector along leg \( i \) (from base anchor to platform anchor) is then

\[ \mathbf{p}_i(x,y,\varphi) = \mathbf{c}_i(\mathbf{x}) - B_i = \begin{bmatrix} x \\ y \end{bmatrix} + \mathbf{R}(\varphi) \begin{bmatrix} P_{ix} \\ P_{iy} \end{bmatrix} - \begin{bmatrix} B_{ix} \\ B_{iy} \end{bmatrix}. \]

By construction, the prismatic joint length \( L_i \) satisfies

\[ L_i = \big\|\mathbf{p}_i(x,y,\varphi)\big\|, \qquad L_i^2 = \mathbf{p}_i^\top(x,y,\varphi)\,\mathbf{p}_i(x,y,\varphi). \]

This leg-wise distance relation is the basic kinematic equation that we will exploit in IK and FK.

4.1 Inverse Kinematics (platform pose → leg lengths)

For IK we are given a desired platform pose \( \mathbf{x}_d=[x_d\ y_d\ \varphi_d]^\top \) and we must compute the required prismatic joint lengths \( L_i \). Using the expressions in the previous section,

\[ L_i(\mathbf{x}_d) = \left\| \begin{bmatrix} x_d \\ y_d \end{bmatrix} + \mathbf{R}(\varphi_d) \begin{bmatrix} P_{ix} \\ P_{iy} \end{bmatrix} - \begin{bmatrix} B_{ix} \\ B_{iy} \end{bmatrix} \right\|, \quad i=1,2,3. \]

These are explicit expressions in terms of the known pose \( \mathbf{x}_d \). Thus, IK for this parallel mechanism involves only evaluating trigonometric functions and a square root for each leg; the legs are decoupled in IK.

4.2 Forward Kinematics (leg lengths → platform pose)

For FK we are given the actuated leg lengths \( L_i \) and must determine the possible platform poses \( (x,y,\varphi) \) satisfying

\[ \Phi_i(x,y,\varphi,L_i) = \mathbf{p}_i^\top(x,y,\varphi)\,\mathbf{p}_i(x,y,\varphi) - L_i^2 = 0, \quad i=1,2,3. \]

This is a coupled nonlinear system in \( x,y,\varphi \). A common analytic strategy is to subtract one equation from the others to eliminate quadratic terms in \( x,y \). For example, \( \Phi_2-\Phi_1=0 \) and \( \Phi_3-\Phi_1=0 \) yield two equations that are linear in \( x \) and \( y \) for each fixed \( \varphi \). The result is

\[ \mathbf{A}(\varphi) \begin{bmatrix} x \\[4pt] y \end{bmatrix} = \mathbf{b}(\varphi,\mathbf{L}), \]

where \( \mathbf{A}(\varphi)\in\mathbb{R}^{2\times 2} \) and \( \mathbf{b}(\varphi,\mathbf{L})\in\mathbb{R}^2 \) are explicitly computable trigonometric functions of \( \varphi \), \( B_i \) and \( P_i \), and of the squared lengths \( L_i^2 \). Provided \( \det\mathbf{A}(\varphi)\neq 0 \), we have

\[ \begin{bmatrix} x \\[4pt] y \end{bmatrix} = \mathbf{A}^{-1}(\varphi)\,\mathbf{b}(\varphi,\mathbf{L}). \]

Substituting this expression into one of the original constraint equations \( \Phi_i=0 \) yields a single scalar equation in \( \varphi \). In general, for the 3-RPR mechanism this becomes a polynomial in \( \tan(\varphi/2) \) of degree up to six, leading to as many as six distinct assembly modes (real solutions) for a given set of leg lengths.

This contrast is characteristic of parallel mechanisms: IK is local, leg-wise, and explicit; FK is global, loop-wise, and algebraic. Numerically, FK is often solved with iterative methods (e.g. Newton–Raphson) starting from a known assembly mode or a nearby configuration.

flowchart TD
  QA["Given leg lengths q_a = (L1,L2,L3)"] --> C1["Form constraint Phi(x,y,phi,q_a) = 0"]
  C1 --> N1["Iterative FK: Newton or continuation on (x,y,phi)"]
  Xd["Given desired pose x_d"] --> C2["Compute each Li = ||p_i(x_d)||"]
  C2 --> CHK["Check joint limits, workspace, collisions"]
        

From Chapter 7, differential kinematics for a serial robot is expressed as \( \dot{\mathbf{x}} = \mathbf{J}_{\mathrm{ser}}(\mathbf{q})\dot{\mathbf{q}} \). For the constraint formulation of a parallel robot, differentiating \( \boldsymbol{\Phi}(\mathbf{x},\mathbf{q}_a)=\mathbf{0} \) gives

\[ \mathbf{J}_x(\mathbf{x},\mathbf{q}_a)\,\dot{\mathbf{x}} + \mathbf{J}_q(\mathbf{x},\mathbf{q}_a)\,\dot{\mathbf{q}}_a = \mathbf{0}, \]

where \( \mathbf{J}_x=\partial\boldsymbol{\Phi}/\partial\mathbf{x} \) and \( \mathbf{J}_q=\partial\boldsymbol{\Phi}/\partial\mathbf{q}_a \). Assuming \( \mathbf{J}_x \) is invertible at a given configuration, we obtain the parallel manipulator Jacobian

\[ \dot{\mathbf{x}} = -\mathbf{J}_x^{-1}(\mathbf{x},\mathbf{q}_a)\, \mathbf{J}_q(\mathbf{x},\mathbf{q}_a)\,\dot{\mathbf{q}}_a. \]

In this sense, local FK (mapping velocities from joint to task space) is valid only when the constraint Jacobian \( \mathbf{J}_x \) is nonsingular. Near configurations where \( \det\mathbf{J}_x=0 \), FK may become locally non-invertible, corresponding to singularities and to changes in the number of real FK solutions (assembly modes). These aspects will be treated rigorously in Lessons 2 and 3.

We now implement IK and FK for a 3-RPR mechanism with equilateral base and platform. We use NumPy for vector operations and implement a Newton method for FK using the analytic Jacobian of the constraint equations.

import numpy as np

# Geometry: equilateral base and platform triangles
# Base anchor points B_i in base frame
B = np.array([
    [0.0, 0.0],
    [1.0, 0.0],
    [0.5, np.sqrt(3.0) / 2.0]
])

# Platform anchor points P_i in platform frame (equilateral triangle of radius rp)
rp = 0.2
P = rp * np.array([
    [1.0, 0.0],
    [-0.5, np.sqrt(3.0) / 2.0],
    [-0.5, -np.sqrt(3.0) / 2.0]
])


def rot2(phi):
    """2D rotation matrix."""
    c = np.cos(phi)
    s = np.sin(phi)
    return np.array([[c, -s],
                     [s,  c]])


def ik_3rpr(x, y, phi):
    """
    Inverse kinematics: given planar pose (x, y, phi),
    return leg lengths L (shape (3,)).
    """
    R = rot2(phi)
    t = np.array([x, y])
    L = np.zeros(3)
    for i in range(3):
        ci = t + R @ P[i]       # platform anchor in base frame
        pi = ci - B[i]          # leg vector
        L[i] = np.linalg.norm(pi)
    return L


def fk_residual(z, L):
    """
    Residual vector Phi(z) for FK, where
    z = [x, y, phi], and L is array of leg lengths.
    Phi_i = ||p_i||^2 - L_i^2.
    """
    x, y, phi = z
    R = rot2(phi)
    t = np.array([x, y])
    res = np.zeros(3)
    for i in range(3):
        ci = t + R @ P[i]
        pi = ci - B[i]
        res[i] = pi @ pi - L[i] ** 2
    return res


def fk_jacobian(z):
    """
    Jacobian dPhi/dz for the 3-RPR FK problem.
    z = [x, y, phi].
    """
    x, y, phi = z
    R = rot2(phi)
    # Derivative of R w.r.t. phi
    s = np.sin(phi)
    c = np.cos(phi)
    R_phi = np.array([[-s, -c],
                      [ c, -s]])

    t = np.array([x, y])
    J = np.zeros((3, 3))
    for i in range(3):
        Pi = P[i]
        ci = t + R @ Pi
        pi = ci - B[i]
        # d/dx (||p_i||^2) = 2 p_i_x
        # d/dy (||p_i||^2) = 2 p_i_y
        dphidx = 2.0 * pi[0]
        dphidy = 2.0 * pi[1]
        dpi_dphi = R_phi @ Pi
        dphidphi = 2.0 * (pi @ dpi_dphi)
        J[i, 0] = dphidx
        J[i, 1] = dphidy
        J[i, 2] = dphidphi
    return J


def fk_3rpr_newton(L, z0, tol=1e-10, max_iter=50):
    """
    Newton iteration for FK:
    solve Phi(z) = 0 for z = [x, y, phi].
    L: given leg lengths (shape (3,))
    z0: initial guess (array-like of length 3)
    """
    z = np.array(z0, dtype=float)
    for k in range(max_iter):
        res = fk_residual(z, L)
        norm_res = np.linalg.norm(res)
        if norm_res < tol:
            return z, True, k
        J = fk_jacobian(z)
        try:
            dz = np.linalg.solve(J, -res)
        except np.linalg.LinAlgError:
            return z, False, k
        z = z + dz
    return z, False, max_iter


if __name__ == "__main__":
    # Test round trip FK/IK
    x_true, y_true, phi_true = 0.2, 0.1, 0.3
    L = ik_3rpr(x_true, y_true, phi_true)
    z0 = np.array([0.0, 0.0, 0.0])  # rough initial guess
    z_sol, converged, iters = fk_3rpr_newton(L, z0)
    print("converged:", converged, "in", iters, "iterations")
    print("true pose:", x_true, y_true, phi_true)
    print("FK pose:", z_sol)
      

The analytic Jacobian fk_jacobian implements the expressions derived from \( \Phi_i=L_i^2-\|\mathbf{p}_i\|^2 \). Near singularities, np.linalg.solve becomes ill-conditioned, reflecting the degeneracy of local FK.

Below is a minimal C++ implementation using the standard library only. For larger systems, one would typically use Eigen for linear algebra, but here we implement a tiny 3×3 solver explicitly for clarity.

#include <iostream>
#include <array>
#include <cmath>

struct Vec2 {
    double x;
    double y;
};

struct Pose {
    double x;
    double y;
    double phi;
};

using Vec3 = std::array<double, 3>;

static const std::array<Vec2, 3> B = { {
    {0.0, 0.0},
    {1.0, 0.0},
    {0.5, std::sqrt(3.0) / 2.0}
} };

static const double rp = 0.2;
static const std::array<Vec2, 3> P = { {
    { rp * 1.0,                 rp * 0.0 },
    { rp * (-0.5),              rp * (std::sqrt(3.0) / 2.0) },
    { rp * (-0.5),              rp * (-std::sqrt(3.0) / 2.0) }
} };

inline Vec2 rot2(const Vec2 &v, double phi) {
    double c = std::cos(phi);
    double s = std::sin(phi);
    return { c * v.x - s * v.y,
             s * v.x + c * v.y };
}

Vec3 ik_3rpr(const Pose &pose) {
    Vec3 L{};
    for (int i = 0; i < 3; ++i) {
        Vec2 Pi = P[i];
        Vec2 Pi_base = rot2(Pi, pose.phi);
        Vec2 ci { pose.x + Pi_base.x, pose.y + Pi_base.y };
        Vec2 pi { ci.x - B[i].x, ci.y - B[i].y };
        L[i] = std::sqrt(pi.x * pi.x + pi.y * pi.y);
    }
    return L;
}

Vec3 fk_residual(const Pose &pose, const Vec3 &L) {
    Vec3 r{};
    for (int i = 0; i < 3; ++i) {
        Vec2 Pi = P[i];
        Vec2 Pi_base = rot2(Pi, pose.phi);
        Vec2 ci { pose.x + Pi_base.x, pose.y + Pi_base.y };
        Vec2 pi { ci.x - B[i].x, ci.y - B[i].y };
        double norm2 = pi.x * pi.x + pi.y * pi.y;
        r[i] = norm2 - L[i] * L[i];
    }
    return r;
}

std::array<Vec3, 3> fk_jacobian(const Pose &pose) {
    std::array<Vec3, 3> J{};
    double c = std::cos(pose.phi);
    double s = std::sin(pose.phi);
    // derivative of rotation w.r.t. phi
    for (int i = 0; i < 3; ++i) {
        Vec2 Pi = P[i];
        Vec2 Pi_base { c * Pi.x - s * Pi.y,
                       s * Pi.x + c * Pi.y };
        Vec2 ci { pose.x + Pi_base.x, pose.y + Pi_base.y };
        Vec2 pi { ci.x - B[i].x, ci.y - B[i].y };
        // R_phi * Pi
        Vec2 dpi_dphi { -s * Pi.x - c * Pi.y,
                         c * Pi.x - s * Pi.y };

        double dphidx   = 2.0 * pi.x;
        double dphidy   = 2.0 * pi.y;
        double dphidphi = 2.0 * (pi.x * dpi_dphi.x + pi.y * dpi_dphi.y);

        J[i][0] = dphidx;
        J[i][1] = dphidy;
        J[i][2] = dphidphi;
    }
    return J;
}

// Solve 3x3 linear system J * dx = rhs using Cramer's rule (for simplicity)
bool solve3x3(const std::array<Vec3, 3> &J, const Vec3 &rhs, Vec3 &dx) {
    auto det3 = [](const std::array<Vec3, 3> &M) {
        return M[0][0]*(M[1][1]*M[2][2] - M[1][2]*M[2][1])
             - M[0][1]*(M[1][0]*M[2][2] - M[1][2]*M[2][0])
             + M[0][2]*(M[1][0]*M[2][1] - M[1][1]*M[2][0]);
    };

    double detJ = det3(J);
    if (std::fabs(detJ) < 1e-12) {
        return false;
    }

    std::array<Vec3, 3> Mx = J;
    std::array<Vec3, 3> My = J;
    std::array<Vec3, 3> Mphi = J;

    for (int i = 0; i < 3; ++i) {
        Mx[i][0]   = rhs[i];
        My[i][1]   = rhs[i];
        Mphi[i][2] = rhs[i];
    }

    dx[0] = det3(Mx)   / detJ;
    dx[1] = det3(My)   / detJ;
    dx[2] = det3(Mphi) / detJ;
    return true;
}

bool fk_3rpr_newton(const Vec3 &L, Pose &pose, int max_iter = 50, double tol = 1e-10) {
    for (int k = 0; k < max_iter; ++k) {
        Vec3 r = fk_residual(pose, L);
        double norm_r = std::sqrt(r[0]*r[0] + r[1]*r[1] + r[2]*r[2]);
        if (norm_r < tol) {
            return true;
        }
        std::array<Vec3, 3> J = fk_jacobian(pose);
        Vec3 rhs { -r[0], -r[1], -r[2] };
        Vec3 dx{};
        if (!solve3x3(J, rhs, dx)) {
            return false; // singular
        }
        pose.x   += dx[0];
        pose.y   += dx[1];
        pose.phi += dx[2];
    }
    return false;
}

int main() {
    Pose pose_true {0.2, 0.1, 0.3};
    Vec3 L = ik_3rpr(pose_true);

    Pose pose_guess {0.0, 0.0, 0.0};
    bool ok = fk_3rpr_newton(L, pose_guess);
    std::cout << "Converged: " << ok << "\n";
    std::cout << "True pose: "
              << pose_true.x << ", "
              << pose_true.y << ", "
              << pose_true.phi << "\n";
    std::cout << "FK pose:   "
              << pose_guess.x << ", "
              << pose_guess.y << ", "
              << pose_guess.phi << "\n";
    return 0;
}
      

The ill-conditioning of solve3x3 near singular configurations reflects the same limitations as in the Python code. In practice, one would use pivoting and robust numerical libraries.

A Java implementation follows a similar structure, using primitive arrays for simplicity. The Newton solver is coded explicitly for the 3-variable system.

public class ThreeRPR {

    static class Pose {
        double x, y, phi;
        Pose(double x, double y, double phi) { this.x = x; this.y = y; this.phi = phi; }
    }

    static class Vec2 {
        double x, y;
        Vec2(double x, double y) { this.x = x; this.y = y; }
    }

    static Vec2[] B = new Vec2[] {
        new Vec2(0.0, 0.0),
        new Vec2(1.0, 0.0),
        new Vec2(0.5, Math.sqrt(3.0) / 2.0)
    };

    static double rp = 0.2;
    static Vec2[] P = new Vec2[] {
        new Vec2(rp * 1.0,              rp * 0.0),
        new Vec2(rp * (-0.5),           rp * (Math.sqrt(3.0) / 2.0)),
        new Vec2(rp * (-0.5),           rp * (-Math.sqrt(3.0) / 2.0))
    };

    static Vec2 rot2(Vec2 v, double phi) {
        double c = Math.cos(phi);
        double s = Math.sin(phi);
        return new Vec2(c * v.x - s * v.y, s * v.x + c * v.y);
    }

    static double[] ik3RPR(Pose pose) {
        double[] L = new double[3];
        for (int i = 0; i < 3; ++i) {
            Vec2 PiBase = rot2(P[i], pose.phi);
            double cx = pose.x + PiBase.x;
            double cy = pose.y + PiBase.y;
            double px = cx - B[i].x;
            double py = cy - B[i].y;
            L[i] = Math.sqrt(px * px + py * py);
        }
        return L;
    }

    static double[] fkResidual(Pose pose, double[] L) {
        double[] r = new double[3];
        for (int i = 0; i < 3; ++i) {
            Vec2 PiBase = rot2(P[i], pose.phi);
            double cx = pose.x + PiBase.x;
            double cy = pose.y + PiBase.y;
            double px = cx - B[i].x;
            double py = cy - B[i].y;
            double norm2 = px * px + py * py;
            r[i] = norm2 - L[i] * L[i];
        }
        return r;
    }

    static double[][] fkJacobian(Pose pose) {
        double[][] J = new double[3][3];
        double c = Math.cos(pose.phi);
        double s = Math.sin(pose.phi);
        for (int i = 0; i < 3; ++i) {
            Vec2 Pi = P[i];
            double pix = c * Pi.x - s * Pi.y;
            double piy = s * Pi.x + c * Pi.y;
            double cx = pose.x + pix;
            double cy = pose.y + piy;
            double px = cx - B[i].x;
            double py = cy - B[i].y;
            double dpxdphi = -s * Pi.x - c * Pi.y;
            double dpydphi =  c * Pi.x - s * Pi.y;

            double dphidx   = 2.0 * px;
            double dphidy   = 2.0 * py;
            double dphidphi = 2.0 * (px * dpxdphi + py * dpydphi);

            J[i][0] = dphidx;
            J[i][1] = dphidy;
            J[i][2] = dphidphi;
        }
        return J;
    }

    static boolean solve3x3(double[][] A, double[] b, double[] x) {
        double detA =
            A[0][0]*(A[1][1]*A[2][2] - A[1][2]*A[2][1]) -
            A[0][1]*(A[1][0]*A[2][2] - A[1][2]*A[2][0]) +
            A[0][2]*(A[1][0]*A[2][1] - A[1][1]*A[2][0]);
        if (Math.abs(detA) < 1e-12) return false;

        double[][] Ax = new double[3][3];
        double[][] Ay = new double[3][3];
        double[][] Aphi = new double[3][3];
        for (int i = 0; i < 3; ++i) {
            for (int j = 0; j < 3; ++j) {
                Ax[i][j] = A[i][j];
                Ay[i][j] = A[i][j];
                Aphi[i][j] = A[i][j];
            }
        }
        for (int i = 0; i < 3; ++i) {
            Ax[i][0] = b[i];
            Ay[i][1] = b[i];
            Aphi[i][2] = b[i];
        }

        double detAx =
            Ax[0][0]*(Ax[1][1]*Ax[2][2] - Ax[1][2]*Ax[2][1]) -
            Ax[0][1]*(Ax[1][0]*Ax[2][2] - Ax[1][2]*Ax[2][0]) +
            Ax[0][2]*(Ax[1][0]*Ax[2][1] - Ax[1][1]*Ax[2][0]);

        double detAy =
            Ay[0][0]*(Ay[1][1]*Ay[2][2] - Ay[1][2]*Ay[2][1]) -
            Ay[0][1]*(Ay[1][0]*Ay[2][2] - Ay[1][2]*Ay[2][0]) +
            Ay[0][2]*(Ay[1][0]*Ay[2][1] - Ay[1][1]*Ay[2][0]);

        double detAphi =
            Aphi[0][0]*(Aphi[1][1]*Aphi[2][2] - Aphi[1][2]*Aphi[2][1]) -
            Aphi[0][1]*(Aphi[1][0]*Aphi[2][2] - Aphi[1][2]*Aphi[2][0]) +
            Aphi[0][2]*(Aphi[1][0]*Aphi[2][1] - Aphi[1][1]*Aphi[2][0]);

        x[0] = detAx / detA;
        x[1] = detAy / detA;
        x[2] = detAphi / detA;
        return true;
    }

    static boolean fk3RPRNewton(double[] L, Pose pose, int maxIter, double tol) {
        for (int k = 0; k < maxIter; ++k) {
            double[] r = fkResidual(pose, L);
            double norm = Math.sqrt(r[0]*r[0] + r[1]*r[1] + r[2]*r[2]);
            if (norm < tol) return true;

            double[][] J = fkJacobian(pose);
            double[] rhs = new double[] { -r[0], -r[1], -r[2] };
            double[] dx = new double[3];
            if (!solve3x3(J, rhs, dx)) return false;
            pose.x   += dx[0];
            pose.y   += dx[1];
            pose.phi += dx[2];
        }
        return false;
    }

    public static void main(String[] args) {
        Pose poseTrue = new Pose(0.2, 0.1, 0.3);
        double[] L = ik3RPR(poseTrue);
        Pose guess = new Pose(0.0, 0.0, 0.0);
        boolean ok = fk3RPRNewton(L, guess, 50, 1e-10);
        System.out.println("Converged: " + ok);
        System.out.println("True pose: " + poseTrue.x + ", " + poseTrue.y + ", " + poseTrue.phi);
        System.out.println("FK pose:   " + guess.x + ", " + guess.y + ", " + guess.phi);
    }
}
      

The patterns of IK and FK parallel those in Python and C++, but the language constructs differ. The key concept is unchanged: IK is leg-wise and explicit, FK solves a coupled nonlinear system.

In MATLAB, we can write vectorized functions for IK and FK, and integrate them into Simulink as MATLAB Function blocks or via S-Functions. Below is a script-level implementation using fsolve for FK (from Optimization Toolbox) and explicit formulas for IK.

function lesson16_3RPR_demo()
    % Base anchors
    B = [0.0, 0.0;
         1.0, 0.0;
         0.5, sqrt(3.0) / 2.0];

    % Platform anchors
    rp = 0.2;
    P = rp * [ 1.0,              0.0;
              -0.5,              sqrt(3.0) / 2.0;
              -0.5,             -sqrt(3.0) / 2.0];

    x_true  = 0.2;
    y_true  = 0.1;
    phi_true = 0.3;

    L = ik_3rpr_matlab([x_true; y_true; phi_true], B, P);

    z0 = [0.0; 0.0; 0.0];
    opts = optimoptions("fsolve", "Display", "off", "Jacobian", "on");
    [z_sol, fval, exitflag] = fsolve(@(z) fk_3rpr_matlab(z, L, B, P), z0, opts);

    disp("Exit flag:");
    disp(exitflag);
    disp("True pose:");
    disp([x_true; y_true; phi_true]);
    disp("FK pose:");
    disp(z_sol);
end

function L = ik_3rpr_matlab(z, B, P)
    x   = z(1);
    y   = z(2);
    phi = z(3);
    R = [cos(phi), -sin(phi);
         sin(phi),  cos(phi)];
    t = [x; y];
    L = zeros(3, 1);
    for i = 1:3
        ci = t + R * P(i, :).';   % platform anchor in base frame
        pi = ci - B(i, :).';
        L(i) = norm(pi);
    end
end

function [Phi, J] = fk_3rpr_matlab(z, L, B, P)
    x   = z(1);
    y   = z(2);
    phi = z(3);

    c = cos(phi);
    s = sin(phi);
    R = [c, -s;
         s,  c];
    Rphi = [-s, -c;
             c, -s];

    t = [x; y];
    Phi = zeros(3, 1);
    J = zeros(3, 3);

    for i = 1:3
        Pi = P(i, :).';
        ci = t + R * Pi;
        pi = ci - B(i, :).';
        Phi(i) = pi.' * pi - L(i)^2;

        dpi_dphi = Rphi * Pi;
        J(i, 1) = 2.0 * pi(1);
        J(i, 2) = 2.0 * pi(2);
        J(i, 3) = 2.0 * (pi.' * dpi_dphi);
    end
end
      

In Simulink, a typical workflow is:

• Implement ik_3rpr_matlab as a MATLAB Function block to map desired platform pose signals to prismatic joint commands.
• Implement fk_3rpr_matlab (or a reduced version without Jacobian) as a MATLAB Function block to estimate platform pose from measured leg lengths.
• Combine these with dynamic models (introduced in later chapters) for closed-loop simulation.

Wolfram Mathematica is well suited for symbolic derivation of FK equations and for solving polynomial systems. Below is a simple script illustrating IK and a numeric FK solve for the 3-RPR mechanism.

(* Geometry *)
B = { {0., 0.},
     {1., 0.},
     {1./2., Sqrt[3.]/2.} };

rp = 0.2;
P = rp*{ {1., 0.},
        {-1./2., Sqrt[3.]/2.},
        {-1./2., -Sqrt[3.]/2.} };

rot2[phi_] := { {Cos[phi], -Sin[phi]},
               {Sin[phi],  Cos[phi]} };

ik3RPR[{x_, y_, phi_}] := Module[{R, t, ci, pi},
  R = rot2[phi];
  t = {x, y};
  Table[
    ci = t + R.P[[i]];
    pi = ci - B[[i]];
    Norm[pi],
    {i, 1, 3}
  ]
];

fkEquations[{x_, y_, phi_}, L_List] := Module[{R, t, ci, pi},
  R = rot2[phi];
  t = {x, y};
  Table[
    ci = t + R.P[[i]];
    pi = ci - B[[i]];
    pi.pi - L[[i]]^2,
    {i, 1, 3}
  ]
];

(* Test round trip *)
xTrue = 0.2; yTrue = 0.1; phiTrue = 0.3;
Lvals = ik3RPR[{xTrue, yTrue, phiTrue}];

sol = FindRoot[
  fkEquations[{x, y, phi}, Lvals],
  { {x, 0.0}, {y, 0.0}, {phi, 0.0} }
];

solPose = {x, y, phi} /. sol
      

For analytic work, one can use Eliminate or Resultant to derive the univariate polynomial in \( \tan(\varphi/2) \) representing FK, and study its roots symbolically.

In this lesson we formalized the fundamental difference between serial and parallel manipulators in terms of FK and IK. Serial robots admit explicit FK and typically complicated IK, while parallel robots invert this relationship: IK is leg-wise and explicit, whereas FK requires solving coupled polynomial constraints arising from loop closure. We illustrated this on a planar 3-RPR mechanism, derived its leg-length equations, and implemented IK and FK in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica. We also introduced the constraint Jacobian framework that will underpin the analysis of parallel Jacobians, singularities, and dynamics in the subsequent lessons of this chapter.