From 83b1165c51ba9f87b7f535f888b669e4abfb8dc9 Mon Sep 17 00:00:00 2001 From: Matt Date: Thu, 25 Sep 2025 16:21:32 -0400 Subject: [PATCH] Add joint freezing through direct QP equality constraint --- CHANGELOG.md | 6 ++++ mink/solve_ik.py | 93 +++++++++++++++++++++++++++++++++++++++++++++--- 2 files changed, 95 insertions(+), 4 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 57fba2fd..3610882e 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,6 +2,12 @@ All notable changes to this project will be documented in this file. +## [0.0.14] - 2025-09-26 + +### Changed + +- Added joint locking via QP equality constraints as an alternative to removing the joints from the Mujoco model. + ## [0.0.13] - 2025-09-12 ### Bugfix diff --git a/mink/solve_ik.py b/mink/solve_ik.py index 9f7af326..c02344b5 100644 --- a/mink/solve_ik.py +++ b/mink/solve_ik.py @@ -1,11 +1,12 @@ """Build and solve the inverse kinematics problem.""" -from typing import List, Optional, Sequence, Tuple +from typing import List, Optional, Sequence, Tuple, Union import numpy as np import qpsolvers from .configuration import Configuration +from .constants import dof_width from .exceptions import NoSolutionFound from .limits import ConfigurationLimit, Limit from .tasks import BaseTask, Objective @@ -41,12 +42,80 @@ def _compute_qp_inequalities( return np.vstack(G_list), np.hstack(h_list) +def _resolve_frozen_dof_indices( + configuration: Configuration, frozen_joints: Optional[Sequence[Union[int, str]]] +) -> List[int]: + """Resolve joint identifiers to tangent-space DoF indices to freeze. + + Args: + configuration: Robot configuration. + frozen_joints: Optional sequence of joint IDs or names to freeze + (zero velocity). If a joint is floating-base (free), all 6 DoFs are + frozen. If hinge/slide, the single DoF is frozen. + + Returns: + A list of DoF indices in the tangent space to be constrained to zero. + """ + if not frozen_joints: + return [] + + model = configuration.model + dof_indices: List[int] = [] + for j in frozen_joints: + jnt_id: int + if isinstance(j, str): + jnt_id = model.joint(j).id + else: + jnt_id = j + vadr = model.jnt_dofadr[jnt_id] + width = dof_width(model.jnt_type[jnt_id]) + dof_indices.extend(range(vadr, vadr + width)) + # Remove duplicates while preserving order. + seen = set() + unique_dofs: List[int] = [] + for idx in dof_indices: + if idx not in seen: + seen.add(idx) + unique_dofs.append(idx) + return unique_dofs + + +def _compute_qp_equalities( + configuration: Configuration, frozen_joints: Optional[Sequence[Union[int, str]]] +) -> Tuple[Optional[np.ndarray], Optional[np.ndarray]]: + """Build equality constraints A Δq = b to freeze selected joints. + + We enforce zero velocity on the selected joints, which translates to + Δq = 0 along the corresponding tangent-space DoFs since v = Δq / dt. + + Args: + configuration: Robot configuration. + frozen_joints: Joint IDs or names to freeze. If None or empty, returns + (None, None). + + Returns: + A pair (A, b) or (None, None) if there are no equalities. + """ + dof_indices = _resolve_frozen_dof_indices(configuration, frozen_joints) + if not dof_indices: + return None, None + + nv = configuration.model.nv + m = len(dof_indices) + A = np.zeros((m, nv)) + for row, dof in enumerate(dof_indices): + A[row, dof] = 1.0 + b = np.zeros((m,)) + return A, b + + def build_ik( configuration: Configuration, tasks: Sequence[BaseTask], dt: float, damping: float = 1e-12, limits: Optional[Sequence[Limit]] = None, + frozen_joints: Optional[Sequence[Union[int, str]]] = None, ) -> qpsolvers.Problem: r"""Build the quadratic program given the current configuration and tasks. @@ -56,7 +125,8 @@ def build_ik( \begin{align*} \min_{\Delta q} & \frac{1}{2} \Delta q^T H \Delta q + c^T \Delta q \\ - \text{s.t.} \quad & G \Delta q \leq h + \text{s.t.} \quad & G \Delta q \leq h \\ + & A \Delta q = b \end{align*} where :math:`\Delta q = v / dt` is the vector of joint displacements. @@ -70,13 +140,17 @@ def build_ik( dofs, including floating-base coordinates. limits: List of limits to enforce. Set to empty list to disable. If None, defaults to a configuration limit. + frozen_joints: Optional sequence of joint names or IDs whose velocities + must be zero. This is enforced via equality constraints on the + corresponding tangent-space DoFs. Returns: Quadratic program of the inverse kinematics problem. """ P, q = _compute_qp_objective(configuration, tasks, damping) G, h = _compute_qp_inequalities(configuration, limits, dt) - return qpsolvers.Problem(P, q, G, h) + A, b = _compute_qp_equalities(configuration, frozen_joints) + return qpsolvers.Problem(P, q, G, h, A, b) def solve_ik( @@ -87,6 +161,7 @@ def solve_ik( damping: float = 1e-12, safety_break: bool = False, limits: Optional[Sequence[Limit]] = None, + frozen_joints: Optional[Sequence[Union[int, str]]] = None, **kwargs, ) -> np.ndarray: r"""Solve the differential inverse kinematics problem. @@ -107,6 +182,9 @@ def solve_ik( warning and continue execution. limits: List of limits to enforce. Set to empty list to disable. If None, defaults to a configuration limit. + frozen_joints: Optional sequence of joint names or IDs whose velocities + must be zero. This is enforced via equality constraints on the + corresponding tangent-space DoFs. kwargs: Keyword arguments to forward to the backend QP solver. Raises: @@ -118,7 +196,14 @@ def solve_ik( Velocity :math:`v` in tangent space. """ configuration.check_limits(safety_break=safety_break) - problem = build_ik(configuration, tasks, dt, damping, limits) + problem = build_ik( + configuration, + tasks, + dt, + damping, + limits, + frozen_joints=frozen_joints, + ) result = qpsolvers.solve_problem(problem, solver=solver, **kwargs) if not result.found: raise NoSolutionFound(solver)