How to Build a Robot Arm IK Solver in ROS2 | NERO Arm Parametric Inverse Kinematics

[Agilex_Robotics]

Complete Tutorial on Nero Arm Angle Parametric IK

Reference paper from Tsinghua University Paper —— Inverse kinematic optimization for 7-DoF serial manipulators with joint limits

• Paper Link: https://jst.tsinghuajournals.com/article/2020/4286/20201206.htm.

---

Part 1. Overview

This document provides a complete mathematical tutorial on parameterized inverse kinematics (IK) for the NERO 7-DoF robotic arm.

The content mainly corresponds to:

• Tsinghua University paper: Inverse Kinematics Solution for 7-DoF Robotic Arms with Joint Limit Optimization
• Implementation: ik_solver.py
• ROS2 real-time runtime node: ik_joint_state_publisher.py

Part 2. Algorithmic Background and Core Concepts

2.1 Fundamental Characteristics of 7-DoF Redundant Robot Arms

A 7-DoF robotic arm with an S-R-S configuration (Spherical Shoulder – Revolute Elbow – Spherical Wrist) introduces one additional redundant degree of freedom compared with a conventional 6-DoF manipulator.

This means that:

• When the end-effector pose is fixed, the joint configuration may still have infinitely many solutions, and the arm can still move internally while keeping the end-effector stationary.

This type of motion, where the end-effector remains fixed while the robot reconfigures itself, is referred to as null-space motion.

Redundancy provides several important advantages:

1. Joint limit avoidance
2. Obstacle avoidance
3. Elbow posture optimization
4. Smoother trajectory generation

2.2 Elbow Angle Parameterization (Core Contribution of the Paper)

The core idea of the paper is:

Use a single parameter to represent the entire redundant degree of freedom —— this paramter are called elbow angle \psi (theta \theta in the code implementation).

Geometric Definition of the Elbow Angle

When the end-effector pose is fixed, both points S and point W are fixed in space.

The elbow point E then traces a circle in 3D space.
The rotational angle within the plane of this circle is defined as the elbow angle \psi.

• S: Shoulder center (intersection point of the first 3 joint axes)
• E: Elbow center (location of Joint 4)
• W: Wrist center (intersection point of the last 3 joint axes)
• Points S–E–W form a triangle with fixed side lengths
• The elboww angle \psi determines the position of point E on the circle.

In one sentence:

• \psi → elbow posture changes → joint angles change → end-effector remains unchanged

2.3 Differences Between This Method and Traditional Numerical IK Solvers

Comparison Aspect	Numerical Iterative Methods (Jacobian / Damped Least Squares)	Elbow-Angle Parameterized Analytical IK
Solution Strategy	Iterative convergence, dependent on initialization	Geometric derivation with closed-form solution
Computational Speed	Slow (ms–10 ms)	Extremely fast (<0.1 ms)
Convergence	May fail to converge; susceptible to local minima	Globally optimal and divergence-free
Joint Limit Handling	Passive constraint handling; easy to violate limits	Active feasible-region control; never exceeds limits
Null-Space Control	Requires projection operators; prone to instability	Direct control through \psi; naturally stable

---

Part 3.Complete Algorithm Workflow

The entire algorithm consists of four core stages:

1. Extract S, W, and θ_4 from the target pose.
2. Compute the elbow point E from the elbow angle ψ, and analytically solve q_1q_3 and q_5q_7
3. Compute the feasible region of the elbow angle under all joint-limit constraints
4. Optimize the elbow angle within the feasible region using a weighted quadratic objective function

The following sections correspond directly to the equations in the paper and the implementation in code.

---

3.1 Step 1: Solving for S, W, and \theta_4 from the Target Pose

Theory from the Paper

Given the end-effector pose T_{07}, we first solve for:

• Shoulder point S
• Wrist point W (obtained by offsetting the end-effector frame backward by d_6)
• Elbow joint angle \theta_4 (uniquely determined from the S–E–W triangle using the law of cosines)
• As illustrated in the figure, points S, W, E, and D
  
  

Law of Cosines

cos \theta_4 = \frac{||SW||^2-||SE||^2-||EW||^2}{2||SE|| ||EW||}

Code Implementation: _compute_swe_from_target

def _compute_swe_from_target(T07: np.ndarray, p: NeroParams) -> Tuple[np.ndarray, np.ndarray, Optional[float], np.ndarray]:
    R = T07[:3, :3]
    p_target = T07[:3, 3]
    z7 = R[:, 2]
    d6 = float(p.d_i[6])
    d1 = float(p.d_i[0])

    # End-effector flange center
    O7 = p_target - p.post_transform_d8 * z7
    # Wrist center W: offset backward from the flange by d6
    W = O7 - d6 * z7
    # Shoulder center S: fixed at height d1 above the base
    S = np.array([0.0, 0.0, d1], dtype=float)

    # Solve the absolute value of θ4 using the law of cosines
    q4_abs = _solve_theta4_from_triangle(S, W, p)

    # Unit vector from shoulder to wrist
    v_sw = W - S
    n_sw = np.linalg.norm(v_sw)
    u_sw = v_sw / n_sw if n_sw > 1e-12 else np.array([0.0, 0.0, 1.0])

    return S, W, q4_abs, u_sw

