From c997e9ab9cf787b4bfb93550206e5042eb0cc600 Mon Sep 17 00:00:00 2001 From: mickaelbegon Date: Fri, 5 Jun 2026 15:13:35 -0400 Subject: [PATCH 1/6] Add ProxQP differential inverse kinematics --- binding/python3/rigid_body.py | 204 ++++++++++++++++++ .../python3/test_inverse_kinematics.py | 74 +++++++ 2 files changed, 278 insertions(+) diff --git a/binding/python3/rigid_body.py b/binding/python3/rigid_body.py index 29309cf46..065dcdecb 100644 --- a/binding/python3/rigid_body.py +++ b/binding/python3/rigid_body.py @@ -1,9 +1,29 @@ +from dataclasses import dataclass + from scipy import optimize import numpy as np from . import biorbd from .utils import get_range_q +try: + import proxsuite + + has_proxsuite = True +except ImportError: + proxsuite = None + has_proxsuite = False + + +@dataclass +class DifferentialInverseKinematicsResult: + fun: np.ndarray + nfev: int + njev: int + message: str + status: int + success: bool + def marker_index(model, marker_name: str) -> int: """ @@ -439,3 +459,187 @@ def sol(self): ) return self.output + + +class DifferentialInverseKinematics(InverseKinematics): + """ + Differential inverse kinematics solver using ProxQP. + + This class solves a sequence of linearized marker-tracking QPs. It currently + handles marker tracking and generalized-coordinate bounds, but no holonomic + loop constraints. + """ + + def _initial_guess(self, initial_q: np.ndarray | None): + if initial_q is not None: + q = np.asarray(initial_q, dtype=float) + if q.shape != (self.nb_q,): + raise ValueError(f"initial_q should have shape ({self.nb_q},)") + return q.copy() + + q_min, q_max = self.bounds + q = np.zeros(self.nb_q) + finite_bounds = np.isfinite(q_min) & np.isfinite(q_max) + q[finite_bounds] = (q_min[finite_bounds] + q_max[finite_bounds]) / 2 + lower_only = np.isfinite(q_min) & ~np.isfinite(q_max) + q[lower_only] = q_min[lower_only] + upper_only = ~np.isfinite(q_min) & np.isfinite(q_max) + q[upper_only] = q_max[upper_only] + return q + + @staticmethod + def _proxqp_bounds(bounds: tuple[np.ndarray, np.ndarray], q: np.ndarray): + lower, upper = bounds + lower = lower - q + upper = upper - q + lower = np.where(np.isfinite(lower), lower, -1e20) + upper = np.where(np.isfinite(upper), upper, 1e20) + return lower, upper + + def _solve_qp( + self, + hessian: np.ndarray, + gradient: np.ndarray, + lower_bounds: np.ndarray, + upper_bounds: np.ndarray, + tolerance: float, + ): + qp = proxsuite.proxqp.dense.QP(self.nb_q, 0, self.nb_q) + qp.settings.eps_abs = tolerance + qp.settings.eps_rel = tolerance + qp.settings.verbose = False + qp.init( + hessian, + gradient, + np.empty((0, self.nb_q)), + np.empty((0,)), + np.eye(self.nb_q), + lower_bounds, + upper_bounds, + ) + qp.solve() + return np.array(qp.results.x).reshape(self.nb_q) + + def solve( + self, + initial_q: np.ndarray | None = None, + marker_weights: np.ndarray | None = None, + max_iterations: int = 10, + tolerance: float = 1e-8, + regularization: float = 1e-8, + verbose: bool = True, + ): + """ + Solve inverse kinematics by repeated differential QPs. + + Parameters + ---------- + initial_q: np.ndarray | None + Initial generalized coordinates for the first frame. If None, the + midpoint of finite model bounds is used when possible. + marker_weights: np.ndarray | None + One scalar weight per model marker. Missing markers are ignored. + max_iterations: int + Maximum number of QP linearization steps per frame. + tolerance: float + Stopping tolerance on marker residual norm and ProxQP accuracy. + regularization: float + Positive diagonal regularization added to the QP Hessian. + verbose: bool + If True, prints frame progress every 100 frames. + + Returns + ------- + q : np.ndarray + Generalized coordinates with shape (nb_q, nb_frames). + """ + if not has_proxsuite: + raise RuntimeError( + "DifferentialInverseKinematics requires the optional dependency 'proxsuite'." + ) + if max_iterations < 1: + raise ValueError("max_iterations must be at least 1") + if regularization < 0: + raise ValueError("regularization must be non-negative") + + if marker_weights is None: + marker_weights = np.ones(self.nb_markers) + marker_weights = np.asarray(marker_weights, dtype=float) + if marker_weights.shape != (self.nb_markers,): + raise ValueError(f"marker_weights should have shape ({self.nb_markers},)") + + bounds = self.bounds + q = self._initial_guess(initial_q) + + for f in range(self.nb_frames): + if verbose and f % 100 == 0: + print(f"Frame {f}/{self.nb_frames}") + + if f != 0: + q = self.q[:, f - 1].copy() + + residual = np.array([]) + success = False + iterations = 0 + for iterations in range(1, max_iterations + 1): + indices_to_keep = self.indices_to_keep[f] + if not indices_to_keep: + break + + markers_real = self.xp_markers[:, :, f][:, indices_to_keep] + residual = self._marker_diff( + np.array(self.biorbd_model.technicalMarkers(q))[indices_to_keep], + markers_real, + ) + if np.linalg.norm(residual) <= tolerance: + success = True + break + + jacobian = self._marker_jacobian( + np.array(self.biorbd_model.technicalMarkersJacobian(q))[ + indices_to_keep + ] + ) + weights = np.repeat(marker_weights[indices_to_keep], 3) + weighted_jacobian = jacobian * weights[:, np.newaxis] + hessian = jacobian.T @ weighted_jacobian + hessian += np.eye(self.nb_q) * regularization + gradient = jacobian.T @ (weights * residual) + + lower_bounds, upper_bounds = self._proxqp_bounds(bounds, q) + dq = self._solve_qp( + hessian, gradient, lower_bounds, upper_bounds, tolerance + ) + q = q + dq + + if np.linalg.norm(dq) <= tolerance: + residual = self._marker_diff( + np.array(self.biorbd_model.technicalMarkers(q))[ + indices_to_keep + ], + markers_real, + ) + success = np.linalg.norm(residual) <= tolerance + break + + self.q[:, f] = q + message = ( + "Converged" + if success + else "Maximum number of QP linearizations reached" + ) + self.list_sol.append( + DifferentialInverseKinematicsResult( + fun=residual, + nfev=iterations, + njev=iterations, + message=message, + status=1 if success else 0, + success=success, + ) + ) + + return self.q + + +InverseKinematicsProxQP = DifferentialInverseKinematics diff --git a/test/binding/python3/test_inverse_kinematics.py b/test/binding/python3/test_inverse_kinematics.py index 4d5345bb5..0c13493be 100644 --- a/test/binding/python3/test_inverse_kinematics.py +++ b/test/binding/python3/test_inverse_kinematics.py @@ -55,6 +55,80 @@ def test_solve(brbd, method): np.testing.assert_almost_equal(np.squeeze(np.round(ik_q, 1).T), qinit, decimal=1) +@pytest.mark.parametrize("brbd", brbd_to_test) +def test_differential_inverse_kinematics_requires_proxsuite(brbd): + if brbd.backend == brbd.CASADI: + pytest.skip("Skip inverse kinematics for biorbd_casadi") + if brbd.has_proxsuite: + pytest.skip("This test covers the missing optional dependency path") + + biorbd_model = brbd.Model("../../models/pyomecaman.bioMod") + markers = np.zeros((3, biorbd_model.nbMarkers(), 1)) + ik = brbd.DifferentialInverseKinematics(biorbd_model, markers) + + with pytest.raises(RuntimeError, match="proxsuite"): + ik.solve(verbose=False) + + +@pytest.mark.parametrize("brbd", brbd_to_test) +def test_differential_inverse_kinematics_solve(brbd): + if brbd.backend == brbd.CASADI: + pytest.skip("Skip inverse kinematics for biorbd_casadi") + if not brbd.has_proxsuite: + pytest.skip("proxsuite is not installed") + + biorbd_model = brbd.Model("../../models/pyomecaman.bioMod") + + # Remove the dampings in this test + joint_dampings = [0, 0, 0] + biorbd_model.segment(0).setJointDampings(joint_dampings) + + qinit = np.array( + [0.1, 0.1, -0.3, 0.35, 1.15, -0.35, 1.15, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] + ) + + markers = np.ndarray((3, biorbd_model.nbMarkers(), 1)) + markers[:, :, 0] = np.array( + [mark.to_array() for mark in biorbd_model.markers(qinit)] + ).T + + ik = brbd.DifferentialInverseKinematics(biorbd_model, markers) + ik_q = ik.solve( + initial_q=np.zeros(biorbd_model.nbQ()), + max_iterations=10, + tolerance=1e-8, + verbose=False, + ) + + np.testing.assert_almost_equal(np.squeeze(np.round(ik_q, 1).T), qinit, decimal=1) + + +@pytest.mark.parametrize("brbd", brbd_to_test) +def test_differential_inverse_kinematics_ignores_missing_markers(brbd): + if brbd.backend == brbd.CASADI: + pytest.skip("Skip inverse kinematics for biorbd_casadi") + if not brbd.has_proxsuite: + pytest.skip("proxsuite is not installed") + + biorbd_model = brbd.Model("../../models/pyomecaman.bioMod") + qinit = np.array( + [0.1, 0.1, -0.3, 0.35, 1.15, -0.35, 1.15, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] + ) + + markers = np.ndarray((3, biorbd_model.nbMarkers(), 1)) + markers[:, :, 0] = np.array( + [mark.to_array() for mark in biorbd_model.markers(qinit)] + ).T + markers[:, 0, 0] = np.nan + + ik = brbd.DifferentialInverseKinematics(biorbd_model, markers) + ik.solve(initial_q=qinit, max_iterations=2, tolerance=1e-8, verbose=False) + residuals = ik.sol()["residuals"] + + assert np.isnan(residuals[0, 0]) + assert np.nanmax(residuals[:, 0]) < 1e-8 + + if __name__ == "__main__": for brbd in brbd_to_test: for method in ["only_lm", "lm", "trf"]: From 21fe10f86cdedeb5efd453b644dbbaac5e80ea9d Mon Sep 17 00:00:00 2001 From: mickaelbegon Date: Fri, 5 Jun 2026 15:13:59 -0400 Subject: [PATCH 2/6] Declare optional ProxQP dependency --- pyproject.toml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/pyproject.toml b/pyproject.toml index 05fbb382b..4c28567e7 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -29,5 +29,10 @@ license = {text = "MIT"} urls = { "Homepage" = "https://github.com/pyomeca/biorbd" } requires-python = ">=3.10" +[project.optional-dependencies] +proxqp = [ + "proxsuite" +] + [tool.cibuildwheel] build-frontend = "build" From d1b77c6074e4eaaa243583745dfb3afc5e8b9ef3 Mon Sep 17 00:00:00 2001 From: mickaelbegon Date: Sat, 6 Jun 2026 06:27:42 -0400 Subject: [PATCH 3/6] Report final DIK residuals --- binding/python3/rigid_body.py | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/binding/python3/rigid_body.py b/binding/python3/rigid_body.py index 065dcdecb..b023f2ced 100644 --- a/binding/python3/rigid_body.py +++ b/binding/python3/rigid_body.py @@ -622,6 +622,16 @@ def solve( success = np.linalg.norm(residual) <= tolerance break + if self.indices_to_keep[f]: + markers_real = self.xp_markers[:, :, f][:, self.indices_to_keep[f]] + residual = self._marker_diff( + np.array(self.biorbd_model.technicalMarkers(q))[ + self.indices_to_keep[f] + ], + markers_real, + ) + success = success or np.linalg.norm(residual) <= tolerance + self.q[:, f] = q message = ( "Converged" From a11d2453dfb4cf0fe6f063fc219bff6a30e6c1da Mon Sep 17 00:00:00 2001 From: mickaelbegon Date: Fri, 12 Jun 2026 06:48:56 -0400 Subject: [PATCH 4/6] Address DIK review comments --- binding/python3/__init__.py | 1 + binding/python3/inverse_kinematics.py | 570 ++++++++++++++++++++++++++ binding/python3/rigid_body.py | 513 +---------------------- 3 files changed, 578 insertions(+), 506 deletions(-) create mode 100644 binding/python3/inverse_kinematics.py diff --git a/binding/python3/__init__.py b/binding/python3/__init__.py index 52ac79319..3277e4acc 100644 --- a/binding/python3/__init__.py +++ b/binding/python3/__init__.py @@ -4,6 +4,7 @@ from .biorbd import * from ._version import __version__ from .surface_max_torque_actuator import * +from .inverse_kinematics import * from .rigid_body import * from .utils import * diff --git a/binding/python3/inverse_kinematics.py b/binding/python3/inverse_kinematics.py new file mode 100644 index 000000000..701ab0c0e --- /dev/null +++ b/binding/python3/inverse_kinematics.py @@ -0,0 +1,570 @@ +from dataclasses import dataclass + +from scipy import optimize +import numpy as np + +from .utils import get_range_q + +try: + import proxsuite + + has_proxsuite = True +except ImportError: + proxsuite = None + has_proxsuite = False + + +@dataclass +class DifferentialInverseKinematicsResult: + """ + Per-frame result returned by the ProxQP differential IK solver. + + The named fields describe the ProxQP solve more explicitly than scipy's + OptimizeResult names. The fun, nfev and njev properties are kept so that + InverseKinematics.sol() can summarize both scipy and ProxQP results. + """ + + residual_vector: np.ndarray + marker_function_evaluations: int + jacobian_evaluations: int + message: str + status: int + success: bool + + @property + def fun(self): + return self.residual_vector + + @property + def nfev(self): + return self.marker_function_evaluations + + @property + def njev(self): + return self.jacobian_evaluations + + +class InverseKinematics: + """ + The class for generate inverse kinematics from c3d files + + Attributes: + ---------- + biorbd_model: biorbd.Model + The biorbd model loaded + marker_names: list[str] + The list of markers' name + nb_markers: int + The number of markers in the model + xp_markers: np.array + The position of the markers from the c3d + nb_frames: int + The number of frame in the c3d + nb_q: int + The number of dof in the model + q: np.array + generalized coordinates + bounds: tuple(np.ndarray, np.ndarray) + The min and max ranges of the model Q + indices_to_remove: list(list(int)) + The list of markers index which have a nan value in xp_markers + indices_to_keep: list(list(int)) + The list of markers index which have a number value in xp_markers + list_sol: list(scipy.OptimizeResult) + The list of results of least_square function + output: dict() + The output of the solution: + residuals_xyz: np.ndarray + The array of the final return of _marker_diff function. + The final difference between markers position in the model and in the c3d. + residuals: np.ndarray + The array of the norm of residuals_xyz position for each markers in each frame. + nfev: np.ndarray + The array of the number of iteration of the _marker_diff function for each frame. + njev: np.ndarray + The array of the number of iteration of the _marker_jac function for each frame. + max_marker: list(str) + The list of markers that have the highest residual. + So the markers that have the biggest difference between the model and the c3d for each frame. + message: list(str) + The list of the verbal description of the termination reason of the least_square function for each frame. + status: list(int) + The reason for algorithm termination for each frame + -1 : improper input parameters status returned from MINPACK. + 0 : the maximum number of function evaluations is exceeded. + 1 : gtol termination condition is satisfied. + 2 : ftol termination condition is satisfied. + 3 : xtol termination condition is satisfied. + 4 : Both ftol and xtol termination conditions are satisfied. + success: list(bool) + The list of success for each frame. True if one of the convergence criteria is satisfied (status > 0). + nb_dim: int + The number of dimension of the model + + Methods + ------- + _get_nan_index(self) + Find, for each frame, the index of the markers which has a nan value + _marker_diff(markers_model: np.ndarray, markers_real: np.ndarray) + Compute the difference between the marker position in the model and the position in the data. + _marker_jacobian(self, jacobian_matrix) + Generate the Jacobian matrix for each frame. + optimize(self, n_frame: int, method: str, bounds: tuple() = None) + Uses least_square function to minimize the difference between markers' positions of model and c3d. + solve(self, method: str = "lm") + Solve the inverse kinematics by using least_square method from scipy. + sol(self) + Create and return a dict which contains the output each optimization. + + """ + + def __init__( + self, + model, + marker_data: np.ndarray, + ): + """ + Parameters + ---------- + model: biorbd.Model + The biorbd model loaded with biorbd Eigen backend + marker_data: np.ndarray + The position of the markers from the c3d of shape (nb_dim, nb_marker, nb_frame), + nb_marker should be equal to the number of markers in the model, unit should be in meters. + """ + self.biorbd_model = model + self.marker_names = [ + self.biorbd_model.markerNames()[i].to_string() + for i in range(len(self.biorbd_model.markerNames())) + ] + self.nb_markers = self.biorbd_model.nbMarkers() + + if isinstance(marker_data, np.ndarray): + if ( + marker_data.ndim >= 2 + and marker_data.shape[0] <= 3 + and marker_data.shape[1] == self.nb_markers + ): + self.xp_markers = marker_data + self.nb_frames = marker_data.shape[2] + else: + raise ValueError( + f"The standard dimension of the NumPy array should be (nb_dim, nb_marker, nb_frame)" + ) + else: + raise ValueError("The standard inputs is a numpy.ndarray") + + self.nb_q = self.biorbd_model.nbQ() + self.q = np.zeros((self.nb_q, self.nb_frames)) + + self.bounds = get_range_q(self.biorbd_model) + + self.indices_to_remove = [] + self.indices_to_keep = [] + self._get_nan_index() + + self.list_sol = [] + + self.output = dict() + self.nb_dim = self.xp_markers.shape[0] + + def _get_nan_index(self): + """ + Find, for each frame, the index of the markers which has a nan value + """ + for j in range(self.nb_frames): + self.indices_to_remove.append( + list(np.unique(np.isnan(self.xp_markers[:, :, j]).nonzero()[1])) + ) + self.indices_to_keep.append( + list(np.unique(np.isfinite(self.xp_markers[:, :, j]).nonzero()[1])) + ) + + @staticmethod + def _marker_diff(markers_model: np.ndarray, markers_real: np.ndarray): + """ + Compute the difference between the marker position in the model and the position in the data + + Parameters: + ----------- + markers_model: np.ndarray + he position of the markers from the model during a certain frame + markers_real: np.ndarray + The position of the markers from the c3d during a certain frame + + Return: + ------ + The difference vector between markers' position in the model and in the c3d + """ + nb_marker = len(markers_real[0]) + + vect_pos_markers = np.zeros(3 * nb_marker) + + for m, value in enumerate(markers_model): + vect_pos_markers[m * 3 : (m + 1) * 3] = value.to_array() + + return vect_pos_markers - np.reshape(markers_real.T, (3 * nb_marker,)) + + def _marker_jacobian(self, jacobian_matrix: np.ndarray): + """ + Generate the Jacobian matrix for each frame. + + Parameters: + ----------- + jacobian_matrix: np.ndarray + The Jacobian matrix of the model + + Return: + ------ + The Jacobian matrix with right dimension + """ + nb_markers = len(jacobian_matrix) + jacobian = np.zeros((3 * nb_markers, self.nb_q)) + + for m, value in enumerate(jacobian_matrix): + jacobian[m * 3 : (m + 1) * 3, :] = value.to_array() + + return jacobian + + def solve(self, method: str = "lm"): + """ + Solve the inverse kinematics by using least_square method from scipy + + Parameters: + ---------- + method: str + The method used by least_square to optimize the difference. + + If method = 'lm', the 'trf' method will be used for the first frame, in order to respect the bounds of the model. + Then, the 'lm' method will be used for the following frames. + If method = 'trf', the 'trf' method will be used for all the frames. + If method = 'only_lm', the 'lm' method will be used for all the frames. + + In least_square: + -‘trf’ : Trust Region Reflective algorithm, particularly suitable for large sparse problems + with bounds. + Generally robust method. + -‘lm’ : Levenberg-Marquardt algorithm as implemented in MINPACK. + Doesn’t handle bounds and sparse Jacobians. + Usually the most efficient method for small unconstrained problems. + + Returns + ---------- + q : np.array + generalized coordinates + """ + initial_bounds = (-np.inf, np.inf) if method == "only_lm" else self.bounds + initial_method = "lm" if method == "only_lm" else "trf" + + bounds = self.bounds if method == "trf" else (-np.inf, np.inf) + method = "lm" if method == "only_lm" else method + + if method != "lm" and method != "trf" and method != "only_lm": + raise ValueError( + 'This method is not implemented please use "trf", "lm" or "only_lm" as argument' + ) + + for f in range(self.nb_frames): + if f % 100 == 0: + print(f"Frame {f}/{self.nb_frames}") + if initial_method != "lm": + x0 = ( + np.array( + [ + (bounds_inf + bounds_sup) / 2 + for bounds_inf, bounds_sup in zip( + initial_bounds[0], initial_bounds[1] + ) + ] + ) + if f == 0 + else self.q[:, f - 1] + ) + else: + x0 = np.ones(self.nb_q) * 0.0001 if f == 0 else self.q[:, f - 1] + + sol = optimize.least_squares( + fun=lambda q, marker_real, indices_to_keep: self._marker_diff( + np.array(self.biorbd_model.technicalMarkers(q))[indices_to_keep], + marker_real, + ), + args=( + self.xp_markers[:, :, f][:, self.indices_to_keep[f]], + self.indices_to_keep[f], + ), + bounds=initial_bounds if f == 0 else bounds, + jac=lambda q, jacobian_matrix, indices_to_keep: self._marker_jacobian( + np.array(self.biorbd_model.technicalMarkersJacobian(q))[ + indices_to_keep + ] + ), + x0=x0, + method=initial_method if f == 0 else method, + xtol=1e-6, + tr_options=dict(disp=False), + ) + self.q[:, f] = sol.x + self.list_sol.append(sol) + return self.q + + def sol(self): + """ + Create and return a dict that contains the output of each optimization. + + Return + ------ + self.output: dict() + The output of least_square function, such as number of iteration per frames, + and the marker with highest residual + """ + residuals_xyz = np.zeros((self.nb_markers * self.nb_dim, self.nb_frames)) + residuals = np.zeros((self.nb_markers, self.nb_frames)) + nfev = [sol.nfev for sol in self.list_sol] + njev = [sol.njev for sol in self.list_sol] + + for f in range(self.nb_frames): + # residuals_xyz must contains position for each markers on axis x, y and z + # (or less depending on number of dimensions) + # So, if we simply used indices_to_keep or to_remove, it doesn't work because it contains the markers' + # indices [markers 0, markers 4, ...]. + # We have to create another list which contains indices for x, y and z + # [indices_to_keep or remove for markers 0 on x, indices_to_keep or remove for markers 0 on y, ...] + # And that correspond to [0, 1, 2, 4, 5, 6, ...] in our example. + indices_to_keep_xyz = [ + h * self.nb_dim + k + for h in self.indices_to_keep[f] + for k in range(self.nb_dim) + ] + indices_to_remove_xyz = [ + h * self.nb_dim + k + for h in self.indices_to_remove[f] + for k in range(self.nb_dim) + ] + + # After we get the indices, we put the solution on the residuals + residuals_xyz[:, f][indices_to_keep_xyz] = self.list_sol[f].fun + + # The markers that we removed have 0 as position on x, y and z but this is not true. + # So we replace it by nan value. + residuals_xyz[:, f][indices_to_remove_xyz] = np.nan + residuals[:, f] = np.linalg.norm( + np.reshape(residuals_xyz[:, f], [self.nb_markers, self.nb_dim]), axis=1 + ) + + self.output = dict( + residuals=residuals, + residuals_xyz=residuals_xyz, + nfev=nfev, + njev=njev, + max_marker=[self.marker_names[i] for i in np.argmax(residuals, axis=0)], + message=[sol.message for sol in self.list_sol], + status=[sol.status for sol in self.list_sol], + success=[sol.success for sol in self.list_sol], + ) + + return self.output + + +class DifferentialInverseKinematics(InverseKinematics): + """ + Differential inverse kinematics solver using ProxQP. + + This class solves a sequence of linearized marker-tracking QPs. It handles + marker tracking and generalized-coordinate bounds, but no holonomic loop + constraints. + + Compared to the scipy least-squares backend, this solver exposes a QP step + at each linearization. That makes it easier to add linear constraints later + and can be efficient on warm-started frame sequences. The tradeoff is that + it is a local differential method: difficult frames may require several + linearizations, and the result depends on the initial guess more directly + than a full nonlinear least-squares solve. + + This implementation keeps the frame and linearization loops in Python. That + keeps the prototype easy to inspect and test, but performance-critical uses + or large trials should eventually move this loop to a compiled C++ class. + """ + + def _initial_guess(self, initial_q: np.ndarray | None): + if initial_q is not None: + q = np.asarray(initial_q, dtype=float) + if q.shape != (self.nb_q,): + raise ValueError(f"initial_q should have shape ({self.nb_q},)") + return q.copy() + + q_min, q_max = self.bounds + q = np.zeros(self.nb_q) + finite_bounds = np.isfinite(q_min) & np.isfinite(q_max) + q[finite_bounds] = (q_min[finite_bounds] + q_max[finite_bounds]) / 2 + lower_only = np.isfinite(q_min) & ~np.isfinite(q_max) + q[lower_only] = q_min[lower_only] + upper_only = ~np.isfinite(q_min) & np.isfinite(q_max) + q[upper_only] = q_max[upper_only] + return q + + @staticmethod + def _proxqp_bounds(bounds: tuple[np.ndarray, np.ndarray], q: np.ndarray): + lower, upper = bounds + lower = lower - q + upper = upper - q + lower = np.where(np.isfinite(lower), lower, -1e20) + upper = np.where(np.isfinite(upper), upper, 1e20) + return lower, upper + + def _solve_qp( + self, + hessian: np.ndarray, + gradient: np.ndarray, + lower_bounds: np.ndarray, + upper_bounds: np.ndarray, + tolerance: float, + ): + qp = proxsuite.proxqp.dense.QP(self.nb_q, 0, self.nb_q) + qp.settings.eps_abs = tolerance + qp.settings.eps_rel = tolerance + qp.settings.verbose = False + qp.init( + hessian, + gradient, + np.empty((0, self.nb_q)), + np.empty((0,)), + np.eye(self.nb_q), + lower_bounds, + upper_bounds, + ) + qp.solve() + return np.array(qp.results.x).reshape(self.nb_q) + + def solve( + self, + initial_q: np.ndarray | None = None, + marker_weights: np.ndarray | None = None, + max_iterations: int = 10, + tolerance: float = 1e-8, + regularization: float = 1e-8, + verbose: bool = True, + ): + """ + Solve inverse kinematics by repeated differential QPs. + + Parameters + ---------- + initial_q: np.ndarray | None + Initial generalized coordinates for the first frame. If None, the + midpoint of finite model bounds is used when possible. + marker_weights: np.ndarray | None + One scalar weight per model marker. Missing markers are ignored. + max_iterations: int + Maximum number of QP linearization steps per frame. + tolerance: float + Stopping tolerance on marker residual norm and ProxQP accuracy. + regularization: float + Positive diagonal regularization added to the QP Hessian. + verbose: bool + If True, prints frame progress every 100 frames. + + Returns + ------- + q : np.ndarray + Generalized coordinates with shape (nb_q, nb_frames). + """ + if not has_proxsuite: + raise RuntimeError( + "DifferentialInverseKinematics requires the optional dependency 'proxsuite'." + ) + if max_iterations < 1: + raise ValueError("max_iterations must be at least 1") + if regularization < 0: + raise ValueError("regularization must be non-negative") + + if marker_weights is None: + marker_weights = np.ones(self.nb_markers) + marker_weights = np.asarray(marker_weights, dtype=float) + if marker_weights.shape != (self.nb_markers,): + raise ValueError(f"marker_weights should have shape ({self.nb_markers},)") + + bounds = self.bounds + q = self._initial_guess(initial_q) + + for f in range(self.nb_frames): + if verbose and f % 100 == 0: + print(f"Frame {f}/{self.nb_frames}") + + if f != 0: + q = self.q[:, f - 1].copy() + + residual = np.array([]) + success = False + iterations = 0 + for iterations in range(1, max_iterations + 1): + indices_to_keep = self.indices_to_keep[f] + if not indices_to_keep: + break + + markers_real = self.xp_markers[:, :, f][:, indices_to_keep] + residual = self._marker_diff( + np.array(self.biorbd_model.technicalMarkers(q))[indices_to_keep], + markers_real, + ) + if np.linalg.norm(residual) <= tolerance: + success = True + break + + jacobian = self._marker_jacobian( + np.array(self.biorbd_model.technicalMarkersJacobian(q))[ + indices_to_keep + ] + ) + weights = np.repeat(marker_weights[indices_to_keep], 3) + weighted_jacobian = jacobian * weights[:, np.newaxis] + hessian = jacobian.T @ weighted_jacobian + hessian += np.eye(self.nb_q) * regularization + gradient = jacobian.T @ (weights * residual) + + lower_bounds, upper_bounds = self._proxqp_bounds(bounds, q) + dq = self._solve_qp( + hessian, gradient, lower_bounds, upper_bounds, tolerance + ) + q = q + dq + + if np.linalg.norm(dq) <= tolerance: + residual = self._marker_diff( + np.array(self.biorbd_model.technicalMarkers(q))[ + indices_to_keep + ], + markers_real, + ) + success = np.linalg.norm(residual) <= tolerance + break + + if self.indices_to_keep[f]: + markers_real = self.xp_markers[:, :, f][:, self.indices_to_keep[f]] + residual = self._marker_diff( + np.array(self.biorbd_model.technicalMarkers(q))[ + self.indices_to_keep[f] + ], + markers_real, + ) + success = success or np.linalg.norm(residual) <= tolerance + + self.q[:, f] = q + message = ( + "Converged" + if success + else "Maximum number of QP linearizations reached" + ) + self.list_sol.append( + DifferentialInverseKinematicsResult( + residual_vector=residual, + marker_function_evaluations=iterations, + jacobian_evaluations=iterations, + message=message, + status=1 if success else 0, + success=success, + ) + ) + + return self.q + + +InverseKinematicsProxQP = DifferentialInverseKinematics diff --git a/binding/python3/rigid_body.py b/binding/python3/rigid_body.py index b023f2ced..9f94a7b29 100644 --- a/binding/python3/rigid_body.py +++ b/binding/python3/rigid_body.py @@ -1,28 +1,13 @@ -from dataclasses import dataclass - -from scipy import optimize import numpy as np from . import biorbd -from .utils import get_range_q - -try: - import proxsuite - - has_proxsuite = True -except ImportError: - proxsuite = None - has_proxsuite = False - - -@dataclass -class DifferentialInverseKinematicsResult: - fun: np.ndarray - nfev: int - njev: int - message: str - status: int - success: bool +from .inverse_kinematics import ( + DifferentialInverseKinematics, + DifferentialInverseKinematicsResult, + InverseKinematics, + InverseKinematicsProxQP, + has_proxsuite, +) def marker_index(model, marker_name: str) -> int: @@ -169,487 +154,3 @@ def extended_kalman_filter( qddot_out[:, i] = qddot.to_array() return t, q_out, qdot_out, qddot_out - - -class InverseKinematics: - """ - The class for generate inverse kinematics from c3d files - - Attributes: - ---------- - biorbd_model: biorbd.Model - The biorbd model loaded - marker_names: list[str] - The list of markers' name - nb_markers: int - The number of markers in the model - xp_markers: np.array - The position of the markers from the c3d - nb_frames: int - The number of frame in the c3d - nb_q: int - The number of dof in the model - q: np.array - generalized coordinates - bounds: tuple(np.ndarray, np.ndarray) - The min and max ranges of the model Q - indices_to_remove: list(list(int)) - The list of markers index which have a nan value in xp_markers - indices_to_keep: list(list(int)) - The list of markers index which have a number value in xp_markers - list_sol: list(scipy.OptimizeResult) - The list of results of least_square function - output: dict() - The output of the solution: - residuals_xyz: np.ndarray - The array of the final return of _marker_diff function. - The final difference between markers position in the model and in the c3d. - residuals: np.ndarray - The array of the norm of residuals_xyz position for each markers in each frame. - nfev: np.ndarray - The array of the number of iteration of the _marker_diff function for each frame. - njev: np.ndarray - The array of the number of iteration of the _marker_jac function for each frame. - max_marker: list(str) - The list of markers that have the highest residual. - So the markers that have the biggest difference between the model and the c3d for each frame. - message: list(str) - The list of the verbal description of the termination reason of the least_square function for each frame. - status: list(int) - The reason for algorithm termination for each frame - -1 : improper input parameters status returned from MINPACK. - 0 : the maximum number of function evaluations is exceeded. - 1 : gtol termination condition is satisfied. - 2 : ftol termination condition is satisfied. - 3 : xtol termination condition is satisfied. - 4 : Both ftol and xtol termination conditions are satisfied. - success: list(bool) - The list of success for each frame. True if one of the convergence criteria is satisfied (status > 0). - nb_dim: int - The number of dimension of the model - - Methods - ------- - _get_nan_index(self) - Find, for each frame, the index of the markers which has a nan value - _marker_diff(markers_model: np.ndarray, markers_real: np.ndarray) - Compute the difference between the marker position in the model and the position in the data. - _marker_jacobian(self, jacobian_matrix) - Generate the Jacobian matrix for each frame. - optimize(self, n_frame: int, method: str, bounds: tuple() = None) - Uses least_square function to minimize the difference between markers' positions of model and c3d. - solve(self, method: str = "lm") - Solve the inverse kinematics by using least_square method from scipy. - sol(self) - Create and return a dict which contains the output each optimization. - - """ - - def __init__( - self, - model, - marker_data: np.ndarray, - ): - """ - Parameters - ---------- - model: biorbd.Model - The biorbd model loaded with biorbd Eigen backend - marker_data: np.ndarray - The position of the markers from the c3d of shape (nb_dim, nb_marker, nb_frame), - nb_marker should be equal to the number of markers in the model, unit should be in meters. - """ - self.biorbd_model = model - self.marker_names = [ - self.biorbd_model.markerNames()[i].to_string() for i in range(len(self.biorbd_model.markerNames())) - ] - self.nb_markers = self.biorbd_model.nbMarkers() - - if isinstance(marker_data, np.ndarray): - if marker_data.ndim >= 2 and marker_data.shape[0] <= 3 and marker_data.shape[1] == self.nb_markers: - self.xp_markers = marker_data - self.nb_frames = marker_data.shape[2] - else: - raise ValueError(f"The standard dimension of the NumPy array should be (nb_dim, nb_marker, nb_frame)") - else: - raise ValueError("The standard inputs is a numpy.ndarray") - - self.nb_q = self.biorbd_model.nbQ() - self.q = np.zeros((self.nb_q, self.nb_frames)) - - self.bounds = get_range_q(self.biorbd_model) - - self.indices_to_remove = [] - self.indices_to_keep = [] - self._get_nan_index() - - self.list_sol = [] - - self.output = dict() - self.nb_dim = self.xp_markers.shape[0] - - def _get_nan_index(self): - """ - Find, for each frame, the index of the markers which has a nan value - """ - for j in range(self.nb_frames): - self.indices_to_remove.append(list(np.unique(np.isnan(self.xp_markers[:, :, j]).nonzero()[1]))) - self.indices_to_keep.append(list(np.unique(np.isfinite(self.xp_markers[:, :, j]).nonzero()[1]))) - - @staticmethod - def _marker_diff(markers_model: np.ndarray, markers_real: np.ndarray): - """ - Compute the difference between the marker position in the model and the position in the data - - Parameters: - ----------- - markers_model: np.ndarray - he position of the markers from the model during a certain frame - markers_real: np.ndarray - The position of the markers from the c3d during a certain frame - - Return: - ------ - The difference vector between markers' position in the model and in the c3d - """ - nb_marker = len(markers_real[0]) - - vect_pos_markers = np.zeros(3 * nb_marker) - - for m, value in enumerate(markers_model): - vect_pos_markers[m * 3 : (m + 1) * 3] = value.to_array() - - return vect_pos_markers - np.reshape(markers_real.T, (3 * nb_marker,)) - - def _marker_jacobian(self, jacobian_matrix: np.ndarray): - """ - Generate the Jacobian matrix for each frame. - - Parameters: - ----------- - jacobian_matrix: np.ndarray - The Jacobian matrix of the model - - Return: - ------ - The Jacobian matrix with right dimension - """ - nb_markers = len(jacobian_matrix) - jacobian = np.zeros((3 * nb_markers, self.nb_q)) - - for m, value in enumerate(jacobian_matrix): - jacobian[m * 3 : (m + 1) * 3, :] = value.to_array() - - return jacobian - - def solve(self, method: str = "lm"): - """ - Solve the inverse kinematics by using least_square method from scipy - - Parameters: - ---------- - method: str - The method used by least_square to optimize the difference. - - If method = 'lm', the 'trf' method will be used for the first frame, in order to respect the bounds of the model. - Then, the 'lm' method will be used for the following frames. - If method = 'trf', the 'trf' method will be used for all the frames. - If method = 'only_lm', the 'lm' method will be used for all the frames. - - In least_square: - -‘trf’ : Trust Region Reflective algorithm, particularly suitable for large sparse problems - with bounds. - Generally robust method. - -‘lm’ : Levenberg-Marquardt algorithm as implemented in MINPACK. - Doesn’t handle bounds and sparse Jacobians. - Usually the most efficient method for small unconstrained problems. - - Returns - ---------- - q : np.array - generalized coordinates - """ - initial_bounds = (-np.inf, np.inf) if method == "only_lm" else self.bounds - initial_method = "lm" if method == "only_lm" else "trf" - - bounds = self.bounds if method == "trf" else (-np.inf, np.inf) - method = "lm" if method == "only_lm" else method - - if method != "lm" and method != "trf" and method != "only_lm": - raise ValueError('This method is not implemented please use "trf", "lm" or "only_lm" as argument') - - for f in range(self.nb_frames): - if f % 100 == 0: - print(f"Frame {f}/{self.nb_frames}") - if initial_method != "lm": - x0 = ( - np.array( - [ - (bounds_inf + bounds_sup) / 2 - for bounds_inf, bounds_sup in zip(initial_bounds[0], initial_bounds[1]) - ] - ) - if f == 0 - else self.q[:, f - 1] - ) - else: - x0 = np.ones(self.nb_q) * 0.0001 if f == 0 else self.q[:, f - 1] - - sol = optimize.least_squares( - fun=lambda q, marker_real, indices_to_keep: self._marker_diff( - np.array(self.biorbd_model.technicalMarkers(q))[indices_to_keep], marker_real - ), - args=(self.xp_markers[:, :, f][:, self.indices_to_keep[f]], self.indices_to_keep[f]), - bounds=initial_bounds if f == 0 else bounds, - jac=lambda q, jacobian_matrix, indices_to_keep: self._marker_jacobian( - np.array(self.biorbd_model.technicalMarkersJacobian(q))[indices_to_keep] - ), - x0=x0, - method=initial_method if f == 0 else method, - xtol=1e-6, - tr_options=dict(disp=False), - ) - self.q[:, f] = sol.x - self.list_sol.append(sol) - return self.q - - def sol(self): - """ - Create and return a dict that contains the output of each optimization. - - Return - ------ - self.output: dict() - The output of least_square function, such as number of iteration per frames, - and the marker with highest residual - """ - residuals_xyz = np.zeros((self.nb_markers * self.nb_dim, self.nb_frames)) - residuals = np.zeros((self.nb_markers, self.nb_frames)) - nfev = [sol.nfev for sol in self.list_sol] - njev = [sol.njev for sol in self.list_sol] - - for f in range(self.nb_frames): - # residuals_xyz must contains position for each markers on axis x, y and z - # (or less depending on number of dimensions) - # So, if we simply used indices_to_keep or to_remove, it doesn't work because it contains the markers' - # indices [markers 0, markers 4, ...]. - # We have to create another list which contains indices for x, y and z - # [indices_to_keep or remove for markers 0 on x, indices_to_keep or remove for markers 0 on y, ...] - # And that correspond to [0, 1, 2, 4, 5, 6, ...] in our example. - indices_to_keep_xyz = [h * self.nb_dim + k for h in self.indices_to_keep[f] for k in range(self.nb_dim)] - indices_to_remove_xyz = [h * self.nb_dim + k for h in self.indices_to_remove[f] for k in range(self.nb_dim)] - - # After we get the indices, we put the solution on the residuals - residuals_xyz[:, f][indices_to_keep_xyz] = self.list_sol[f].fun - - # The markers that we removed have 0 as position on x, y and z but this is not true. - # So we replace it by nan value. - residuals_xyz[:, f][indices_to_remove_xyz] = np.nan - residuals[:, f] = np.linalg.norm(np.reshape(residuals_xyz[:, f], [self.nb_markers, self.nb_dim]), axis=1) - - self.output = dict( - residuals=residuals, - residuals_xyz=residuals_xyz, - nfev=nfev, - njev=njev, - max_marker=[self.marker_names[i] for i in np.argmax(residuals, axis=0)], - message=[sol.message for sol in self.list_sol], - status=[sol.status for sol in self.list_sol], - success=[sol.success for sol in self.list_sol], - ) - - return self.output - - -class DifferentialInverseKinematics(InverseKinematics): - """ - Differential inverse kinematics solver using ProxQP. - - This class solves a sequence of linearized marker-tracking QPs. It currently - handles marker tracking and generalized-coordinate bounds, but no holonomic - loop constraints. - """ - - def _initial_guess(self, initial_q: np.ndarray | None): - if initial_q is not None: - q = np.asarray(initial_q, dtype=float) - if q.shape != (self.nb_q,): - raise ValueError(f"initial_q should have shape ({self.nb_q},)") - return q.copy() - - q_min, q_max = self.bounds - q = np.zeros(self.nb_q) - finite_bounds = np.isfinite(q_min) & np.isfinite(q_max) - q[finite_bounds] = (q_min[finite_bounds] + q_max[finite_bounds]) / 2 - lower_only = np.isfinite(q_min) & ~np.isfinite(q_max) - q[lower_only] = q_min[lower_only] - upper_only = ~np.isfinite(q_min) & np.isfinite(q_max) - q[upper_only] = q_max[upper_only] - return q - - @staticmethod - def _proxqp_bounds(bounds: tuple[np.ndarray, np.ndarray], q: np.ndarray): - lower, upper = bounds - lower = lower - q - upper = upper - q - lower = np.where(np.isfinite(lower), lower, -1e20) - upper = np.where(np.isfinite(upper), upper, 1e20) - return lower, upper - - def _solve_qp( - self, - hessian: np.ndarray, - gradient: np.ndarray, - lower_bounds: np.ndarray, - upper_bounds: np.ndarray, - tolerance: float, - ): - qp = proxsuite.proxqp.dense.QP(self.nb_q, 0, self.nb_q) - qp.settings.eps_abs = tolerance - qp.settings.eps_rel = tolerance - qp.settings.verbose = False - qp.init( - hessian, - gradient, - np.empty((0, self.nb_q)), - np.empty((0,)), - np.eye(self.nb_q), - lower_bounds, - upper_bounds, - ) - qp.solve() - return np.array(qp.results.x).reshape(self.nb_q) - - def solve( - self, - initial_q: np.ndarray | None = None, - marker_weights: np.ndarray | None = None, - max_iterations: int = 10, - tolerance: float = 1e-8, - regularization: float = 1e-8, - verbose: bool = True, - ): - """ - Solve inverse kinematics by repeated differential QPs. - - Parameters - ---------- - initial_q: np.ndarray | None - Initial generalized coordinates for the first frame. If None, the - midpoint of finite model bounds is used when possible. - marker_weights: np.ndarray | None - One scalar weight per model marker. Missing markers are ignored. - max_iterations: int - Maximum number of QP linearization steps per frame. - tolerance: float - Stopping tolerance on marker residual norm and ProxQP accuracy. - regularization: float - Positive diagonal regularization added to the QP Hessian. - verbose: bool - If True, prints frame progress every 100 frames. - - Returns - ------- - q : np.ndarray - Generalized coordinates with shape (nb_q, nb_frames). - """ - if not has_proxsuite: - raise RuntimeError( - "DifferentialInverseKinematics requires the optional dependency 'proxsuite'." - ) - if max_iterations < 1: - raise ValueError("max_iterations must be at least 1") - if regularization < 0: - raise ValueError("regularization must be non-negative") - - if marker_weights is None: - marker_weights = np.ones(self.nb_markers) - marker_weights = np.asarray(marker_weights, dtype=float) - if marker_weights.shape != (self.nb_markers,): - raise ValueError(f"marker_weights should have shape ({self.nb_markers},)") - - bounds = self.bounds - q = self._initial_guess(initial_q) - - for f in range(self.nb_frames): - if verbose and f % 100 == 0: - print(f"Frame {f}/{self.nb_frames}") - - if f != 0: - q = self.q[:, f - 1].copy() - - residual = np.array([]) - success = False - iterations = 0 - for iterations in range(1, max_iterations + 1): - indices_to_keep = self.indices_to_keep[f] - if not indices_to_keep: - break - - markers_real = self.xp_markers[:, :, f][:, indices_to_keep] - residual = self._marker_diff( - np.array(self.biorbd_model.technicalMarkers(q))[indices_to_keep], - markers_real, - ) - if np.linalg.norm(residual) <= tolerance: - success = True - break - - jacobian = self._marker_jacobian( - np.array(self.biorbd_model.technicalMarkersJacobian(q))[ - indices_to_keep - ] - ) - weights = np.repeat(marker_weights[indices_to_keep], 3) - weighted_jacobian = jacobian * weights[:, np.newaxis] - hessian = jacobian.T @ weighted_jacobian - hessian += np.eye(self.nb_q) * regularization - gradient = jacobian.T @ (weights * residual) - - lower_bounds, upper_bounds = self._proxqp_bounds(bounds, q) - dq = self._solve_qp( - hessian, gradient, lower_bounds, upper_bounds, tolerance - ) - q = q + dq - - if np.linalg.norm(dq) <= tolerance: - residual = self._marker_diff( - np.array(self.biorbd_model.technicalMarkers(q))[ - indices_to_keep - ], - markers_real, - ) - success = np.linalg.norm(residual) <= tolerance - break - - if self.indices_to_keep[f]: - markers_real = self.xp_markers[:, :, f][:, self.indices_to_keep[f]] - residual = self._marker_diff( - np.array(self.biorbd_model.technicalMarkers(q))[ - self.indices_to_keep[f] - ], - markers_real, - ) - success = success or np.linalg.norm(residual) <= tolerance - - self.q[:, f] = q - message = ( - "Converged" - if success - else "Maximum number of QP linearizations reached" - ) - self.list_sol.append( - DifferentialInverseKinematicsResult( - fun=residual, - nfev=iterations, - njev=iterations, - message=message, - status=1 if success else 0, - success=success, - ) - ) - - return self.q - - -InverseKinematicsProxQP = DifferentialInverseKinematics From b904fa0c2d489d0395a72b04f3e6df57f50891d6 Mon Sep 17 00:00:00 2001 From: mickaelbegon Date: Fri, 12 Jun 2026 11:57:25 -0400 Subject: [PATCH 5/6] Add loop-closure DIK example --- binding/python3/inverse_kinematics.py | 130 ++++++++++-- binding/python3/rigid_body.py | 1 + .../inverse_kinematics_loop_constraint.py | 194 ++++++++++++++++++ .../python3/test_inverse_kinematics.py | 116 +++++++++++ 4 files changed, 429 insertions(+), 12 deletions(-) create mode 100644 examples/python3/inverse_kinematics_loop_constraint.py diff --git a/binding/python3/inverse_kinematics.py b/binding/python3/inverse_kinematics.py index 701ab0c0e..e95ba0b4d 100644 --- a/binding/python3/inverse_kinematics.py +++ b/binding/python3/inverse_kinematics.py @@ -44,6 +44,54 @@ def njev(self): return self.jacobian_evaluations +@dataclass +class MarkerLoopClosureConstraint: + """ + Linearized loop-closure constraint between two technical markers. + + The constraint enforces that both markers occupy the same global position. + At each differential IK linearization, it contributes: + + ``(J_predecessor - J_successor) dq = -(p_predecessor - p_successor)``. + """ + + predecessor_marker: str | int + successor_marker: str | int + axes: tuple[int, ...] = (0, 1, 2) + + @staticmethod + def _marker_index(marker_names: list[str], marker: str | int): + if isinstance(marker, int): + return marker + try: + return marker_names.index(marker) + except ValueError: + raise ValueError(f"{marker} is not in the biorbd model") from None + + def residual_and_jacobian(self, inverse_kinematics, q: np.ndarray): + predecessor_index = self._marker_index( + inverse_kinematics.marker_names, self.predecessor_marker + ) + successor_index = self._marker_index( + inverse_kinematics.marker_names, self.successor_marker + ) + axes = np.asarray(self.axes, dtype=int) + + markers = np.array(inverse_kinematics.biorbd_model.technicalMarkers(q)) + jacobians = np.array( + inverse_kinematics.biorbd_model.technicalMarkersJacobian(q) + ) + + residual = ( + markers[predecessor_index].to_array() - markers[successor_index].to_array() + )[axes] + jacobian = ( + jacobians[predecessor_index].to_array() + - jacobians[successor_index].to_array() + )[axes, :] + return residual, jacobian + + class InverseKinematics: """ The class for generate inverse kinematics from c3d files @@ -370,12 +418,12 @@ class DifferentialInverseKinematics(InverseKinematics): Differential inverse kinematics solver using ProxQP. This class solves a sequence of linearized marker-tracking QPs. It handles - marker tracking and generalized-coordinate bounds, but no holonomic loop - constraints. + marker tracking, generalized-coordinate bounds, and optional linearized + marker loop-closure constraints. Compared to the scipy least-squares backend, this solver exposes a QP step - at each linearization. That makes it easier to add linear constraints later - and can be efficient on warm-started frame sequences. The tradeoff is that + at each linearization. That makes it easier to add linear constraints and + can be efficient on warm-started frame sequences. The tradeoff is that it is a local differential method: difficult frames may require several linearizations, and the result depends on the initial guess more directly than a full nonlinear least-squares solve. @@ -418,16 +466,23 @@ def _solve_qp( lower_bounds: np.ndarray, upper_bounds: np.ndarray, tolerance: float, + equality_matrix: np.ndarray | None = None, + equality_vector: np.ndarray | None = None, ): - qp = proxsuite.proxqp.dense.QP(self.nb_q, 0, self.nb_q) + nb_equalities = 0 if equality_matrix is None else equality_matrix.shape[0] + qp = proxsuite.proxqp.dense.QP(self.nb_q, nb_equalities, self.nb_q) qp.settings.eps_abs = tolerance qp.settings.eps_rel = tolerance qp.settings.verbose = False + equality_matrix = ( + np.empty((0, self.nb_q)) if equality_matrix is None else equality_matrix + ) + equality_vector = np.empty((0,)) if equality_vector is None else equality_vector qp.init( hessian, gradient, - np.empty((0, self.nb_q)), - np.empty((0,)), + equality_matrix, + equality_vector, np.eye(self.nb_q), lower_bounds, upper_bounds, @@ -435,6 +490,21 @@ def _solve_qp( qp.solve() return np.array(qp.results.x).reshape(self.nb_q) + def _constraints_residual_and_jacobian( + self, q: np.ndarray, constraints: list[MarkerLoopClosureConstraint] + ): + if not constraints: + return np.empty((0,)), np.empty((0, self.nb_q)) + + residuals = [] + jacobians = [] + for constraint in constraints: + residual, jacobian = constraint.residual_and_jacobian(self, q) + residuals.append(residual) + jacobians.append(jacobian) + + return np.concatenate(residuals), np.vstack(jacobians) + def solve( self, initial_q: np.ndarray | None = None, @@ -442,6 +512,8 @@ def solve( max_iterations: int = 10, tolerance: float = 1e-8, regularization: float = 1e-8, + constraints: list[MarkerLoopClosureConstraint] | None = None, + constraint_tolerance: float | None = None, verbose: bool = True, ): """ @@ -460,6 +532,11 @@ def solve( Stopping tolerance on marker residual norm and ProxQP accuracy. regularization: float Positive diagonal regularization added to the QP Hessian. + constraints: list[MarkerLoopClosureConstraint] | None + Linearized equality constraints to enforce at each QP step. + constraint_tolerance: float | None + Stopping tolerance on the nonlinear constraint residual norm. If + None, the marker tolerance is used. verbose: bool If True, prints frame progress every 100 frames. @@ -476,6 +553,9 @@ def solve( raise ValueError("max_iterations must be at least 1") if regularization < 0: raise ValueError("regularization must be non-negative") + if constraint_tolerance is None: + constraint_tolerance = tolerance + constraints = [] if constraints is None else constraints if marker_weights is None: marker_weights = np.ones(self.nb_markers) @@ -507,8 +587,12 @@ def solve( markers_real, ) if np.linalg.norm(residual) <= tolerance: - success = True - break + constraint_residual, _ = self._constraints_residual_and_jacobian( + q, constraints + ) + if np.linalg.norm(constraint_residual) <= constraint_tolerance: + success = True + break jacobian = self._marker_jacobian( np.array(self.biorbd_model.technicalMarkersJacobian(q))[ @@ -522,8 +606,18 @@ def solve( gradient = jacobian.T @ (weights * residual) lower_bounds, upper_bounds = self._proxqp_bounds(bounds, q) + ( + constraint_residual, + constraint_jacobian, + ) = self._constraints_residual_and_jacobian(q, constraints) dq = self._solve_qp( - hessian, gradient, lower_bounds, upper_bounds, tolerance + hessian, + gradient, + lower_bounds, + upper_bounds, + tolerance, + constraint_jacobian, + -constraint_residual, ) q = q + dq @@ -534,7 +628,13 @@ def solve( ], markers_real, ) - success = np.linalg.norm(residual) <= tolerance + constraint_residual, _ = self._constraints_residual_and_jacobian( + q, constraints + ) + success = ( + np.linalg.norm(residual) <= tolerance + and np.linalg.norm(constraint_residual) <= constraint_tolerance + ) break if self.indices_to_keep[f]: @@ -545,7 +645,13 @@ def solve( ], markers_real, ) - success = success or np.linalg.norm(residual) <= tolerance + constraint_residual, _ = self._constraints_residual_and_jacobian( + q, constraints + ) + success = success or ( + np.linalg.norm(residual) <= tolerance + and np.linalg.norm(constraint_residual) <= constraint_tolerance + ) self.q[:, f] = q message = ( diff --git a/binding/python3/rigid_body.py b/binding/python3/rigid_body.py index 9f94a7b29..5379f8050 100644 --- a/binding/python3/rigid_body.py +++ b/binding/python3/rigid_body.py @@ -6,6 +6,7 @@ DifferentialInverseKinematicsResult, InverseKinematics, InverseKinematicsProxQP, + MarkerLoopClosureConstraint, has_proxsuite, ) diff --git a/examples/python3/inverse_kinematics_loop_constraint.py b/examples/python3/inverse_kinematics_loop_constraint.py new file mode 100644 index 000000000..d5c4520cc --- /dev/null +++ b/examples/python3/inverse_kinematics_loop_constraint.py @@ -0,0 +1,194 @@ +from pathlib import Path +from tempfile import NamedTemporaryFile +import contextlib +import io +import time + +import numpy as np +from scipy import optimize + +import biorbd + + +def _loop_model_with_markers(): + """ + Create a temporary copy of the loop-constrained model with tracking markers. + + The original test model declares the loop constraint, but has no markers. + This example adds two hidden closure markers and six visible tracking + markers so inverse kinematics can be benchmarked. + """ + repo_root = Path(__file__).resolve().parents[2] + model_path = repo_root / "test" / "models" / "loopConstrainedModel.bioMod" + model_text = model_path.read_text() + marker_text = """ +marker loop_pre + parent Segment5 + position 0 0 0 +endmarker + +marker loop_succ + parent Segment3 + position 0 0 0 +endmarker + +marker track_s2a + parent Segment2 + position 0.1 0.2 -0.4 +endmarker + +marker track_s2b + parent Segment2 + position -0.1 0.1 -1.0 +endmarker + +marker track_s3 + parent Segment3 + position 0.1 -0.1 -0.8 +endmarker + +marker track_s4a + parent Segment4 + position -0.1 0.2 -0.4 +endmarker + +marker track_s4b + parent Segment4 + position 0.1 0.1 -1.0 +endmarker + +marker track_s5 + parent Segment5 + position -0.1 -0.1 -0.8 +endmarker + +""" + model_text = model_text.replace( + "\nloopconstraint\n", marker_text + "loopconstraint\n", 1 + ) + model_file = NamedTemporaryFile("w", suffix=".bioMod", delete=False) + model_file.write(model_text) + model_file.close() + return Path(model_file.name) + + +def _markers_to_array(model, q): + return np.array([marker.to_array() for marker in model.technicalMarkers(q)]).T + + +def _loop_gap(model, q): + markers = _markers_to_array(model, q).T + return markers[0] - markers[1] + + +def _project_to_loop(model, q_desired, initial_q): + def residual(q): + return np.concatenate((1000 * _loop_gap(model, q), 0.1 * (q - q_desired))) + + sol = optimize.least_squares( + residual, + initial_q, + max_nfev=200, + xtol=1e-12, + ftol=1e-12, + gtol=1e-12, + ) + return sol.x + + +def _generate_closed_loop_data(model, n_frames): + q = np.zeros(model.nbQ()) + q_ref = np.ndarray((model.nbQ(), n_frames)) + phase = np.linspace(0, 2 * np.pi, n_frames) + + for frame, phase_i in enumerate(phase): + q_desired = 0.15 * np.sin(phase_i + np.arange(model.nbQ()) * 0.3) + q = _project_to_loop(model, q_desired, q) + q_ref[:, frame] = q + + markers = np.ndarray((3, model.nbMarkers(), n_frames)) + for frame in range(n_frames): + markers[:, :, frame] = _markers_to_array(model, q_ref[:, frame]) + + # The closure markers are model-only constraints, not measured markers. + markers[:, 0:2, :] = np.nan + return q_ref, markers + + +def _summarize(model, name, q, ik, elapsed): + sol = ik.sol() + marker_residuals = sol["residuals"] + loop_gaps = np.array( + [np.linalg.norm(_loop_gap(model, q[:, f])) for f in range(q.shape[1])] + ) + return { + "method": name, + "s_per_frame": elapsed / q.shape[1], + "mean_marker_residual_m": np.nanmean(marker_residuals), + "mean_loop_gap_m": loop_gaps.mean(), + "max_loop_gap_m": loop_gaps.max(), + "mean_iterations": np.mean(sol["nfev"]), + } + + +def _run_inverse_kinematics(model, markers, method): + ik = biorbd.InverseKinematics(model, markers) + tic = time.perf_counter() + with contextlib.redirect_stdout(io.StringIO()): + q = ik.solve(method=method) + return q, ik, time.perf_counter() - tic + + +def _run_differential_inverse_kinematics(model, markers, initial_q, constraints): + ik = biorbd.DifferentialInverseKinematics(model, markers) + tic = time.perf_counter() + q = ik.solve( + initial_q=initial_q, + max_iterations=20, + tolerance=1e-8, + constraints=constraints, + constraint_tolerance=1e-8, + verbose=False, + ) + return q, ik, time.perf_counter() - tic + + +def main(n_frames=20): + model_path = _loop_model_with_markers() + model = biorbd.Model(str(model_path)) + q_ref, markers = _generate_closed_loop_data(model, n_frames) + + rows = [] + for method in ("only_lm", "lm", "trf"): + q, ik, elapsed = _run_inverse_kinematics(model, markers, method) + rows.append(_summarize(model, method, q, ik, elapsed)) + + q, ik, elapsed = _run_differential_inverse_kinematics( + model, markers, initial_q=q_ref[:, 0], constraints=None + ) + rows.append(_summarize(model, "proxqp_dik", q, ik, elapsed)) + + loop_constraint = biorbd.MarkerLoopClosureConstraint("loop_pre", "loop_succ") + q, ik, elapsed = _run_differential_inverse_kinematics( + model, markers, initial_q=q_ref[:, 0], constraints=[loop_constraint] + ) + rows.append(_summarize(model, "proxqp_dik_loop", q, ik, elapsed)) + + print( + "method,s_per_frame,mean_marker_residual_m," + "mean_loop_gap_m,max_loop_gap_m,mean_iterations" + ) + for row in rows: + print( + f"{row['method']},{row['s_per_frame']:.8f}," + f"{row['mean_marker_residual_m']:.12g}," + f"{row['mean_loop_gap_m']:.12g}," + f"{row['max_loop_gap_m']:.12g}," + f"{row['mean_iterations']:.3g}" + ) + + return rows + + +if __name__ == "__main__": + main() diff --git a/test/binding/python3/test_inverse_kinematics.py b/test/binding/python3/test_inverse_kinematics.py index 0c13493be..0e5c3decb 100644 --- a/test/binding/python3/test_inverse_kinematics.py +++ b/test/binding/python3/test_inverse_kinematics.py @@ -4,6 +4,8 @@ import pytest import numpy as np +from pathlib import Path +from scipy import optimize brbd_to_test = [] try: @@ -26,6 +28,87 @@ raise RuntimeError("No biorbd version could be imported") +def _loop_model_with_markers(tmp_path): + model_path = tmp_path / "loopConstrainedModelWithMarkers.bioMod" + source_model_path = ( + Path(__file__).parents[2] / "models" / "loopConstrainedModel.bioMod" + ) + with open(source_model_path) as model_file: + model_text = model_file.read() + + marker_text = """ +marker loop_pre + parent Segment5 + position 0 0 0 +endmarker + +marker loop_succ + parent Segment3 + position 0 0 0 +endmarker + +marker track_s2a + parent Segment2 + position 0.1 0.2 -0.4 +endmarker + +marker track_s2b + parent Segment2 + position -0.1 0.1 -1.0 +endmarker + +marker track_s3 + parent Segment3 + position 0.1 -0.1 -0.8 +endmarker + +marker track_s4a + parent Segment4 + position -0.1 0.2 -0.4 +endmarker + +marker track_s4b + parent Segment4 + position 0.1 0.1 -1.0 +endmarker + +marker track_s5 + parent Segment5 + position -0.1 -0.1 -0.8 +endmarker + +""" + model_text = model_text.replace( + "\nloopconstraint\n", marker_text + "loopconstraint\n", 1 + ) + model_path.write_text(model_text) + return model_path + + +def _technical_markers_to_array(model, q): + return np.array([marker.to_array() for marker in model.technicalMarkers(q)]).T + + +def _loop_gap(model, q): + markers = _technical_markers_to_array(model, q).T + return markers[0] - markers[1] + + +def _project_to_loop(model, q_desired, initial_q): + def residual(q): + return np.concatenate((1000 * _loop_gap(model, q), 0.1 * (q - q_desired))) + + sol = optimize.least_squares( + residual, + initial_q, + max_nfev=200, + xtol=1e-12, + ftol=1e-12, + gtol=1e-12, + ) + return sol.x + + @pytest.mark.parametrize("brbd", brbd_to_test) @pytest.mark.parametrize("method", ["only_lm", "lm", "trf"]) def test_solve(brbd, method): @@ -129,6 +212,39 @@ def test_differential_inverse_kinematics_ignores_missing_markers(brbd): assert np.nanmax(residuals[:, 0]) < 1e-8 +@pytest.mark.parametrize("brbd", brbd_to_test) +def test_differential_inverse_kinematics_with_marker_loop_constraint(brbd, tmp_path): + if brbd.backend == brbd.CASADI: + pytest.skip("Skip inverse kinematics for biorbd_casadi") + if not brbd.has_proxsuite: + pytest.skip("proxsuite is not installed") + + biorbd_model = brbd.Model(str(_loop_model_with_markers(tmp_path))) + q_closed = _project_to_loop( + biorbd_model, + 0.1 * np.sin(np.arange(biorbd_model.nbQ()) * 0.3), + np.zeros(biorbd_model.nbQ()), + ) + + markers = np.ndarray((3, biorbd_model.nbMarkers(), 1)) + markers[:, :, 0] = _technical_markers_to_array(biorbd_model, q_closed) + markers[:, 0:2, 0] = np.nan + + ik = brbd.DifferentialInverseKinematics(biorbd_model, markers) + q = ik.solve( + initial_q=q_closed, + max_iterations=5, + tolerance=1e-8, + constraints=[brbd.MarkerLoopClosureConstraint("loop_pre", "loop_succ")], + constraint_tolerance=1e-8, + verbose=False, + ) + + np.testing.assert_almost_equal( + np.linalg.norm(_loop_gap(biorbd_model, q[:, 0])), 0 + ) + + if __name__ == "__main__": for brbd in brbd_to_test: for method in ["only_lm", "lm", "trf"]: From 4b25d45936df31be935d1e0cb0f35f9a3d169c50 Mon Sep 17 00:00:00 2001 From: mickaelbegon Date: Fri, 12 Jun 2026 12:08:29 -0400 Subject: [PATCH 6/6] Add TRF loop benchmark comparison --- .../inverse_kinematics_loop_constraint.py | 66 ++++++++++++++++++- 1 file changed, 64 insertions(+), 2 deletions(-) diff --git a/examples/python3/inverse_kinematics_loop_constraint.py b/examples/python3/inverse_kinematics_loop_constraint.py index d5c4520cc..4450bd88f 100644 --- a/examples/python3/inverse_kinematics_loop_constraint.py +++ b/examples/python3/inverse_kinematics_loop_constraint.py @@ -117,7 +117,10 @@ def _generate_closed_loop_data(model, n_frames): def _summarize(model, name, q, ik, elapsed): sol = ik.sol() - marker_residuals = sol["residuals"] + return _summarize_q(model, name, q, elapsed, sol["residuals"], sol["nfev"]) + + +def _summarize_q(model, name, q, elapsed, marker_residuals, iterations): loop_gaps = np.array( [np.linalg.norm(_loop_gap(model, q[:, f])) for f in range(q.shape[1])] ) @@ -127,7 +130,7 @@ def _summarize(model, name, q, ik, elapsed): "mean_marker_residual_m": np.nanmean(marker_residuals), "mean_loop_gap_m": loop_gaps.mean(), "max_loop_gap_m": loop_gaps.max(), - "mean_iterations": np.mean(sol["nfev"]), + "mean_iterations": np.mean(iterations), } @@ -139,6 +142,58 @@ def _run_inverse_kinematics(model, markers, method): return q, ik, time.perf_counter() - tic +def _visible_marker_residuals(model, q, markers_real, indices_to_keep): + markers_model = _markers_to_array(model, q) + residuals = markers_model[:, indices_to_keep] - markers_real[:, indices_to_keep] + return residuals.T.reshape(-1) + + +def _run_trf_loop(model, markers, initial_q, loop_weight=100): + q_out = np.ndarray((model.nbQ(), markers.shape[2])) + marker_residuals = np.ndarray((model.nbMarkers(), markers.shape[2])) * np.nan + iterations = [] + q = initial_q.copy() + tic = time.perf_counter() + + for frame in range(markers.shape[2]): + indices_to_keep = list( + np.unique(np.isfinite(markers[:, :, frame]).nonzero()[1]) + ) + + def residual(q_tp): + return np.concatenate( + ( + _visible_marker_residuals( + model, q_tp, markers[:, :, frame], indices_to_keep + ), + loop_weight * _loop_gap(model, q_tp), + ) + ) + + sol = optimize.least_squares( + residual, + q, + method="trf", + xtol=1e-10, + ftol=1e-10, + gtol=1e-10, + max_nfev=200, + ) + q = sol.x + q_out[:, frame] = q + iterations.append(sol.nfev) + + marker_residual_vector = _visible_marker_residuals( + model, q, markers[:, :, frame], indices_to_keep + ) + marker_residuals[indices_to_keep, frame] = np.linalg.norm( + marker_residual_vector.reshape(-1, 3), axis=1 + ) + + elapsed = time.perf_counter() - tic + return q_out, marker_residuals, iterations, elapsed + + def _run_differential_inverse_kinematics(model, markers, initial_q, constraints): ik = biorbd.DifferentialInverseKinematics(model, markers) tic = time.perf_counter() @@ -163,6 +218,13 @@ def main(n_frames=20): q, ik, elapsed = _run_inverse_kinematics(model, markers, method) rows.append(_summarize(model, method, q, ik, elapsed)) + q, marker_residuals, iterations, elapsed = _run_trf_loop( + model, markers, initial_q=q_ref[:, 0] + ) + rows.append( + _summarize_q(model, "trf_loop", q, elapsed, marker_residuals, iterations) + ) + q, ik, elapsed = _run_differential_inverse_kinematics( model, markers, initial_q=q_ref[:, 0], constraints=None )