Inverse Kinematics for Any URDF: Tasks, Weights, and Solver Design
Published:
This post supports English / 中文 switching via the site language toggle in the top navigation.
TL;DR
For a robot described by any valid URDF, inverse kinematics should be approached as a weighted nonlinear least-squares problem built on top of forward kinematics and Jacobians. The critical design choices are not the exact link names in the model, but the task definition, the relative weights between translation and rotation, the damping and regularization terms that stabilize the solve, and the iterative update rule that keeps the solver well behaved in a real control loop.
1. Problem Formulation
Once a URDF has been parsed into a kinematic model, the IK problem is conceptually uniform across robots. A solver receives one or more target poses, evaluates the current pose of the corresponding frames, computes task-space errors, and finds a joint update that reduces those errors. The code below shows the essential pattern: a frame pose is extracted from the current configuration, a relative pose error is computed in SE(3), and a frame Jacobian maps joint-space motion to task-space motion.
def _pose_task(self, q: np.ndarray, frame_id: int, target: pin.SE3):
pin.forwardKinematics(self.model, self.data, q)
pin.updateFramePlacements(self.model, self.data)
current = self.data.oMf[frame_id]
err = pin.log(current.actInv(target)).vector
J = pin.computeFrameJacobian(
self.model, self.data, q, frame_id, pin.ReferenceFrame.LOCAL
)
return J, err
This abstraction is the reason a single solver architecture can be reused across many URDFs. The specific frame identifiers may differ from one robot to another, but the mathematical structure is unchanged: define a target frame, compute its local pose error, compute its Jacobian, and reduce that error iteratively.
2. Task Stacking Is the Core Design Choice
A practical IK system rarely solves only one task. The more general formulation is to stack several tasks with different priorities into a single optimization problem. In the provided script, two end-effector pose targets are treated as the main objectives, while two intermediate-link targets act as weaker shaping terms. That is more informative than a robot-specific implementation detail because it reveals how to structure IK for arbitrary models: isolate the behavior that must be satisfied, then add softer tasks that improve posture, coordination, or motion quality without dominating the main objective.
The implementation makes this explicit by computing one Jacobian and one error vector per task, then assembling them into one large system. The relevant code is short:
J_lm, e_lm = self._pose_task(q, self.main_frames["left"], left_main)
J_rm, e_rm = self._pose_task(q, self.main_frames["right"], right_main)
J_ls, e_ls = self._pose_task(q, self.secondary_frames["left"], left_secondary)
J_rs, e_rs = self._pose_task(q, self.secondary_frames["right"], right_secondary)
J_stack = np.vstack([J_lm, J_rm, J_ls, J_rs])
e_stack = np.hstack([e_lm, e_rm, e_ls, e_rs])
This stacked form is more important than the dual-arm setting itself. For any URDF, the right question is not “what is the exact link name,” but “which frame errors belong in the objective, and how strongly should each one influence the update.” This becomes especially important when the robot has more than six degrees of freedom in an arm chain. A 7-DOF arm can satisfy the same end-effector pose with infinitely many joint configurations along its null space, so a single pose target is often not enough to determine the motion you actually want. In practice, a secondary target is often used to guide that redundancy toward a meaningful posture, such as keeping an elbow lifted, preventing it from collapsing inward, or shaping the arm into a natural configuration.
3. Weights Define the Semantics of the Solve
Once multiple tasks are present, the solver needs a way to express their relative importance. Weights are the mechanism that encode this. In the shared code, position and orientation are weighted separately, and primary tasks are assigned much larger values than secondary ones. That design is not cosmetic tuning; it tells the optimizer what counts as success.
@dataclass
class IKSolverWeights:
main_pos: float = 100.0
main_rot: float = 100.0
secondary_pos: float = 5
secondary_rot: float = 0.5
damping: float = 1e-3
q_reg: float = 1e-4
The weighting is then applied directly to both the Jacobian rows and the corresponding error terms:
@staticmethod
def _apply_pose_weights(J, err, w_pos, w_rot):
w = np.array([w_pos, w_pos, w_pos, w_rot, w_rot, w_rot], dtype=float)
return w[:, None] * J, w * err
This separation between translational and rotational weights is particularly important in practice. Pose errors live in mixed units and mixed semantics. A one-centimeter position error and a small angular error are not directly comparable, either numerically or behaviorally. A technically sound IK formulation should therefore avoid treating all six components as if they were interchangeable. The weights should reflect application intent: end-effector position may dominate orientation in one task, while tool orientation may be the primary objective in another.
4. Damping and Regularization Make the Optimization Usable
An undamped least-squares solve is often too fragile for real robots, especially near singular configurations or when tasks compete. The script stabilizes the normal equations with two additional terms: a damping term that improves numerical conditioning and a joint regularization term that biases the solution toward a reference posture. The resulting system is still simple, but much more robust:
H = J_stack.T @ J_stack
H += self.weights.damping * np.eye(self.model.nv)
H += self.weights.q_reg * np.eye(self.model.nv)
g = J_stack.T @ e_stack - self.weights.q_reg * (q - self.q_ref)
dq = np.linalg.solve(H, g)
From an optimization perspective, this is the part that turns a clean derivation into a practical solver. Damping suppresses pathological updates when the Jacobian becomes poorly conditioned. Joint regularization prevents unnecessary drift in redundant systems and helps the solver stay near a reasonable posture when many joint configurations produce similar task-space behavior. For URDF-agnostic IK, these terms are essential because a general-purpose solver cannot assume that every robot will have convenient geometry or well-separated tasks.
5. Iterative Integration Matters More Than Closed-Form Ambition
The update rule is equally important. After solving for a joint increment, the script does not jump directly to a final answer; it integrates a scaled update and repeats for a bounded number of iterations. This is exactly the right design for online IK:
q = pin.integrate(self.model, q, step_scale * dq)
q = np.clip(q, self.lower, self.upper)
if np.linalg.norm(e_stack) < 1e-4:
break
The step scale controls solver aggressiveness. If the update is too large, the linearization is trusted too far away from the current state and the iteration may oscillate or overshoot. If it is too small, convergence becomes slow. The bounded iterative loop is therefore not an implementation detail but a core part of the method. In a real control pipeline, the current solution serves as the warm start for the next cycle, and solver quality depends heavily on whether each incremental step is stable and predictable.
6. Joint Limits Must Be Treated as Part of the Solver
URDF-based IK is not complete if it ignores joint bounds. The script clips the configuration after every integrated update, which is a simple but effective safeguard. More elaborate constrained formulations are possible, but the conceptual point is the same: an invalid configuration is not an acceptable intermediate state just because the task-space residual is small.
This is also why solver evaluation should not focus only on end-effector accuracy. A numerically small residual is not enough if the solution lives on the edge of a singularity, runs into limits continuously, or requires large oscillatory corrections from one iteration to the next. For a reusable IK architecture, robustness criteria must include feasibility and numerical behavior, not only task error.
The same argument applies to redundancy resolution. For a 7-DOF arm, feasibility is not only about reaching the end-effector pose, but also about selecting one posture out of infinitely many possibilities in the null space. In bimanual systems, this matters even more because mirror-symmetric pose targets can still produce visually awkward or mechanically undesirable elbow placements. A secondary objective on an intermediate frame is therefore not just a refinement; it is often the mechanism that makes the final motion look deliberate and physically natural.
7. What General IK for Arbitrary URDFs Should Look Like
A technically sound IK pipeline for arbitrary URDFs should therefore be organized around a consistent sequence. It should parse the robot model, choose the task frames, represent targets in SE(3), compute frame-level pose errors and Jacobians, scale those errors according to task semantics, solve a damped and regularized least-squares system, integrate a conservative joint update, enforce limits, and iterate until the residual is small or the iteration budget is exhausted. The underlying idea is generic even when the robot is not.
The main lesson from the code is not tied to this particular dual-arm example. The transferable insight is that inverse kinematics becomes general once it is phrased in terms of task construction and solver design rather than robot-specific casework. If the task stack is well chosen, the weights reflect the intended behavior, and the numerical update is stabilized properly, the same architecture can be adapted to a large range of URDF-defined robots with minimal conceptual change.