Helper Function: _solve_theta4_from_triangle

def _solve_theta4_from_triangle(S: np.ndarray, W: np.ndarray, p: NeroParams) -> Optional[float]:
    l_sw = np.linalg.norm(W - S)
    l_se = abs(p.d_i[2])
    l_ew = abs(p.d_i[4])

    c4 = (l_sw**2 - l_se**2 - l_ew**2) / (2.0 * l_se * l_ew)
    c4 = np.clip(c4, -1.0, 1.0)

    return math.acos(c4)

Key Insight

The elbow joint angle θ_4 depends only on the geometric link lengths and is completely independent of the arm angle ψ.

3.2 Step 2: Solving the Elbow Point E from the Arm Angle \psi (Core Geometry)

Theory from the Paper

The elbow point E lies on a circle whose chord is defined by the segment SW:

E= C + r (cos\psi*e_1 + sin\psi*e_2)

Where:

• C: circle center
• r: circle radius
• e_1,e_2: orthonormal basis vectors spanning the circle plane

Code Implementation: _elbow_from_arm_angle

def _elbow_from_arm_angle(S: np.ndarray, W: np.ndarray, theta0: float, p: NeroParams) -> Optional[np.ndarray]:
    l_se = abs(p.d_i[2])
    l_ew = abs(p.d_i[4])

    sw = W - S
    l_sw = np.linalg.norm(sw)
    u_sw = sw / l_sw

    # Projection of circle center C onto line SW
    x = (l_se**2 - l_ew**2 + l_sw**2) / (2.0 * l_sw)

    r2 = l_se**2 - x**2
    r = math.sqrt(max(0.0, r2))

    C = S + x * u_sw

    # Construct circle-plane coordinate system e1, e2
    os_vec = S.copy()
    t = np.cross(os_vec, u_sw)

    e1 = t / np.linalg.norm(t)

    e2 = np.cross(u_sw, e1)
    e2 = e2 / np.linalg.norm(e2)

    # Compute elbow point E from arm angle theta0
    E = C + r * (math.cos(theta0) * e1 + math.sin(theta0) * e2)

    return E

This is the geometric core of the entire algorithm.

3.3 Step 3: Analytically Solving All Joint Angles from S–E–W

3.3.1 Shoulder Joints: q1,q2,q3

The paper derives a direct closed-form solution using geometric projection:

• q1 is obtained from the projection of point E onto the base plane
• q2 is determined by the height of E
• q3 is solved from the direction of the wrist relative to the elbow

Code: _solve_q123_from_swe

def _solve_q123_from_swe(E: np.ndarray, W: np.ndarray, q4: float, p: NeroParams) -> List[np.ndarray]:
    d0 = p.d_i[0]
    d2 = p.d_i[2]
    d4 = p.d_i[4]

    Ex, Ey, Ez = E

    # q2
    c2 = (Ez - d0) / d2
    c2 = np.clip(c2, -1.0, 1.0)

    s2_abs = math.sqrt(max(0.0, 1.0 - c2**2))

    s4 = math.sin(q4)
    c4 = math.cos(q4)

    sols = []

    # Traverse both positive and negative s2 configurations
    for s2 in (s2_abs, -s2_abs):

        # q1
        c1 = -Ex / (d2 * s2)
        s1 = -Ey / (d2 * s2)

        n1 = math.hypot(c1, s1)

        c1 /= n1
        s1 /= n1

        q1 = math.atan2(s1, c1)
        q2 = math.atan2(s2, c2)

        # q3
        v = W - E
        col2 = -v / d4

        u1, u2, u3 = col2

        b1 = (s2 * c1 * c4 - u1) / s4
        b2 = (u2 - s1 * s2 * c4) / s4

        s3 = s1 * b1 + c1 * b2
        c2c3 = -c1 * b1 + s1 * b2

        c3 = c2c3 / c2 if abs(c2) > 1e-8 else (u3 + c2 * c4) / (s2 * s4)

        n3 = math.hypot(s3, c3)

        s3 /= n3
        c3 /= n3

        q3 = math.atan2(s3, c3)

        sols.append(np.array([q1, q2, q3]))

    return sols

3.3.2 Wrist Joints: q5,q6,q7

The paper analytically extracts the wrist joint angles directly from the transformation matrix T_{47}

• cos \theta_6 = T_{47}[1,2]
• \theta_5 and \theta_7 are computed from neighboring matrix element ratios

Code: _extract_567_from_T47_paper

