Whole Body Self Collision Barrier

CHANGELOG.md

@@ -10,7 +10,14 @@ All notable changes to this project will be documented in this file.
   - Abstract Barrier ``Barrier``
   - Frame Position Barrier ``PositionBarrier``
   - Body Spherical Barrier ``BodySphericalBarrier``
-- Examples for `UR5` manipulator and `go2` quadruped robot, and `yumi` two-armed manipulator which illustrate barriers effect.
+  - Whole-body Self-Collision Avoidance Barrier ``SelfCollisionBarrier``
+- `ComTask` for Center of Mass tracking.
+- **Breaking:** Updated the logic for handling the joint limits:
+  - 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`.
+- Example: UR5 manipulator and GO2 quadruped robot with `PositionBarrier`
+- Example: YUMI two-armed manipulator with `BodySphericalBarrier`
+- Example: G1 humanoid squatting through regulating CoM.
 
 ### Changed
 

doc/barriers.rst

@@ -17,3 +17,15 @@ Position barrier
 
 .. automodule:: pink.barriers.position_barrier
     :members:
+
+Body Spherical barrier
+======================
+
+.. automodule:: pink.barriers.body_spherical_barrier
+    :members:
+
+Whole Body Collision Avoidance
+==============================
+
+.. automodule:: pink.barriers.self_collision_barrier
+    :members:

examples/barriers/README.md

@@ -43,7 +43,7 @@ https://github.com/domrachev03/pink/assets/28687492/78281f44-3676-4d4d-9619-768b
 | End-effector | $10^{2}$ |
 
 
-## Yumi self-collision avoidance
+## Yumi end-effector self-collision avoidance
 Yumi two-armed manimpulator with constraint on minimal distance between frames, defined by end-effectors
 
 
@@ -60,3 +60,21 @@ https://github.com/domrachev03/pink/assets/28687492/f8c4bc8d-63e3-4bf7-a34f-e7ed
 | Barrier | Gain |
 |------|------|
 | Body Spherical | $10^{2}$ |
+
+## Iiwa whole-body collision avoidance
+Two iiwas with custom collision geometry with some barely feasible tasks for end-effectors.
+
+
+https://github.com/domrachev03/pink/assets/28687492/d64163b6-399f-4bbf-ac50-1135fa69c2da
+
+
+
+| Task | Cost |
+|------|------|
+| End-effector | (50,10) |
+| Posture | $10^{-3}$ |
+
+| Barrier | Gain |
+|------|------|
+| Self Collision Avoidance | $10$ |
+

examples/barriers/iiwa14_spheres_collision.srdf

@@ -0,0 +1,67 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!--This does not replace URDF, and is not an extension of URDF.
+    This is a format for representing semantic information about the robot structure.
+    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
+-->
+<robot name="iiwa14">
+    <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
+    <disable_collisions link1="left_iiwa_link_0" link2="left_iiwa_link_1" reason="Adjacent"/>
+    <disable_collisions link1="left_iiwa_link_1" link2="left_iiwa_link_2" reason="Adjacent"/>
+    <disable_collisions link1="left_iiwa_link_2" link2="left_iiwa_link_3" reason="Adjacent"/>
+    <disable_collisions link1="left_iiwa_link_3" link2="left_iiwa_link_4" reason="Adjacent"/>
+    <disable_collisions link1="left_iiwa_link_4" link2="left_iiwa_link_5" reason="Adjacent"/>
+    <disable_collisions link1="left_iiwa_link_5" link2="left_iiwa_link_6" reason="Adjacent"/>
+    <disable_collisions link1="left_iiwa_link_5" link2="left_iiwa_link_7" reason="Adjacent"/>
+    <disable_collisions link1="left_iiwa_link_6" link2="left_iiwa_link_7" reason="Adjacent"/>
+
+    <disable_collisions link1="left_iiwa_link_1" link2="left_iiwa_link_3" reason="Never"/>
+    <disable_collisions link1="left_iiwa_link_1" link2="left_iiwa_link_4" reason="Never"/>
+    <disable_collisions link1="left_iiwa_link_1" link2="left_iiwa_link_5" reason="Never"/>
+    <disable_collisions link1="left_iiwa_link_1" link2="left_iiwa_link_6" reason="Never"/>
+    <disable_collisions link1="left_iiwa_link_1" link2="left_iiwa_link_7" reason="Never"/>
+
+    <disable_collisions link1="left_iiwa_link_2" link2="left_iiwa_link_4" reason="Never"/>
+    <disable_collisions link1="left_iiwa_link_2" link2="left_iiwa_link_5" reason="Never"/>
+    <disable_collisions link1="left_iiwa_link_2" link2="left_iiwa_link_6" reason="Never"/>
+    <disable_collisions link1="left_iiwa_link_2" link2="left_iiwa_link_7" reason="Never"/>
+
+    <disable_collisions link1="left_iiwa_link_3" link2="left_iiwa_link_5" reason="Never"/>
+    <disable_collisions link1="left_iiwa_link_3" link2="left_iiwa_link_6" reason="Never"/>
+    <disable_collisions link1="left_iiwa_link_3" link2="left_iiwa_link_7" reason="Never"/>
+
+    <disable_collisions link1="left_iiwa_link_4" link2="left_iiwa_link_6" reason="Never"/>
+    <disable_collisions link1="left_iiwa_link_4" link2="left_iiwa_link_7" reason="Never"/>
+
+    <disable_collisions link1="right_iiwa_link_0" link2="right_iiwa_link_1" reason="Adjacent"/>
+    <disable_collisions link1="right_iiwa_link_1" link2="right_iiwa_link_2" reason="Adjacent"/>
+    <disable_collisions link1="right_iiwa_link_2" link2="right_iiwa_link_3" reason="Adjacent"/>
+    <disable_collisions link1="right_iiwa_link_3" link2="right_iiwa_link_4" reason="Adjacent"/>
+    <disable_collisions link1="right_iiwa_link_4" link2="right_iiwa_link_5" reason="Adjacent"/>
+    <disable_collisions link1="right_iiwa_link_5" link2="right_iiwa_link_6" reason="Adjacent"/>
+    <disable_collisions link1="right_iiwa_link_5" link2="right_iiwa_link_7" reason="Adjacent"/>
+    <disable_collisions link1="right_iiwa_link_6" link2="right_iiwa_link_7" reason="Adjacent"/>
+
+    <disable_collisions link1="right_iiwa_link_0" link2="right_iiwa_link_3" reason="Never"/>
+    <disable_collisions link1="right_iiwa_link_0" link2="right_iiwa_link_4" reason="Never"/>
+    <disable_collisions link1="right_iiwa_link_0" link2="right_iiwa_link_5" reason="Never"/>
+    <disable_collisions link1="right_iiwa_link_0" link2="right_iiwa_link_6" reason="Never"/>
+    <disable_collisions link1="right_iiwa_link_0" link2="right_iiwa_link_7" reason="Never"/>
+
+    <disable_collisions link1="right_iiwa_link_1" link2="right_iiwa_link_3" reason="Never"/>
+    <disable_collisions link1="right_iiwa_link_1" link2="right_iiwa_link_4" reason="Never"/>
+    <disable_collisions link1="right_iiwa_link_1" link2="right_iiwa_link_5" reason="Never"/>
+    <disable_collisions link1="right_iiwa_link_1" link2="right_iiwa_link_6" reason="Never"/>
+    <disable_collisions link1="right_iiwa_link_1" link2="right_iiwa_link_7" reason="Never"/>
+
+    <disable_collisions link1="right_iiwa_link_2" link2="right_iiwa_link_4" reason="Never"/>
+    <disable_collisions link1="right_iiwa_link_2" link2="right_iiwa_link_5" reason="Never"/>
+    <disable_collisions link1="right_iiwa_link_2" link2="right_iiwa_link_6" reason="Never"/>
+    <disable_collisions link1="right_iiwa_link_2" link2="right_iiwa_link_7" reason="Never"/>
+
+    <disable_collisions link1="right_iiwa_link_3" link2="right_iiwa_link_5" reason="Never"/>
+    <disable_collisions link1="right_iiwa_link_3" link2="right_iiwa_link_6" reason="Never"/>
+    <disable_collisions link1="right_iiwa_link_3" link2="right_iiwa_link_7" reason="Never"/>
+
+    <disable_collisions link1="right_iiwa_link_4" link2="right_iiwa_link_6" reason="Never"/>
+    <disable_collisions link1="right_iiwa_link_4" link2="right_iiwa_link_7" reason="Never"/>
+</robot>

examples/barriers/kukas_self_collision.py

@@ -0,0 +1,210 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+#
+# SPDX-License-Identifier: Apache-2.0
+# Copyright 2024 Ivan Domrachev, Simeon Nedelchev
+
+"""Two iiwa14-s with full-body self-collision avoidance using hpp-fcl."""
+
+import os
+import meshcat_shapes
+import numpy as np
+import pinocchio as pin
+import qpsolvers
+from loop_rate_limiters import RateLimiter
+
+from robot_descriptions.iiwa14_description import PACKAGE_PATH, REPOSITORY_PATH
+
+import pink
+from pink import solve_ik
+from pink.utils import process_collision_pairs
+from pink.barriers import SelfCollisionBarrier
+from pink.tasks import FrameTask, PostureTask
+from pink.visualization import start_meshcat_visualizer
+
+
+def prefix_frames(
+    model: pin.Model, visual_model: pin.GeometryModel, geometry_model: pin.GeometryModel, prefix: str
+) -> None:
+    for frame in model.frames:
+        frame.name = f"{prefix}_{frame.name}"
+    for i, name in enumerate(model.names):
+        model.names[i] = f"{prefix}_{name}"
+    for geom in visual_model.geometryObjects:
+        geom.name = f"{prefix}_{geom.name}"
+    for geom in collision_model.geometryObjects:
+        geom.name = f"{prefix}_{geom.name}"
+
+
+if __name__ == "__main__":
+    # Empty model
+    model, visual_model, collision_model = pin.Model(), pin.GeometryModel(), pin.GeometryModel()
+    urdf_path = os.path.join(PACKAGE_PATH, "urdf", "iiwa14_spheres_collision.urdf")
+
+    # === Left arm ====
+    left_arm = pin.RobotWrapper.BuildFromURDF(urdf_path, package_dirs=[os.path.dirname(REPOSITORY_PATH)])
+
+    # Add prefix to frames, links and geons of the arm
+    prefix_frames(left_arm.model, left_arm.visual_model, left_arm.collision_model, "left")
+
+    # Place left arm on the left to the origin
+    left_arm_placement = pin.SE3.Identity()
+    left_arm_placement.translation = np.array([0.0, 0.2, 0.0])
+
+    # Add left arm's model, visual model and collision model to the main model
+    _, visual_model = pin.appendModel(
+        model,
+        left_arm.model,
+        visual_model,
+        left_arm.visual_model,
+        0,
+        left_arm_placement,
+    )
+    model, collision_model = pin.appendModel(
+        model,
+        left_arm.model,
+        collision_model,
+        left_arm.collision_model,
+        0,
+        left_arm_placement,
+    )
+
+    # === Right arm ====
+    right_arm = pin.RobotWrapper.BuildFromURDF(urdf_path, package_dirs=[os.path.dirname(REPOSITORY_PATH)])
+
+    # Add prefix to frames, links and geons of the arm
+    prefix_frames(right_arm.model, right_arm.visual_model, right_arm.collision_model, "right")
+
+    # Place left arm on the left to the origin
+    right_arm_placement = pin.SE3.Identity()
+    right_arm_placement.translation = np.array([0.0, -0.2, 0.0])
+
+    # Add left arm's model, visual model and collision model to the main model
+    _, visual_model = pin.appendModel(
+        model,
+        right_arm.model,
+        visual_model,
+        right_arm.visual_model,
+        0,
+        right_arm_placement,
+    )
+    model, collision_model = pin.appendModel(
+        model,
+        right_arm.model,
+        collision_model,
+        right_arm.collision_model,
+        0,
+        right_arm_placement,
+    )
+
+    # Assemble to the robot
+    robot = pin.RobotWrapper(
+        model,
+        collision_model=collision_model,
+        visual_model=visual_model,
+    )
+
+    srdf_path = os.path.dirname(os.path.realpath(__file__)) + "/iiwa14_spheres_collision.srdf"
+    print(srdf_path)
+    viz = start_meshcat_visualizer(robot)
+    q_ref = np.zeros(robot.model.nq)
+
+    # Collisions: processing collisions from urdf (include all) and srdf (exclude specified)
+    # and updating collision model and creating corresponding collision data
+    robot.collision_data = process_collision_pairs(robot.model, robot.collision_model, srdf_path)
+
+    configuration = pink.Configuration(
+        robot.model,
+        robot.data,
+        q_ref,
+        collision_model=robot.collision_model,  # Collision model is required for self_collision_barrier
+        collision_data=robot.collision_data,
+    )
+
+    # Pink tasks
+    left_end_effector_task = FrameTask(
+        "left_iiwa_link_7",
+        position_cost=50.0,  # [cost] / [m]
+        orientation_cost=10.0,  # [cost] / [rad]
+    )
+    right_end_effector_task = FrameTask(
+        "right_iiwa_link_7",
+        position_cost=50.0,  # [cost] / [m]
+        orientation_cost=10.0,  # [cost] / [rad]
+    )
+
+    # Pink barriers
+    collision_barrier = SelfCollisionBarrier(
+        n_collision_pairs=len(robot.collision_model.collisionPairs),
+        gain=20.0,
+        safe_displacement_gain=1.0,
+        d_min=0.05,
+    )
+
+    posture_task = PostureTask(
+        cost=1e-3,  # [cost] / [rad]
+    )
+
+    barriers = [collision_barrier]
+    tasks = [left_end_effector_task, right_end_effector_task, posture_task]
+
+    for task in tasks:
+        task.set_target_from_configuration(configuration)
+    viz.display(configuration.q)
+
+    viewer = viz.viewer
+    meshcat_shapes.frame(viewer["left_end_effector"], opacity=1.0)
+    meshcat_shapes.frame(viewer["right_end_effector"], opacity=1.0)
+    meshcat_shapes.frame(viewer["left_end_effector_target"], opacity=1.0)
+    meshcat_shapes.frame(viewer["right_end_effector_target"], opacity=1.0)
+
+    # Select QP solver
+    solver = qpsolvers.available_solvers[0]
+    if "osqp" in qpsolvers.available_solvers:
+        solver = "osqp"
+
+    rate = RateLimiter(frequency=50.0)
+    dt = rate.period
+    t = 0.0  # [s]
+    l_y_des = np.array([0.392, -0.392, 0.6])
+    r_y_des = np.array([0.392, 0.392, 0.6])
+
+    A = l_y_des.copy()
+    B = r_y_des.copy()
+
+    l_dy_des = np.zeros(3)
+    r_dy_des = np.zeros(3)
+
+    while True:
+        # Make a sinusoidal trajectory between points A and B
+        mu = (1 + np.cos(t)) / 2
+        l_y_des[:] = A + (B - A + 0.2 * np.array([0, 0, np.sin(mu * np.pi) ** 2])) * mu
+        r_y_des[:] = B + (A - B + 0.2 * np.array([0, 0, -np.sin(mu * np.pi) ** 2])) * mu
+
+        left_end_effector_task.transform_target_to_world.translation = l_y_des
+        right_end_effector_task.transform_target_to_world.translation = r_y_des
+
+        # Update visualization frames
+        viewer["left_end_effector"].set_transform(
+            configuration.get_transform_frame_to_world(left_end_effector_task.frame).np
+        )
+        viewer["right_end_effector"].set_transform(
+            configuration.get_transform_frame_to_world(right_end_effector_task.frame).np
+        )
+        viewer["left_end_effector_target"].set_transform(left_end_effector_task.transform_target_to_world.np)
+        viewer["right_end_effector_target"].set_transform(right_end_effector_task.transform_target_to_world.np)
+
+        velocity = solve_ik(
+            configuration,
+            tasks,
+            dt,
+            solver=solver,
+            barriers=barriers,
+            safety_break=False,
+        )
+        configuration.integrate_inplace(velocity, dt)
+
+        # Visualize result at fixed FPS
+        viz.display(configuration.q)
+        rate.sleep()
+        t += dt

examples/barriers/meshcat_shapes.py

@@ -39,8 +39,12 @@ def __attach_axes(
     position_cylinder_in_frame = 0.5 * length * np.eye(3)
     for i in range(3):
         dir_name = direction_names[i]
-        material = meshcat.geometry.MeshLambertMaterial(color=colors[i], opacity=opacity)
-        transform_cylinder_to_frame = meshcat.transformations.rotation_matrix(np.pi / 2, rotation_axes[i])
+        material = meshcat.geometry.MeshLambertMaterial(
+            color=colors[i], opacity=opacity
+        )
+        transform_cylinder_to_frame = meshcat.transformations.rotation_matrix(
+            np.pi / 2, rotation_axes[i]
+        )
         transform_cylinder_to_frame[0:3, 3] = position_cylinder_in_frame[i]
         cylinder = meshcat.geometry.Cylinder(length, thickness)
         handle[dir_name].set_object(cylinder, material)
@@ -69,7 +73,9 @@ def frame(
         As per the de-facto standard (Blender, OpenRAVE, RViz, ...), the
         x-axis is red, the y-axis is green and the z-axis is blue.
     """
-    material = meshcat.geometry.MeshLambertMaterial(color=origin_color, opacity=opacity)
+    material = meshcat.geometry.MeshLambertMaterial(
+        color=origin_color, opacity=opacity
+    )
     sphere = meshcat.geometry.Sphere(origin_radius)
     handle.set_object(sphere, material)
     __attach_axes(handle, axis_length, axis_thickness, opacity)
@@ -81,7 +87,9 @@ def sphere(
     color: int = 0x000000,
     opacity: float = 1.0,
 ):
-    material = meshcat.geometry.MeshLambertMaterial(color=color, opacity=opacity)
+    material = meshcat.geometry.MeshLambertMaterial(
+        color=color, opacity=opacity
+    )
     sphere = meshcat.geometry.Sphere(
         radius,
     )

examples/barriers/yumi_self_collision.py

@@ -4,7 +4,7 @@
 # SPDX-License-Identifier: Apache-2.0
 # Copyright 2022 Stéphane Caron
 
-"""Yumi two-armed manipulator with definition of custom frame and 
+"""Yumi two-armed manipulator with definition of custom frame and
 collision avoidance w.r.t. those frames."""
 
 import meshcat_shapes