Allow disabling limits

CHANGELOG.md

@@ -13,8 +13,9 @@ All notable changes to this project will be documented in this file.
   - Whole-body Self-Collision Avoidance Barrier ``SelfCollisionBarrier``
 - `ComTask` for Center of Mass tracking.
 - **Breaking:** Updated the logic for handling the joint limits:
+  - Add a ``limits`` argument to ``build_ik`` and ``solve_ik``
   - The `check_limits` method now includes an optional `safety_break` argument to control whether execution should stop on exception.
-  - The solve_ik function now includes the `safety_break` that is forwarded to `check_limits`.
+  - The ``solve_ik`` function now includes the `safety_break` that is forwarded to `check_limits`.
 - Example: UR5 manipulator and GO2 quadruped robot with `PositionBarrier`
 - Example: YUMI two-armed manipulator with `BodySphericalBarrier`
 - Example: G1 humanoid squatting through regulating CoM.

README.md

@@ -139,6 +139,12 @@ Pink works with all kinds of robot morphologies:
 
 Check out the examples directory for more code.
 
+## Frequently Asked Questions
+
+- [Can I solve **global** inverse kinematics?](https://github.com/stephane-caron/pink/discussions/66#discussioncomment-8224315)
+- [Can I make velocities smoother?](https://github.com/stephane-caron/pink/discussions/103)
+- [My configuration gets stuck somewhere and does not solve the task, what is going on?](https://github.com/stephane-caron/pink/discussions/66#discussioncomment-8224315)
+
 ## Global inverse kinematics
 
 Pink implements differential inverse kinematics, a first-order algorithm that converges to the closest optimum of its cost function. It is a **local** method that does not solve the more difficult problem of [global inverse kinematics](https://github.com/stephane-caron/pink/discussions/66). That is, it may converge to a global optimum, or to a local one stuck to some configuration limits. This behavior is illustrated in the [simple pendulum with configuration limit](https://github.com/stephane-caron/pink/blob/main/examples/simple_pendulum_configuration_limit.py) example.

examples/barriers/arm_ur5.py

@@ -113,7 +113,7 @@
         )
         configuration.integrate_inplace(velocity, dt)
 
-        G, h = pos_barrier.compute_qp_inequality(configuration, dt=dt)
+        G, h = pos_barrier.compute_qp_inequalities(configuration, dt=dt)
         distance_to_manipulator = configuration.get_transform_frame_to_world(
             "ee_link"
         ).translation[1]

pink/barriers/barrier.py

@@ -203,7 +203,7 @@ def compute_qp_objective(
 
         return (H, c)
 
-    def compute_qp_inequality(
+    def compute_qp_inequalities(
         self,
         configuration: Configuration,
         dt: float = 1e-3,

pink/configuration.py

@@ -12,6 +12,7 @@
 and frame Jacobians used for IK can be queried.
 """
 
+import logging
 from typing import Optional
 
 import numpy as np
@@ -20,7 +21,6 @@
 from .exceptions import FrameNotFound, NotWithinConfigurationLimits
 from .limits import ConfigurationLimit, VelocityLimit
 from .utils import VectorSpace, get_root_joint_dim
-import logging
 
 
 class Configuration:
@@ -38,10 +38,11 @@ class Configuration:
     ``pin.forwardKinematics(model, data, configuration)``.) The latter updates
     frame placements.
 
-    Additionally, if collision model is provided, it is used to evaluate distances
-    between frames using following functions:
-    - ``pin.computeCollisions(model, data, collision_model, collision_data, q)``
-    - ``pin.updateGeometryPlacements(model, data, collision_model, collision_data, q)``
+    Additionally, if collision model is provided, it is used to evaluate
+    distances between frames using following functions:
+
+    - ``pin.computeCollisions(model, data, collision_model, collision_data, q)`` # noqa: E501
+    - ``pin.updateGeometryPlacements(model, data, collision_model, collision_data, q)``  # noqa: E501
 
     Notes:
         This class is meant to be used as a subclass of pin.RobotWrapper, not
@@ -56,10 +57,10 @@ class Configuration:
         q: Configuration vector for the robot model.
     """
 
+    collision_data: pin.GeometryData
+    collision_model: pin.GeometryModel
     data: pin.Data
     model: pin.Model
-    collision_model: pin.GeometryModel
-    collision_data: pin.GeometryData
     q: np.ndarray
 
     def __init__(
@@ -82,10 +83,12 @@ def __init__(
                 data. Otherwise, work on the input data directly.
             forward_kinematics: If true (default), compute forward kinematics
                 from the q into the internal data.
-            collision_model: collision geometry model, with already loaded collisions.
-                Default is None, meaning no collisions are processeed.
-            collision_data: collision geometry data, with already loaded collisions.
-                Default is None, which generates it from the model if possible, None otherwise.
+            collision_model: collision geometry model, with already loaded
+                collisions. Default is None, meaning no collisions are
+                processeed.
+            collision_data: collision geometry data, with already loaded
+                collisions. Default is None, which generates it from the model
+                if possible, None otherwise.
 
         Notes:
             Configurations copy data and run forward kinematics by default so
@@ -161,9 +164,9 @@ def check_limits(
 
         Args:
             tol: Tolerance in radians.
-            safe_break (bool): If True, stop execution and raise an exception if
-                the current configuration is outside limits. If False, print a warning
-                and continue execution.
+            safe_break (bool): If True, stop execution and raise an exception
+                if the current configuration is outside limits. If False, print
+                a warning and continue execution.
 
         Raises:
             NotWithinConfigurationLimits: If the current configuration is

pink/limits/__init__.py

@@ -6,10 +6,12 @@
 
 """Limits implemented as inequality constraints in the IK problem."""
 
+from .limit import Limit
 from .configuration_limit import ConfigurationLimit
 from .velocity_limit import VelocityLimit
 
 __all__ = [
     "ConfigurationLimit",
+    "Limit",
     "VelocityLimit",
 ]

pink/solve_ik.py

@@ -13,6 +13,7 @@
 
 from .barriers import Barrier
 from .configuration import Configuration
+from .limits import Limit
 from .tasks import Task
 
 
@@ -66,13 +67,15 @@ def __compute_qp_objective(
 
 def __compute_qp_inequalities(
     configuration,
+    limits: Optional[Iterable[Limit]],
     dt: float,
     barriers: Optional[Iterable[Barrier]] = None,
 ) -> Tuple[Optional[np.ndarray], Optional[np.ndarray]]:
     r"""Compute inequality constraints for the quadratic program.
 
     Args:
         configuration: Robot configuration to read kinematics from.
+        limits: Collection of limits.
         dt: Integration timestep in [s].
         barriers: Collection of barriers.
 
@@ -86,20 +89,22 @@ def __compute_qp_inequalities(
         solvers don't support it. See for instance
         https://github.com/stephane-caron/pink/issues/10.
     """
+    if limits is None:
+        configuration_limit = configuration.model.configuration_limit
+        velocity_limit = configuration.model.velocity_limit
+        limits = (configuration_limit, velocity_limit)
     barriers = barriers if barriers is not None else []
-    configuration_limit = configuration.model.configuration_limit
-    velocity_limit = configuration.model.velocity_limit
-    q = configuration.q
-
     G_list = []
     h_list = []
-    for limit in (configuration_limit, velocity_limit):
-        matvec = limit.compute_qp_inequalities(q, dt)
+    for limit in limits:
+        matvec = limit.compute_qp_inequalities(configuration.q, dt)
         if matvec is not None:
             G_list.append(matvec[0])
             h_list.append(matvec[1])
     for barrier in barriers:
-        G_barrier, h_barrier = barrier.compute_qp_inequality(configuration, dt)
+        G_barrier, h_barrier = barrier.compute_qp_inequalities(
+            configuration, dt
+        )
         G_list.append(G_barrier)
         h_list.append(h_barrier)
     if not G_list:
@@ -112,6 +117,7 @@ def build_ik(
     tasks: Iterable[Task],
     dt: float,
     damping: float = 1e-12,
+    limits: Optional[Iterable[Limit]] = None,
     barriers: Optional[Iterable[Barrier]] = None,
 ) -> qpsolvers.Problem:
     r"""Build quadratic program from current configuration and tasks.
@@ -139,13 +145,16 @@ def build_ik(
             :math:`[\mathrm{tangent}]` is "the" unit of robot velocities.
             Improves numerical stability, but larger values slow down all
             tasks.
+        limits: Collection of limits to enforce. By default, consists of
+            configuration and velocity limits (set to the empty list ``[]`` to
+            disable limits).
         barriers: Collection of barriers.
 
     Returns:
         Quadratic program of the inverse kinematics problem.
     """
     P, q = __compute_qp_objective(configuration, tasks, damping, barriers)
-    G, h = __compute_qp_inequalities(configuration, dt, barriers)
+    G, h = __compute_qp_inequalities(configuration, limits, dt, barriers)
     problem = qpsolvers.Problem(P, q, G, h)
     return problem
 
@@ -155,8 +164,9 @@ def solve_ik(
     tasks: Iterable[Task],
     dt: float,
     solver: str,
-    barriers: Optional[Iterable[Barrier]] = None,
     damping: float = 1e-12,
+    limits: Optional[Iterable[Limit]] = None,
+    barriers: Optional[Iterable[Barrier]] = None,
     safety_break: bool = True,
     **kwargs,
 ) -> np.ndarray:
@@ -175,10 +185,13 @@ def solve_ik(
             :math:`[\mathrm{tangent}]` is "the" unit of robot velocities.
             Improves numerical stability, but larger values slow down all
             tasks.
+        limits: Collection of limits to enforce. By default, consists of
+            configuration and velocity limits (set to the empty list ``[]`` to
+            disable limits).
         barriers: Collection of barriers functions.
         safety_break: If True, stop execution and raise an exception if
-                the current configuration is outside limits. If False, print a warning
-                and continue execution.
+            the current configuration is outside limits. If False, print a
+            warning and continue execution.
         kwargs: Keyword arguments to forward to the backend QP solver.
 
     Returns:
@@ -193,14 +206,13 @@ def solve_ik(
         homogeneous. If it helps we can add a tangent-space scaling to damp the
         floating base differently from joint angular velocities.
     """
-
     configuration.check_limits(safety_break=safety_break)
-
     problem = build_ik(
         configuration,
         tasks,
         dt,
         damping,
+        limits,
         barriers,
     )
     result = qpsolvers.solve_problem(problem, solver=solver, **kwargs)

tests/test_barrier.py

@@ -22,7 +22,9 @@ class TestBarrier(unittest.TestCase):
     def setUp(self):
         """Set up test fixtures."""
 
-        self.robot = load_robot_description("upkie_description", root_joint=pin.JointModelFreeFlyer())
+        self.robot = load_robot_description(
+            "upkie_description", root_joint=pin.JointModelFreeFlyer()
+        )
         self.conf = Configuration(
             self.robot.model,
             self.robot.data,
@@ -32,9 +34,11 @@ def setUp(self):
 
     def test_diff_form_dimensions(self):
         """Velocity barrier dimension is the number of bounded joints."""
-        for barrier in [PositionBarrier("left_hip", p_min=np.zeros(3), p_max=np.zeros(3))]:
+        for barrier in [
+            PositionBarrier("left_hip", p_min=np.zeros(3), p_max=np.zeros(3))
+        ]:
             H, c = barrier.compute_qp_objective(self.conf)
-            G, h = barrier.compute_qp_inequality(self.conf, self.dt)
+            G, h = barrier.compute_qp_inequalities(self.conf, self.dt)
             self.assertEqual(H.shape[0], self.robot.nv)
             self.assertEqual(H.shape[1], self.robot.nv)
             self.assertEqual(c.shape[0], self.robot.nv)
@@ -44,21 +48,27 @@ def test_diff_form_dimensions(self):
     def test_barrier_value_dimension(self):
         """Test tha shape of value in all barriers is correct."""
 
-        for barrier in [PositionBarrier("left_hip", p_min=np.zeros(3), p_max=np.zeros(3))]:
+        for barrier in [
+            PositionBarrier("left_hip", p_min=np.zeros(3), p_max=np.zeros(3))
+        ]:
             v = barrier.compute_barrier(self.conf)
             self.assertEqual(v.shape[0], barrier.dim)
 
     def test_barrier_jacobians_dimension(self):
         """Test that shapes of jacobians in all barriers are correct."""
 
-        for barrier in [PositionBarrier("left_hip", p_min=np.zeros(3), p_max=np.zeros(3))]:
+        for barrier in [
+            PositionBarrier("left_hip", p_min=np.zeros(3), p_max=np.zeros(3))
+        ]:
             J = barrier.compute_jacobian(self.conf)
             self.assertEqual(J.shape[0], barrier.dim)
             self.assertEqual(J.shape[1], self.robot.nv)
 
     def test_barrier_without_penalty_weight(self):
         """Test that objective is zero if no penalty weight is provided."""
-        for barrier in [PositionBarrier("left_hip", p_min=np.zeros(3), p_max=np.zeros(3))]:
+        for barrier in [
+            PositionBarrier("left_hip", p_min=np.zeros(3), p_max=np.zeros(3))
+        ]:
             H, c = barrier.compute_qp_objective(self.conf)
             self.assertTrue(np.allclose(H, 0))
             self.assertTrue(np.allclose(c, 0))
@@ -80,7 +90,11 @@ def test_barrier_penalty_weight(self):
 
     def test_task_repr(self):
         """Test task string representation."""
-        for limit in [PositionBarrier("universe", safe_displacement_gain=0.0, p_min=np.zeros(3))]:
+        for limit in [
+            PositionBarrier(
+                "universe", safe_displacement_gain=0.0, p_min=np.zeros(3)
+            )
+        ]:
             self.assertTrue("gain=" in repr(limit))
             self.assertTrue("safe_displacement=" in repr(limit))
             self.assertTrue("safe_displacement_gain" in repr(limit))
@@ -89,12 +103,14 @@ def test_task_repr(self):
     def test_cached(self):
         """Test that cached results are reused."""
 
-        barrier = PositionBarrier("left_hip", p_min=np.zeros(3), p_max=np.zeros(3))
+        barrier = PositionBarrier(
+            "left_hip", p_min=np.zeros(3), p_max=np.zeros(3)
+        )
         barrier.compute_qp_objective(self.conf)
         # Check that cache is initially triggered
         self.assertIsNotNone(barrier._Barrier__q_cache)
 
         # Check that cache is resetted after update
         self.conf.update(self.conf.q + 1.0)
-        barrier.compute_qp_inequality(self.conf)
+        barrier.compute_qp_inequalities(self.conf)
         self.assertTrue(np.allclose(barrier._Barrier__q_cache, self.conf.q))