def _extract_567_from_T47_paper(T47: np.ndarray) -> List[np.ndarray]:
    sols = []

    c6 = np.clip(T47[1, 2], -1.0, 1.0)

    for sgn in (1.0, -1.0):

        s6 = sgn * math.sqrt(max(0.0, 1.0 - c6**2))

        if abs(s6) < 1e-8:
            continue

        th6 = math.atan2(s6, c6)

        th5 = math.atan2(T47[2, 2] / s6, T47[0, 2] / s6)

        th7 = math.atan2(T47[1, 1] / s6, -T47[1, 0] / s6)

        sols.append(np.array([th5, th6, th7]))

    return sols

---

3.4 Step 4: Joint Limits → Feasible Region of the Arm Angle

Theory from the Paper

Each joint limit interval [q_{min},q_{max}] corresponds to a certain invalid region of the arm angle.

The intersection of all valid intervals yields the feasible arm-angle region \Psi_F.

Only arm angles within this feasible region guarantee that all joints remain inside their limits.

Code: _get_theta0_feasible_region

def _get_theta0_feasible_region(T07: np.ndarray, p: NeroParams, step: float = 0.01) -> List[float]:
    feasible = []

    for theta0 in np.arange(-math.pi, math.pi, step):

        if _ik_one_arm_angle(T07, theta0, p):
            feasible.append(float(theta0))

    return feasible

Internally, the function calls _ik_one_arm_angle, which performs the following steps:

• Substitute the arm angle \psi
• Solve the complete joint configuration
• Check whether all joints satisfy their limits
• If valid → add the arm angle to the feasible region

---

3.5 Step 5: Optimal Arm-Angle Selection (Weighted Quadratic Objective Function)

Theory from the Paper

The objective function is defined as:
f(\psi) = \sum w_i(q_i(\psi)-q_{i,prev})^2

• w_i:Weight coefficient, which increases as the corresponding joint approaches its mechanical limit.
• Objective: To minimize the overall joint motion while keeping all joints as far as possible from their limits.

Weighting Function (Equation 20 in the Paper)

• w_i=\frac{bx}{e_{a(1-x)-1}},x ≥ 0
• w_i=\frac{-bx}{e_{a(1-x)-1}},x \lt 0

Where

• a=2.28
• b=2.28

Code: _weight_limits

def _weight_limits(q: float, q_min: float, q_max: float) -> float:
    span = q_max - q_min

    x = 2.0 * (q - (q_min + q_max) * 0.5) / span

    a = 2.38
    b = 2.28

    if x >= 0:
        den = math.exp(a * (1 - x)) - 1
        return b * x / den
    else:
        den = math.exp(a * (1 + x)) - 1
        return -b * x / den

Optimal Arm-Angle Search

def _optimal_theta0(feasible_theta0, T07, p, q_prev):

    best_cost = inf
    best_t = feasible_theta0[0]

    for t in feasible_theta0:

        sols = _ik_one_arm_angle(T07, t, p)

        for q_full in sols:

            q = q_full[:7]

            cost = 0

            for i in range(7):

                lo, hi = p.joint_limits[i]

                w = _weight_limits(q[i], lo, hi)

                dq = abs(q[i] - q_prev[i])

                cost += w * dq * dq

            if cost < best_cost:
                best_cost = cost
                best_t = t

    return best_t

This is the optimal solution selection strategy proposed in the paper.

In essence, it transforms the problem into:

One-dimensional quadratic-function minimization → globally optimal solution → no iterative solving and no local minima.

---

Part 4.Null-Space Motion Principle (Naturally Embedded)

For a 7-DoF manipulator, the null space is directly controlled by the arm angle \psi.

The principle is straightforward:

• The end-effector pose T_{07} remains unchanged
• Only the arm angle ψ is varied
• The robot joints automatically perform self-reconfiguration while keeping the end-effector fixed
  This is known as null-space motion.

In the implementation, null-space motion can be generated simply by sweeping the arm angle:

for psi in np.linspace(-pi, pi, 100):
    q = _q_from_theta0(psi, T07, p)

No Jacobian matrix is required,
no projection operator is needed,
and the motion remains smooth and stable without oscillation.

---

Part 5.Code Structure Overview (Clean Version)

Core Functions in ik_solver.py(链接)

---

Part 6.Quick Start Guide

import numpy as np
from ik_solver import ik_arm_angle, NeroParams

# Define target end-effector pose
T = np.eye(4)
T[:3, 3] = [0.5, 0.0, 0.5]

# Solve inverse kinematics
q_best, feasible_set = ik_arm_angle(T)

print("Optimal joint configuration:", q_best)
print("Number of feasible arm angles:", len(feasible_set))

---

Part 7.Summary
This method presents a closed-form inverse kinematics solver for a 7-DoF S–R–S robotic manipulator, combined with a 1D quadratic optimization over the arm-angle null space.

Key characteristics:

1.Pure geometric closed-form solution

• No iterative optimization
• No Jacobian-based numerical solving

2.Automatic joint limit compliance

• Feasible region explicitly constrained

3.Optimality guaranteed via quadratic cost function

• Efficient 1D optimization over arm angle

4.Natural support for null-space motion

• Arm angle acts as redundancy parameter

5.Real-time performance

• Extremely fast computation suitable for control loops and embodied systems