Introduction of Barriers, Position barrier

CHANGELOG.md

@@ -3,10 +3,15 @@
 All notable changes to this project will be documented in this file.
 
 ## Unreleased
-
+### Added
+- Control Barrier Functions, namely:
+  - Abstract Barrier ``Barrier``
+  - Frame Position Barrier ``PositionBarrier``
+- Examples for `UR5` manipulator and `go2` quadruped robot, which illustrate barriers effect.
+
 ### Changed
-
 - CICD: Update ruff to 0.4.3
+- Configuration accepts list of Control Barrier Functions
 
 ### Fixed
 

doc/barriers.rst

@@ -0,0 +1,16 @@
+:github_url: https://github.com/stephane-caron/pink/tree/main/doc/barriers.rst
+
+.. _Barriers:
+
+********
+Barriers
+********
+
+.. automodule:: pink.barriers
+    :members:
+
+Position Barrier
+=================
+
+.. automodule:: pink.barriers.position_barrier
+    :members:

doc/index.rst

@@ -20,6 +20,8 @@ Inverse kinematics in Pink is defined by weighted :ref:`tasks <Tasks>` and :ref:
     introduction.rst
     tasks.rst
     limits.rst
+    barriers.rst
     inverse-kinematics.rst
     developer-notes.rst
     references.rst
+

examples/barriers/README.md

