Inverse Kinematics ================== RoboPlan provides two inverse kinematics solvers: a simple Jacobian-based solver and an optimal solver based on quadratic programming. SimpleIK: Jacobian-Based Solver -------------------------------- SimpleIK is a lightweight inverse kinematics solver using the damped least squares (DLS) method, also known as the Levenberg-Marquardt algorithm. Algorithm ^^^^^^^^^ At each iteration, SimpleIK computes joint velocities that minimize the Cartesian error: .. math:: \dot{q} = -J^T (J J^T + \lambda I)^{-1} e Where: - :math:`e = \log_6(T_{\text{goal}}^{-1} T_{\text{current}})` — 6D Cartesian error (position + orientation) - :math:`J` — Frame Jacobian in ``LOCAL`` reference frame - :math:`\lambda` — Damping factor for regularization The joint configuration is updated via integration: .. math:: q \leftarrow q \oplus (\dot{q} \cdot \text{step\_size}) **Properties:** - Simple and efficient — minimal computational overhead - Supports multiple simultaneous goal frames - Collision checking with random restarts on failure - Convergence monitoring based on separate linear and angular error thresholds - Optionally attempt to find a nearest solution to the seed until the timeout is reached Configuration ^^^^^^^^^^^^^ +-----------------------------+--------------------------------------+-----------+ | Parameter | Description | Default | +=============================+======================================+===========+ | ``group_name`` | Joint group to control | "" | +-----------------------------+--------------------------------------+-----------+ | ``max_iters`` | Maximum iterations per attempt | 100 | +-----------------------------+--------------------------------------+-----------+ | ``max_time`` | Maximum computation time (seconds) | 0.005 | +-----------------------------+--------------------------------------+-----------+ | ``max_restarts`` | Number of random restarts on failure | 2 | +-----------------------------+--------------------------------------+-----------+ | ``step_size`` | Integration step size | 0.25 | +-----------------------------+--------------------------------------+-----------+ | ``damping`` | Damping factor :math:`\lambda` | 0.001 | +-----------------------------+--------------------------------------+-----------+ | ``max_linear_error_norm`` | Convergence threshold (meters) | 0.001 | +-----------------------------+--------------------------------------+-----------+ | ``max_angular_error_norm`` | Convergence threshold (radians) | 0.001 | +-----------------------------+--------------------------------------+-----------+ | ``check_collisions`` | Enable collision checking | true | +-----------------------------+--------------------------------------+-----------+ | ``fast_return`` | Return the first solution found. | true | +-----------------------------+--------------------------------------+-----------+ Usage Example ^^^^^^^^^^^^^ .. code-block:: python import numpy as np from roboplan.core import Scene, JointConfiguration, CartesianConfiguration from roboplan.simple_ik import SimpleIkOptions, SimpleIk # Setup scene = Scene("robot", urdf_path, srdf_path, package_paths) options = SimpleIkOptions( group_name="arm", step_size=0.25, max_linear_error_norm=0.001, max_angular_error_norm=0.001, check_collisions=True ) ik_solver = SimpleIk(scene, options) # Define goal goal = CartesianConfiguration() goal.base_frame = "base" goal.tip_frame = "tool0" goal.tform = target_transform # 4x4 SE(3) matrix # Solve start = JointConfiguration() start.positions = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) # initial configuration solution = JointConfiguration() success = ik_solver.solveIk(goal, start, solution) OInK: Optimal Inverse Kinematics --------------------------------- The OInK solver uses Quadratic Programming (QP) to compute joint displacements that achieve multiple objectives while respecting constraints and safety barriers. QP Problem Formulation ^^^^^^^^^^^^^^^^^^^^^^ OInK solves the following QP at each control step: .. math:: \min_{\Delta q} \quad \underbrace{\frac{1}{2} \sum_{k} \| W_k (J_k N_k \Delta q + \alpha_k e_k) \|^2}_{\text{Tasks}} + \underbrace{\frac{\lambda}{2} \|\Delta q\|^2}_{\text{Regularization}} + \underbrace{\sum_{b} \frac{r_b}{2\|J_b\|^2} \|\Delta q - \Delta q_{\text{safe}}\|^2}_{\text{Barrier Regularization}} Subject to: .. math:: \underbrace{l \leq G_c \Delta q \leq u}_{\text{Hard Constraints}} \quad \text{and} \quad \underbrace{G_b \Delta q \leq h_b}_{\text{Barrier Constraints}} Reformulated as: .. math:: \min_{\Delta q} \quad \frac{1}{2} \Delta q^T H \Delta q + c^T \Delta q Where: .. math:: H = \lambda I + \sum_k (N_k^T J_k^T W_k^T W_k J_k N_k + \mu_k I) + \sum_b \frac{r_b}{\|J_b\|^2} I .. math:: c = \sum_k (-\alpha_k N_k^T J_k^T W_k^T W_k e_k) + \sum_b \frac{-r_b}{\|J_b\|^2} \Delta q_{\text{safe}} +---------------------+----------------------------------------------+--------------+ | Symbol | Description | Source | +=====================+==============================================+==============+ | :math:`\Delta q` | Joint displacement (decision variable) | — | +---------------------+----------------------------------------------+--------------+ | :math:`J_k, e_k,` | Task Jacobian, error, weight matrix | Tasks | | :math:`W_k` | | | +---------------------+----------------------------------------------+--------------+ | :math:`\alpha_k` | Task gain (low-pass filter) | Tasks | +---------------------+----------------------------------------------+--------------+ | :math:`N_k` | Cumulative nullspace projector for | Tasks | | | priority level :math:`k` (:math:`N_1 = I`) | (priority) | +---------------------+----------------------------------------------+--------------+ | :math:`\mu_k` | Levenberg-Marquardt damping, | Tasks | | | :math:`\mu_k = \lambda_{\text{LM},k} | | | | \|W_k \alpha_k e_k\|^2` | | +---------------------+----------------------------------------------+--------------+ | :math:`\lambda` | Tikhonov regularization | Solver | +---------------------+----------------------------------------------+--------------+ | :math:`G_c, l, u` | Hard constraint matrix and bounds | Constraints | +---------------------+----------------------------------------------+--------------+ | :math:`G_b, h_b` | Barrier constraint matrix and bounds | Barriers | +---------------------+----------------------------------------------+--------------+ | :math:`r_b, J_b` | Safe displacement gain and barrier | Barriers | | | Jacobian | | +---------------------+----------------------------------------------+--------------+ Task Priorities and Nullspace Projection ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Each task carries an integer ``priority`` (default ``1`` = highest). Tasks at a lower priority level (higher priority *number*) are projected into the nullspace of all higher-priority tasks, so they cannot fight tasks above them. Their contribution is structurally zero in the higher-priority directions. For each priority level :math:`k`, the QP uses a *projected* Jacobian :math:`J_k N_k`, where :math:`N_k` is the cumulative nullspace projector built from the row-stacked Jacobians of all priority levels :math:`1, \ldots, k-1`. :math:`N_1 = I` (no projection at the top level), so a single-priority problem reduces to the standard weighted-sum QP. The projector is computed via a damped pseudoinverse: .. math:: N_k = I - J_{\text{stack}}^T \left( J_{\text{stack}} J_{\text{stack}}^T + \lambda I \right)^{-1} J_{\text{stack}} where :math:`J_{\text{stack}}` is the vertical stack of all priority-level Jacobians strictly above :math:`k`, and the same Tikhonov regularization :math:`\lambda` from the QP is reused as the damping. The damping keeps :math:`(J J^T + \lambda I)` SPD even at singular configurations; at well-conditioned configurations (singular values :math:`\gg \sqrt{\lambda}`), the expression reduces to the standard nullspace projector :math:`I - J^+ J`. Tasks **at the same priority level** are combined linearly through their weights (no projection between them). The decision variable remains :math:`\Delta q`; only the per-task Jacobian is projected. Tasks ^^^^^ Tasks define optimization objectives through Jacobians and errors. FrameTask """"""""" Tracks a target 6-DOF pose (position + orientation). **Error computation:** .. math:: e_{\text{pos}} = p_{\text{target}} - p_{\text{current}} .. math:: e_{\text{rot}} = R_{\text{current}} \cdot \log_3(R_{\text{current}}^T R_{\text{target}}) **Error saturation** (prevents large jumps that invalidate CBF linearization): .. math:: e_{\text{saturated}} = e_{\max} \cdot \tanh\left(\frac{\|e\|}{e_{\max}}\right) \cdot \frac{e}{\|e\|} **Jacobian:** Frame Jacobian in ``LOCAL_WORLD_ALIGNED`` coordinates, negated so QP moves toward target. **Weight matrix:** .. math:: W = \text{diag}(\sqrt{w_{\text{pos}}} \cdot I_3, \sqrt{w_{\text{rot}}} \cdot I_3) +--------------------------+-----------------------------------+-----------+ | Parameter | Description | Default | +==========================+===================================+===========+ | ``position_cost`` | Position error weight | 1.0 | +--------------------------+-----------------------------------+-----------+ | ``orientation_cost`` | Orientation error weight | 1.0 | +--------------------------+-----------------------------------+-----------+ | ``task_gain`` | Low-pass filter gain | 1.0 | | | :math:`\alpha` | | +--------------------------+-----------------------------------+-----------+ | ``lm_damping`` | Levenberg-Marquardt damping | 0.0 | +--------------------------+-----------------------------------+-----------+ | ``max_position_error`` | Saturation limit (meters) | ∞ | +--------------------------+-----------------------------------+-----------+ | ``max_rotation_error`` | Saturation limit (radians) | ∞ | +--------------------------+-----------------------------------+-----------+ | ``priority`` | Priority level (1 = highest; | 1 | | | see Task Priorities) | | +--------------------------+-----------------------------------+-----------+ ConfigurationTask """"""""""""""""" Drives toward a target joint configuration (null-space regularization). **Error:** Manifold-aware difference: :math:`e = \text{difference}(q, q_{\text{target}})` **Jacobian:** :math:`J = -I` (negative identity) **Weight matrix:** :math:`W = \text{diag}(\sqrt{w_1}, \ldots, \sqrt{w_{n_v}})` Also accepts ``task_gain``, ``lm_damping``, and ``priority`` (defaults match ``FrameTaskOptions``). A common pattern is to use a ConfigurationTask at a lower priority level (e.g. ``priority=2``) as a posture / null-space regularizer that will not interfere with a higher-priority FrameTask. Constraints vs Barriers ^^^^^^^^^^^^^^^^^^^^^^^ Hard Constraints """""""""""""""" Hard constraints enforce **strict bounds** that the QP solver cannot violate: .. math:: G \cdot \Delta q \leq h **Properties:** - Exact enforcement — the solution always satisfies the constraint - State-independent bounds — same restriction regardless of distance to limit - No Jacobian needed for joint-space constraints - QP becomes infeasible if constraints conflict **Use for:** Physical actuator limits, joint position/velocity bounds Control Barrier Functions (Barriers) """"""""""""""""""""""""""""""""""""" Barriers enforce **forward invariance** of a safe set through a differential condition: .. math:: \dot{h}(q) + \alpha(h(q)) \geq 0 In discrete time: .. math:: \frac{J_h \cdot \Delta q}{\Delta t} + \alpha(h(q)) \geq 0 \quad \Rightarrow \quad -J_h \cdot \Delta q \leq \Delta t \cdot \alpha(h(q)) **Properties:** - State-dependent bounds — more freedom far from boundary, tighter near it - Smooth behavior — graceful slowdown instead of abrupt stop - Requires Jacobian computation - Always feasible (soft constraint via class-K function) - Subject to linearization error in discrete time **Use for:** Cartesian position bounds, collision avoidance, workspace constraints Comparison """""""""" +------------------------+------------------------------------+--------------------------------------+ | Aspect | Hard Constraint | Barrier (CBF) | +========================+====================================+======================================+ | **Formulation** | :math:`\Delta q \leq` constant | :math:`J \cdot \Delta q \leq` | | | | :math:`f(\text{distance})` | +------------------------+------------------------------------+--------------------------------------+ | **Near boundary** | Same restriction | Tighter (slows down) | +------------------------+------------------------------------+--------------------------------------+ | **Far from boundary** | Same restriction | Looser (more freedom) | +------------------------+------------------------------------+--------------------------------------+ | **Enforcement** | Exact | Approximate (linearization) | +------------------------+------------------------------------+--------------------------------------+ | **Feasibility** | Can fail | Always feasible | +------------------------+------------------------------------+--------------------------------------+ | **Behavior** | Abrupt at limit | Smooth approach | +------------------------+------------------------------------+--------------------------------------+ | **Computation** | Simple | Requires Jacobian | +------------------------+------------------------------------+--------------------------------------+ Constraint Details ^^^^^^^^^^^^^^^^^^ VelocityLimit """"""""""""" Enforces maximum joint velocities as hard bounds: .. math:: -\Delta t \cdot v_{\max} \leq \Delta q \leq \Delta t \cdot v_{\max} PositionLimit """"""""""""" Restricts motion based on distance to joint limits: .. math:: -\gamma (q - q_{\min}) \leq \Delta q \leq \gamma (q_{\max} - q) The gain :math:`\gamma \in (0, 1]` controls aggressiveness. As :math:`q \to q_{\max}`, the upper bound :math:`\to 0`. Barrier Details ^^^^^^^^^^^^^^^ PositionBarrier """"""""""""""" Keeps a frame within an axis-aligned bounding box using CBF constraints. **Barrier function** (for lower bound on axis :math:`i`): .. math:: h_i(q) = p_i(q) - p_{\min,i} **Barrier Jacobian:** .. math:: J_{h_i} = J_{\text{frame},i}(q) \quad \text{(row } i \text{ of frame Jacobian)} **QP constraint** (using saturating class-K function): .. math:: -J_{h_i} \cdot \Delta q \leq \Delta t \cdot \gamma \cdot \frac{h_i}{1 + |h_i|} - m Where: - :math:`\gamma` — barrier gain (aggressiveness) - :math:`m` — safety margin (conservative buffer for linearization error) **Safe displacement regularization** adds to the objective: .. math:: \frac{r}{2\|J_h\|^2} \|\Delta q - \Delta q_{\text{safe}}\|^2 This encourages motion toward a safe configuration when near boundaries. +-------------------------------+-------------------------------------+-----------+ | Parameter | Description | Default | +===============================+=====================================+===========+ | ``gain`` | Class-K function gain | 1.0 | | | :math:`\gamma` | | +-------------------------------+-------------------------------------+-----------+ | ``dt`` | Control timestep | required | +-------------------------------+-------------------------------------+-----------+ | ``safe_displacement_gain`` | Regularization weight :math:`r` | 1.0 | +-------------------------------+-------------------------------------+-----------+ | ``safety_margin`` | Conservative buffer :math:`m` | 0.0 | +-------------------------------+-------------------------------------+-----------+ | ``axis_selection`` | Enable/disable per-axis constraints | all | +-------------------------------+-------------------------------------+-----------+ SelfCollisionBarrier """""""""""""""""""" Keeps the closest pairs of bodies in the robot's collision model from interpenetrating, by enforcing a minimum signed distance on each tracked pair. Distances come from the narrow-phase collision check on the scene's collision model. **Barrier function** (for the :math:`i`-th closest collision pair): .. math:: h_i(q) = d_i(q) - d_{\min} where :math:`d_i(q)` is the signed distance between the two geometries in pair :math:`i`. Pairs are re-selected at every call: at each step the :math:`n_{\text{pairs}}` smallest distances across the full collision model become the active constraints, so the barrier always tracks whichever pairs are most at risk. **Barrier Jacobian** (built from witness points and parent-joint Jacobians): .. math:: J_{h_i} = n^T J^{(1)}_p + (r_1 \times n)^T J^{(1)}_w - n^T J^{(2)}_p - (r_2 \times n)^T J^{(2)}_w Where: - :math:`n` — unit vector from witness point 1 to witness point 2 (world frame) - :math:`r_k` — vector from joint :math:`k`'s origin to its witness point (lever arm) - :math:`J^{(k)}_p, J^{(k)}_w` — linear and angular parts of joint :math:`k`'s ``LOCAL_WORLD_ALIGNED`` Jacobian If the witness points coincide (:math:`d_i \approx 0`) the contact normal is undefined; that Jacobian row is zeroed so the barrier degrades gracefully instead of producing NaNs. The same safe-displacement regularization described for ``PositionBarrier`` applies. +-----------------------------+----------------------------------------+-----------+ | Parameter | Description | Default | +=============================+========================================+===========+ | ``n_collision_pairs`` | Number of closest pairs to constrain | required | | | (must be ≤ total pairs in the model) | | +-----------------------------+----------------------------------------+-----------+ | ``d_min`` | Minimum allowed distance | 0.02 | | | :math:`d_{\min}` (meters) | | +-----------------------------+----------------------------------------+-----------+ | ``gain`` | Class-K function gain :math:`\gamma` | 1.0 | +-----------------------------+----------------------------------------+-----------+ | ``dt`` | Control timestep | required | +-----------------------------+----------------------------------------+-----------+ | ``safe_displacement_gain`` | Regularization weight :math:`r` | 1.0 | +-----------------------------+----------------------------------------+-----------+ | ``safety_margin`` | Conservative buffer :math:`m` | 0.0 | +-----------------------------+----------------------------------------+-----------+ .. note:: Per-pair narrow-phase distance dominates the per-solve cost when many pairs are tracked. Pick the smallest ``n_collision_pairs`` that still covers the pairs you expect to be active. The post-solve ``enforceBarriers()`` check only re-evaluates this active set, so over-sizing ``n_collision_pairs`` makes both the QP assembly and the FK validation slower. Additionally, you should consider using robot models that have optimized collision meshes (e.g., simplified convex hulls or simple geometric primitives). If your collision meshes are too high-quality, this will dramatically increase solve time. Linearization Error and ``enforceBarriers()`` """""""""""""""""""""""""""""""""""""""""""""" The CBF constraint is based on a first-order Taylor expansion: .. math:: h(q + \Delta q) \approx h(q) + J_h \cdot \Delta q This has :math:`O(\|\Delta q\|^2)` error. Near boundaries with large commands, the linearized constraint can be satisfied while the actual barrier is violated. ``enforceBarriers()`` provides a post-solve safety check using forward kinematics: .. code-block:: cpp // After solving QP oink.solveIk(tasks, constraints, barriers, scene, delta_q); // Validate using FK: if h(q + delta_q) < -tolerance, set delta_q = 0 oink.enforceBarriers(barriers, scene, delta_q, tolerance); Implementation Notes ^^^^^^^^^^^^^^^^^^^^ Numerical Properties """""""""""""""""""" - **Positive definiteness**: Guaranteed by Tikhonov regularization :math:`\lambda I` - **Convexity**: Quadratic objective + linear constraints → unique global optimum - **Weight scaling**: Applied as :math:`\sqrt{w}` for better conditioning Solver """""" OInK uses `OSQP `_ with: - Dense accumulation of :math:`H` and :math:`c` - Sparse conversion for solving - Warm-starting between iterations - Workspace caching for constraints Usage Example ^^^^^^^^^^^^^ .. code-block:: python import numpy as np from roboplan.core import Scene, CartesianConfiguration from roboplan.optimal_ik import ( ConfigurationTask, ConfigurationTaskOptions, FrameTask, FrameTaskOptions, Oink, PositionLimit, VelocityLimit, PositionBarrier, SelfCollisionBarrier, ) # Scene + solver. urdf/srdf are XML strings (e.g. from xacro.process_file(...).toxml()). scene = Scene("robot", urdf=urdf_xml, srdf=srdf_xml, package_paths=package_paths) oink = Oink(scene, group_name="arm") nv = len(oink.v_indices) # joint-group velocity dimension dt = 0.01 # Primary task: track an SE(3) target with the end-effector frame. goal = CartesianConfiguration() goal.base_frame = "base_link" goal.tip_frame = "tool0" goal.tform = target_transform # 4x4 SE(3) matrix frame_task = FrameTask(oink, scene, goal, FrameTaskOptions( position_cost=1.0, orientation_cost=0.1, task_gain=1.0, lm_damping=0.01, max_position_error=0.1, # keeps the CBF linearization valid )) # Lower-priority posture regularization. priority=2 projects it into the FrameTask # nullspace, so it only uses the redundant DoF the EE task leaves free. posture_task = ConfigurationTask( oink, q_nominal[oink.q_indices], np.full(nv, 0.05), ConfigurationTaskOptions(priority=2), ) tasks = [frame_task, posture_task] # Hard constraints (exact enforcement). constraints = [ PositionLimit(oink, gain=1.0), VelocityLimit(oink, dt, v_max=np.ones(nv)), ] # Barriers (smooth safety). barriers = [ PositionBarrier( oink, scene, frame_name="tool0", p_min=np.array([-0.5, -0.5, 0.1]), p_max=np.array([0.5, 0.5, 1.0]), dt=dt, gain=5.0, safety_margin=0.01, ), SelfCollisionBarrier( oink, scene, n_collision_pairs=4, # track the 4 closest pairs each step dt=dt, gain=0.01, d_min=0.02, ), ] # One control step. delta_q = np.zeros(nv) oink.solveIk(scene, tasks, constraints, barriers, delta_q, regularization=1e-6) # When the joint group is a subset of the model, scatter the group's velocity into # the full-model nv vector before enforceBarriers / integrate. delta_q_full = np.zeros(model_nv) # full model velocity dimension delta_q_full[oink.v_indices] = delta_q # Optional FK-based safety check: zeros delta_q_full if any barrier would be violated. oink.enforceBarriers(scene, barriers, delta_q_full) q_next = scene.integrate(scene.getCurrentJointPositions(), delta_q_full) scene.setJointPositions(q_next)