Merge 5963484 into 2c7f1f6

CHANGELOG.md

@@ -10,6 +10,7 @@ All notable changes to this project will be documented in this file.
   - Abstract Barrier ``Barrier``
   - Frame Position Barrier ``PositionBarrier``
   - Body Spherical Barrier ``BodySphericalBarrier``
+  - 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.

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/kukas_self_collision.py

@@ -0,0 +1,132 @@
+#!/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 meshcat_shapes
+import numpy as np
+import pinocchio as pin
+import qpsolvers
+from loop_rate_limiters import RateLimiter
+
+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
+
+if __name__ == "__main__":
+    # Load the robot and define the custom frames
+    urdf_path = "examples/barriers/models/iiwa14_spheres_collision.urdf"
+    srdf_path = "examples/barriers/models/iiwa14_spheres_collision.srdf"
+    robot = pin.RobotWrapper.BuildFromURDF(
+        filename=urdf_path,
+        package_dirs=["."],
+        root_joint=None,
+    )
+
+    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(
+        "first_iiwa_link_7",
+        position_cost=50.0,  # [cost] / [m]
+        orientation_cost=10.0,  # [cost] / [rad]
+    )
+    right_end_effector_task = FrameTask(
+        "second_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,
+        )
+        configuration.integrate_inplace(velocity, dt)
+
+        # Visualize result at fixed FPS
+        viz.display(configuration.q)
+        rate.sleep()
+        t += dt

examples/barriers/models/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="first_iiwa_link_0" link2="first_iiwa_link_1" reason="Adjacent"/>
+    <disable_collisions link1="first_iiwa_link_1" link2="first_iiwa_link_2" reason="Adjacent"/>
+    <disable_collisions link1="first_iiwa_link_2" link2="first_iiwa_link_3" reason="Adjacent"/>
+    <disable_collisions link1="first_iiwa_link_3" link2="first_iiwa_link_4" reason="Adjacent"/>
+    <disable_collisions link1="first_iiwa_link_4" link2="first_iiwa_link_5" reason="Adjacent"/>
+    <disable_collisions link1="first_iiwa_link_5" link2="first_iiwa_link_6" reason="Adjacent"/>
+    <disable_collisions link1="first_iiwa_link_5" link2="first_iiwa_link_7" reason="Adjacent"/>
+    <disable_collisions link1="first_iiwa_link_6" link2="first_iiwa_link_7" reason="Adjacent"/>
+
+    <disable_collisions link1="first_iiwa_link_1" link2="first_iiwa_link_3" reason="Never"/>
+    <disable_collisions link1="first_iiwa_link_1" link2="first_iiwa_link_4" reason="Never"/>
+    <disable_collisions link1="first_iiwa_link_1" link2="first_iiwa_link_5" reason="Never"/>
+    <disable_collisions link1="first_iiwa_link_1" link2="first_iiwa_link_6" reason="Never"/>
+    <disable_collisions link1="first_iiwa_link_1" link2="first_iiwa_link_7" reason="Never"/>
+
+    <disable_collisions link1="first_iiwa_link_2" link2="first_iiwa_link_4" reason="Never"/>
+    <disable_collisions link1="first_iiwa_link_2" link2="first_iiwa_link_5" reason="Never"/>
+    <disable_collisions link1="first_iiwa_link_2" link2="first_iiwa_link_6" reason="Never"/>
+    <disable_collisions link1="first_iiwa_link_2" link2="first_iiwa_link_7" reason="Never"/>
+
+    <disable_collisions link1="first_iiwa_link_3" link2="first_iiwa_link_5" reason="Never"/>
+    <disable_collisions link1="first_iiwa_link_3" link2="first_iiwa_link_6" reason="Never"/>
+    <disable_collisions link1="first_iiwa_link_3" link2="first_iiwa_link_7" reason="Never"/>
+
+    <disable_collisions link1="first_iiwa_link_4" link2="first_iiwa_link_6" reason="Never"/>
+    <disable_collisions link1="first_iiwa_link_4" link2="first_iiwa_link_7" reason="Never"/>
+
+    <disable_collisions link1="second_iiwa_link_0" link2="second_iiwa_link_1" reason="Adjacent"/>
+    <disable_collisions link1="second_iiwa_link_1" link2="second_iiwa_link_2" reason="Adjacent"/>
+    <disable_collisions link1="second_iiwa_link_2" link2="second_iiwa_link_3" reason="Adjacent"/>
+    <disable_collisions link1="second_iiwa_link_3" link2="second_iiwa_link_4" reason="Adjacent"/>
+    <disable_collisions link1="second_iiwa_link_4" link2="second_iiwa_link_5" reason="Adjacent"/>
+    <disable_collisions link1="second_iiwa_link_5" link2="second_iiwa_link_6" reason="Adjacent"/>
+    <disable_collisions link1="second_iiwa_link_5" link2="second_iiwa_link_7" reason="Adjacent"/>
+    <disable_collisions link1="second_iiwa_link_6" link2="second_iiwa_link_7" reason="Adjacent"/>
+
+    <disable_collisions link1="second_iiwa_link_0" link2="second_iiwa_link_3" reason="Never"/>
+    <disable_collisions link1="second_iiwa_link_0" link2="second_iiwa_link_4" reason="Never"/>
+    <disable_collisions link1="second_iiwa_link_0" link2="second_iiwa_link_5" reason="Never"/>
+    <disable_collisions link1="second_iiwa_link_0" link2="second_iiwa_link_6" reason="Never"/>
+    <disable_collisions link1="second_iiwa_link_0" link2="second_iiwa_link_7" reason="Never"/>
+
+    <disable_collisions link1="second_iiwa_link_1" link2="second_iiwa_link_3" reason="Never"/>
+    <disable_collisions link1="second_iiwa_link_1" link2="second_iiwa_link_4" reason="Never"/>
+    <disable_collisions link1="second_iiwa_link_1" link2="second_iiwa_link_5" reason="Never"/>
+    <disable_collisions link1="second_iiwa_link_1" link2="second_iiwa_link_6" reason="Never"/>
+    <disable_collisions link1="second_iiwa_link_1" link2="second_iiwa_link_7" reason="Never"/>
+
+    <disable_collisions link1="second_iiwa_link_2" link2="second_iiwa_link_4" reason="Never"/>
+    <disable_collisions link1="second_iiwa_link_2" link2="second_iiwa_link_5" reason="Never"/>
+    <disable_collisions link1="second_iiwa_link_2" link2="second_iiwa_link_6" reason="Never"/>
+    <disable_collisions link1="second_iiwa_link_2" link2="second_iiwa_link_7" reason="Never"/>
+
+    <disable_collisions link1="second_iiwa_link_3" link2="second_iiwa_link_5" reason="Never"/>
+    <disable_collisions link1="second_iiwa_link_3" link2="second_iiwa_link_6" reason="Never"/>
+    <disable_collisions link1="second_iiwa_link_3" link2="second_iiwa_link_7" reason="Never"/>
+
+    <disable_collisions link1="second_iiwa_link_4" link2="second_iiwa_link_6" reason="Never"/>
+    <disable_collisions link1="second_iiwa_link_4" link2="second_iiwa_link_7" reason="Never"/>
+</robot>