diff --git a/CHANGELOG.md b/CHANGELOG.md index 02adbdbb..577cb623 100644 --- a/CHANGELOG.md +++ b/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. diff --git a/README.md b/README.md index f77c9dc2..db3900b1 100644 --- a/README.md +++ b/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. diff --git a/examples/barriers/arm_ur5.py b/examples/barriers/arm_ur5.py index 31643d13..52569377 100644 --- a/examples/barriers/arm_ur5.py +++ b/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] diff --git a/pink/barriers/barrier.py b/pink/barriers/barrier.py index 76b3fb2d..a1eda5f7 100644 --- a/pink/barriers/barrier.py +++ b/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, diff --git a/pink/configuration.py b/pink/configuration.py index 90ad0102..01e8b31d 100644 --- a/pink/configuration.py +++ b/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 diff --git a/pink/limits/__init__.py b/pink/limits/__init__.py index 261f2338..b140c795 100644 --- a/pink/limits/__init__.py +++ b/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", ] diff --git a/pink/solve_ik.py b/pink/solve_ik.py index a23ed0b1..38cb208b 100644 --- a/pink/solve_ik.py +++ b/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,6 +67,7 @@ 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]]: @@ -73,6 +75,7 @@ def __compute_qp_inequalities( 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) diff --git a/tests/test_barrier.py b/tests/test_barrier.py index e6e5c684..2495591f 100644 --- a/tests/test_barrier.py +++ b/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))