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

Continuous 7DoF IK Using Arm-Angle Parameterization for the NERO Robotic Arm

A closed-form 7DoF IK solver with continuous null-space control and no Jacobian iteration.

We implemented a real-time analytical inverse kinematics solver for the NERO 7DoF robotic arm using arm-angle parameterization and geometric redundancy resolution.

Unlike traditional Jacobian-based IK solvers, this approach provides:

• continuous elbow motion
• stable null-space control
• no iterative optimization
• no local minima
• real time solving performance

The entire redundancy of the manipulator is reduced into a single arm-angle parameter Ψ, enabling smooth self-motion while keeping the end-effector pose fixed.

This tutorial covers:

• geometric IK derivation
• SWE triangle modeling
• analytical q1–q7 solving
• joint-limit optimization
• null-space motion generation
• ROS2 integration
• MoveIt2 simulation workflow

Reference & Resources

• Related paper:
  https://jst.tsinghuajournals.com/article/2020/4286/20201206.htm
• Open-source implementation:
  https://github.com/agilexrobotics/robotic_arm_kinematics
• NERO robotic arm platform:
  https://global.agilex.ai/products/nero

1. From Multiple Solutions to Controllable Solutions: A Parametric Approach to 7-DoF Inverse Kinematics

Why is parameterized IK needed?

The main difference between a 7-DoF arm and a 6-DoF arm is not just that it has one extra joint.The real difference is that one end-effector pose can map to multiple joint configurations.

When the end-effector pose is fixed, the joint angles are still not unique — there are infinitely many valid solutions.This kind of motion, where the end-effector stays still while the arm reconfigures itself, is called null-space motion .

This means IK is no longer about finding a unique solution, The problem becomes: which solution should we choose?

To handle this, the redundant degree of freedom is isolated and represented by a single parameter ψ , which controls the elbow posture.

Elbow-Angle Parameterization in IK

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

When the end-effector pose is fixed, S and W are fixed in space, and E moves along a circle in 3D.
The angle on this circular motion is defined as the elbow angle ψ .

In one sentence:

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

Differences Between This Method and Traditional Numerical IK Solvers

2.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))

3.Complete Workflow of the Parametric IK Solver

This IK solver follows a parameterized analytical pipeline:

• Extract S, W, and 𝜃4 from the target pose.
• Compute the elbow point E from the elbow angle ψ, and analytically solve 𝑞1⁢𝑞3 and 𝑞5⁢𝑞7
• Compute the feasible region of the elbow angle under all joint-limit constraints
• 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.

Step 0:Code Structure Overview (Clean Version)

Core Functions in ik_solver.py

Step 1: Extract the geometric anchors

1.Goal

Given the target end-effector pose T07 , we first extract the geometric anchors needed by the analytical IK solver.

2.Geometric Definition

As shown in the figure below:

• B: Base coordinate system
• S: Shoulder center
• W: Wrist center
• E: Elbow center

Given the end-effector pose 𝑇07, we first solve for:

• S: Shoulder point
• W: Wrist point (obtained by offsetting the end-effector frame backward by 𝑑6)
• 𝜃4:Elbow joint angle (uniquely determined from the S–E–W triangle using the law of cosines)

From the triangle S–E–W, the elbow angle θ4 is determined by the side lengths:

3.Code Implementation

Parameter Computation: _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

At this stage, the problem is reduced from a full pose IK task to a pure geometric computation on the S–E–W triangle.

Step 2: Solving the Elbow Point E from the Arm Angle 𝜓 (Core Geometry)

1. Goal

Once S and W are fixed, the elbow point E moves on a circle in 3D space.
The arm angle ψ is used to select a specific elbow posture on that circle.

2. Geometric Interpretation

The elbow point lies on a circle whose chord is defined by S–W:
𝐸 =𝐶 +𝑟⁢(𝑐⁢𝑜⁢𝑠⁢𝜓 ∗𝑒1 +𝑠⁢𝑖⁢𝑛⁢𝜓 ∗𝑒2)

Where:

• C: circle center
• r: circle radius
• 𝑒1,𝑒2: orthonormal basis vectors spanning the circle plane

3. Code Implementation

Point E Computation: _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.

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

3.1.1 Shoulder Joints: 𝑞⁢1,𝑞⁢2,𝑞⁢3

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

• 𝑞⁢1 is obtained from the projection of point E onto the base plane
• 𝑞⁢2 is determined by the height of E
• 𝑞⁢3 is solved from the direction of the wrist relative to the elbow

3.1.2 Code Implementation

Configuration Solutions :_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.2.1 Wrist Joints: 𝑞⁢5,𝑞⁢6,𝑞⁢7

The paper analytically extracts the wrist joint angles directly from the transformation matrix 𝑇47

• 𝑐⁢𝑜⁢𝑠⁢𝜃6 =𝑇47⁡[1,2]
• 𝜃5 and 𝜃7 are computed from neighboring matrix element ratios

3.2.2 Code Implementation

Configuration Solutions : _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

Step 4: Compute the Feasible Elbow-Angle Region

1. Goal

Not every elbow angle ψ produces a valid joint configuration.
Even if the end-effector pose is fixed, some ψ values will push one or more joints beyond their mechanical limits.
So before selecting the final solution, we first build the set of feasible elbow angles.

2.Code Implementation

Feasible Region Computation: _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

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

1.Goal

After the feasible elbow-angle region has been obtained, we still need to choose the best solution among all valid candidates.

The objective function is defined as:

𝑤𝑖: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.

Joint-Limit Penalty Design:Weighting Function (Equation 20 in the Paper)

Where

• a=2.28
• b=2.28

2.Code Implementation

Weight Computation: _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:_optimal_theta0

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.

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

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