How to Build a Robot Arm IK Solver in ROS2 | NERO Arm Parame

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 ψ (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 ψ.

• 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 elbow angle ψ determines the position of point E on the circle.

In one sentence:

ψ→ elbow posture changes → joint angles change → end-effector remains unchanged

2.3 Differences Between This Method and Traditional Numerical IK Solvers

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 q1q3 and q5q7

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 θ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 d6)
• Elbow joint angle θ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

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 ψ (Core Geometry)

Theory from the Paper

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

Where:

• C: circle center
• r: circle radius
• e1, e2: 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}

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 Ψ_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 ψ
• 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:

Weighting Function (Equation 20 in the Paper)

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 ψ.

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