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
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:
- Joint limit avoidance
- Obstacle avoidance
- Elbow posture optimization
- 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:
- Extract S, W, and θ_4 from the target pose.
- Compute the elbow point E from the elbow angle ψ, and analytically solve q_1q_3 and q_5q_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.
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
