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

**URL:** <https://discourse.openrobotics.org/t/how-to-build-a-robot-arm-ik-solver-in-ros2-nero-arm-parametric-inverse-kinematics/54740>\
**Category:** ROS General\
**Created:** [May 12, 2026, 6:51am UTC](https://discourse.openrobotics.org/t/how-to-build-a-robot-arm-ik-solver-in-ros2-nero-arm-parametric-inverse-kinematics/54740 "2026-05-12T06:51:06Z")\
**Posts on this page:** 2\
**Page:** 1

<div class="post-metadata">

**Author:** ![Agilex\_Robotics](https://sea2.discourse-cdn.com/flex022/user_avatar/discourse.openrobotics.org/agilex_robotics/32/10554_2.png) [@Agilex\_Robotics](https://discourse.openrobotics.org/u/Agilex_Robotics)\
**Post date:** [May 12, 2026, 6:51am UTC](https://discourse.openrobotics.org/t/how-to-build-a-robot-arm-ik-solver-in-ros2-nero-arm-parametric-inverse-kinematics/54740/1 "2026-05-12T06:51:06Z")

</div>

# 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](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**  
 ![image](https://us1.discourse-cdn.com/flex022/uploads/ros/original/3X/3/8/38f22b78dbc7551588380519b11d6b8394472803.jpeg)

**Law of Cosines**

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

### Code Implementation: `_compute_swe_from_target`

```Python
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`

```Python
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`

```Python
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`

```Python
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`

```Python
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`

```Python
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`

```Python
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

```Python
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:

```Python
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`(链接)

 ![image](https://us1.discourse-cdn.com/flex022/uploads/ros/original/3X/5/0/50329d93c8725e118040c20f9e26fbae94894478.png)

* * *

## Part 6.Quick Start Guide

```Python
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

---

<div class="post-metadata">

**Author:** ![Agilex\_Robotics](https://sea2.discourse-cdn.com/flex022/user_avatar/discourse.openrobotics.org/agilex_robotics/32/10554_2.png) [@Agilex\_Robotics](https://discourse.openrobotics.org/u/Agilex_Robotics)\
**Post date:** [May 13, 2026, 9:16am UTC](https://discourse.openrobotics.org/t/how-to-build-a-robot-arm-ik-solver-in-ros2-nero-arm-parametric-inverse-kinematics/54740/2 "2026-05-13T09:16:40Z")

</div>

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

![imgv30211le7c24029-3a9f-41e9-99a7-b33c442c414g](https://us1.discourse-cdn.com/flex022/uploads/ros/original/3X/e/7/e7d7b3cd67858aa3bcc98c0c7f8fa6355cc66602.gif)

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 \Psi, 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](https://jst.tsinghuajournals.com/article/2020/4286/20201206.htm)
- Open-source implementation:  
[https://github.com/agilexrobotics/robotic\_arm\_kinematics](https://github.com/agilexrobotics/robotic_arm_kinematics)
- NERO robotic arm platform:  
[https://global.agilex.ai/products/nero](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 \psi 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:

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

### 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 |

* * *

## 2.Quick Start Guide

```Python
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** :

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.

* * *

### Step 0:Code Structure Overview (Clean Version)

#### Core Functions in [ik\_solver.py](https://github.com/vanstrong12138/robotic_arm_kinematics/blob/main/nero_ik/ik_solver.py)

 ![image](https://us1.discourse-cdn.com/flex022/uploads/ros/original/3X/6/9/691eb1e08877bfd81f0853ef9babc7f5637ce9ea.png)

* * *

### 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

 ![image](https://us1.discourse-cdn.com/flex022/uploads/ros/original/3X/3/8/38f22b78dbc7551588380519b11d6b8394472803.jpeg)

Given the end-effector pose T\_{07}, we first solve for:

- **S** : Shoulder point
- **W** : Wrist point (obtained by offsetting the end-effector frame backward by d\_6)
- **\theta\_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:

> cos \theta\_4 = \frac{||SW||^2-||SE||^2-||EW||^2}{2||SE|| ||EW||}

#### 3.Code Implementation

**Parameter Computation: `_compute_swe_from_target`**

```Python
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`**

```Python
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 \psi (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`:  
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

#### 3. Code Implementation

**Point E Computation: `_elbow_from_arm_angle`**

```Python
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: 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

#### 3.1.2 Code Implementation

**Configuration Solutions :`_solve_q123_from_swe`**

```Python
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: 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

**3.2.2 Code Implementation**

**Configuration Solutions : `_extract_567_from_T47_paper`**

```Python
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`**

```Python
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

* * *

### 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:  
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.

Joint-Limit Penalty Design: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

#### 2.Code Implementation

**Weight Computation: `_weight_limits`**

```Python
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`**

```Python
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 \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:

```Python
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