@@ -0,0 +1,48 @@
+Here we will have the examples of using barriers, please go over [notes](https://simeon-ned.com/blog/2024/cbf/) for more info. 
+
+# Barrier Examples
+
+
+- [Arm: UR5](#arm-ur5): with joints and end effector limits
+- [Quadruped: Go2](#go2-squat): Go 2 squatting with floating base position limits
+- [Dual Arms: Yumi](#dual-arm-yumi): self collisions with spheres
+
+## Arm UR5
+
+A UR5 arm tracking a moving target while stopping in front of virtual wall:
+
+
+https://github.com/domrachev03/pink/assets/28687492/f30ba7a1-98a3-44cb-ab52-23f99e42714c
+
+
+| Task | Cost |
+|------|------|
+| End-effector | (50,1) |
+| Posture | $10^{-3}$ |
+
+| Barrier | Gain |
+|------|------|
+| End-effector | $10^{2}$ |
+
+## Go2 squat
+
+Go2 quadruped squating with base position is constrained by z and y coordinates:
+
+https://github.com/domrachev03/pink/assets/26837717/701bdfbe-0dba-4f9d-80e2-c018475f38d6
+
+
+
+| Task | Cost |
+|------|------|
+| Base | (50, 1) |
+| Legs | 200 |
+| Posture | $10^{-3}$ |
+
+| Barrier | Gain |
+|------|------|
+| End-effector | $10^{2}$ |
+
+
+
+
+

examples/barriers/arm_ur5.py

@@ -0,0 +1,117 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+#
+# SPDX-License-Identifier: Apache-2.0
+# Copyright 2024 Stéphane Caron, Ivan Domrachev, Simeon Nedelchev
+
+"""Universal Robots UR5 arm tracking a moving target."""
+
+import meshcat_shapes
+import numpy as np
+import qpsolvers
+from loop_rate_limiters import RateLimiter
+
+import pink
+from pink import solve_ik
+from pink.barriers import PositionBarrier
+from pink.tasks import FrameTask, PostureTask
+from pink.visualization import start_meshcat_visualizer
+
+try:
+    from robot_descriptions.loaders.pinocchio import load_robot_description
+except ModuleNotFoundError as exc:
+    raise ModuleNotFoundError(
+        "Examples need robot_descriptions, "
+        "try ``pip install robot_descriptions``"
+    ) from exc  # noqa: E501
+
+
+if __name__ == "__main__":
+    robot = load_robot_description("ur5_description", root_joint=None)
+    viz = start_meshcat_visualizer(robot)
+
+    end_effector_task = FrameTask(
+        "ee_link",
+        position_cost=10.0,  # [cost] / [m]
+        orientation_cost=1.0,  # [cost] / [rad]
+    )
+
+    posture_task = PostureTask(
+        cost=1e-3,  # [cost] / [rad]
+    )
+
+    pos_barrier = PositionBarrier(
+        "ee_link",
+        indices=[1],
+        p_min=np.array([-0.4]),
+        p_max=np.array([0.6]),
+        gain=np.array([100.0]),
+        safe_displacement_gain=1.0,
+    )
+    barriers = [pos_barrier]
+
+    tasks = [end_effector_task, posture_task]
+
+    q_ref = np.array(
+        [
+            1.27153374,
+            -0.87988708,
+            1.89104795,
+            1.73996951,
+            -0.24610945,
+            -0.74979019,
+        ]
+    )
+    configuration = pink.Configuration(robot.model, robot.data, q_ref)
+    for task in tasks:
+        task.set_target_from_configuration(configuration)
+    viz.display(configuration.q)
+
+    viewer = viz.viewer
+    meshcat_shapes.frame(viewer["end_effector_target"], opacity=0.5)
+    meshcat_shapes.frame(viewer["end_effector"], opacity=1.0)
+
+    # Select QP solver
+    solver = qpsolvers.available_solvers[0]
+    if "osqp" in qpsolvers.available_solvers:
+        solver = "osqp"
+
+    rate = RateLimiter(frequency=200.0)
+    dt = rate.period
+    t = 0.0  # [s]
+    while True:
+        # Update task targets
+        end_effector_target = end_effector_task.transform_target_to_world
+        end_effector_target.translation[1] = 0.0 + 0.65 * np.sin(t / 4)
+        end_effector_target.translation[2] = 0.5
+
+        # Update visualization frames
+        viewer["end_effector_target"].set_transform(end_effector_target.np)
+        viewer["end_effector"].set_transform(
+            configuration.get_transform_frame_to_world(
+                end_effector_task.frame
+            ).np
+        )
+
+        velocity = solve_ik(
+            configuration,
+            tasks,
+            dt,
+            solver=solver,
+            barriers=barriers,
+        )
+        configuration.integrate_inplace(velocity, dt)
+
+        G, h = pos_barrier.compute_qp_inequality(configuration, dt=dt)
+        print(f"Task error: {end_effector_task.compute_error(configuration)}")
+        print(
+            f"Position CBF value: {pos_barrier.compute_barrier(configuration)[0]:0.3f} >= 0"
+        )
+        print(
+            f"""Distance to manipulator: {configuration.get_transform_frame_to_world('ee_link').translation[1]} <= 0.6"""
+        )
+        print("-" * 60)
+        # Visualize result at fixed FPS
+        viz.display(configuration.q)
+        rate.sleep()
+        t += dt

examples/barriers/go2_squat.py

@@ -0,0 +1,137 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+#
+# SPDX-License-Identifier: Apache-2.0
+# Copyright 2024 Stéphane Caron, Ivan Domrachev, Simeon Nedelchev
+
+"""Go2 squat with z-axis barrier."""
+
+import meshcat_shapes
+import numpy as np
+import qpsolvers
+from loop_rate_limiters import RateLimiter
+import pinocchio as pin
+
+import pink
+from pink import solve_ik
+from pink.barriers import PositionBarrier
+from pink.tasks import FrameTask, PostureTask
+from pink.visualization import start_meshcat_visualizer
+
+try:
+    from robot_descriptions.loaders.pinocchio import load_robot_description
+except ModuleNotFoundError as exc:
+    raise ModuleNotFoundError(
+        "Examples need robot_descriptions, "
+        "try ``pip install robot_descriptions``"
+    ) from exc  # noqa: E501
+
+
+if __name__ == "__main__":
+    robot = load_robot_description(
+        "go2_description", root_joint=pin.JointModelFreeFlyer()
+    )
+    viz = start_meshcat_visualizer(robot)
+
+    q_ref = np.array(
+        [
+            -0.0,
+            0.0,
+            0.3,
+            0.0,
+            0.0,
+            0.0,
+            1.0,
+            0.0,
+            0.8,
+            -1.57,
+            0.0,
+            0.8,
+            -1.57,
+            0.0,
+            0.8,
+            -1.57,
+            0.0,
+            0.8,
+            -1.57,
+        ]
+    )
+
+    configuration = pink.Configuration(robot.model, robot.data, q_ref)
+
+    base_task = FrameTask(
+        "base",
+        position_cost=50.0,  # [cost] / [m]
+        orientation_cost=1.0,  # [cost] / [rad]
+    )
+
+    posture_task = PostureTask(
+        cost=1e-5,  # [cost] / [rad]
+    )
+
+    pos_barrier = PositionBarrier(
+        "base",
+        indices=[1, 2],
+        p_max=np.array([0, 0.35]),
+        gain=np.array([100.0, 100.0]),
+        safe_displacement_gain=1.0,
+    )
+    barriers = [pos_barrier]
+
+    tasks = [base_task, posture_task]
+
+    for foot in ["FL_foot", "FR_foot", "RL_foot", "RR_foot"]:
+        task = FrameTask(
+            foot,
+            position_cost=200.0,  # [cost] / [m]
+            orientation_cost=0.0,  # [cost] / [rad]
+        )
+        tasks.append(task)
+
+    for task in tasks:
+        task.set_target_from_configuration(configuration)
+
+    viewer = viz.viewer
+    opacity = 0.5  # Set the desired opacity level (0 transparent, 1 opaque)
+
+    meshcat_shapes.frame(viewer["base_target"], opacity=1.0)
+    meshcat_shapes.frame(viewer["base"], opacity=1.0)
+
+    # Select QP solver
+    solver = qpsolvers.available_solvers[0]
+    if "osqp" in qpsolvers.available_solvers:
+        solver = "osqp"
+
+    rate = RateLimiter(frequency=200.0)
+    dt = rate.period
+    t = 0.0  # [s]
+    period = 4
+    omega = 2 * np.pi / period
+    while True:
+        # Update task targets
+        end_effector_target = base_task.transform_target_to_world
+        phase = (t // period) % 2
+        Ay = 0.1 * (1 - phase)
+        Az = 0.2 * phase
+
+        end_effector_target.translation[1] = 0.0 + Ay * np.sin(omega * t)
+        end_effector_target.translation[2] = 0.3 + Az * np.sin(omega * t)
+
+        # Update visualization frames
+        viewer["base_target"].set_transform(end_effector_target.np)
+        viewer["base"].set_transform(
+            configuration.get_transform_frame_to_world(base_task.frame).np
+        )
+
+        velocity = solve_ik(
+            configuration,
+            tasks,
+            dt,
+            solver=solver,
+            barriers=barriers,
+        )
+        configuration.integrate_inplace(velocity, dt)
+
+        viz.display(configuration.q)
+        rate.sleep()
+        t += dt