diff --git a/cmake b/cmake
index 29c0eb4e659304f44d55a0389e2749812d858659..91b8f5f2168b123a198da079b8e6c09fd1f60285 160000
--- a/cmake
+++ b/cmake
@@ -1 +1 @@
-Subproject commit 29c0eb4e659304f44d55a0389e2749812d858659
+Subproject commit 91b8f5f2168b123a198da079b8e6c09fd1f60285
diff --git a/python/example_robot_data/robots_loader.py b/python/example_robot_data/robots_loader.py
index 0fdd485b194fd835adc510557192d4b586c9361c..94b5380badfce355e3b725ae6376fb52f931b493 100644
--- a/python/example_robot_data/robots_loader.py
+++ b/python/example_robot_data/robots_loader.py
@@ -2,6 +2,7 @@ import sys
import typing
from os.path import dirname, exists, join
+import hppfcl
import numpy as np
import pinocchio as pin
from pinocchio.robot_wrapper import RobotWrapper
@@ -158,6 +159,21 @@ class RobotLoader:
lb[:7] = -1
self.robot.model.lowerPositionLimit = lb
+ def generate_capsule_name(self, base_name: str, existing_names: list) -> str:
+ """Generates a unique capsule name for a geometry object.
+
+ Args:
+ base_name (str): The base name of the geometry object.
+ existing_names (list): List of names already assigned to capsules.
+
+ Returns:
+ str: Unique capsule name.
+ """
+ i = 0
+ while f"{base_name}_capsule_{i}" in existing_names:
+ i += 1
+ return f"{base_name}_capsule_{i}"
+
class B1Loader(RobotLoader):
path = "b1_description"
@@ -512,6 +528,50 @@ class PandaLoader(RobotLoader):
ref_posture = "default"
+class PandaLoaderCollision(PandaLoader):
+ urdf_filename = "panda_collision.urdf"
+
+ def __init__(self, verbose=False):
+ super().__init__(verbose=verbose)
+
+ cmodel = self.robot.collision_model.copy()
+ list_names_capsules = []
+ # Iterate through geometry objects in the collision model
+ for geom_object in cmodel.geometryObjects:
+ geometry = geom_object.geometry
+ # Remove superfluous suffix from the name
+ base_name = "_".join(geom_object.name.split("_")[:-1])
+
+ # Convert cylinders to capsules
+ if isinstance(geometry, hppfcl.Cylinder):
+ name = self.generate_capsule_name(base_name, list_names_capsules)
+ list_names_capsules.append(name)
+ capsule = pin.GeometryObject(
+ name=name,
+ parent_frame=int(geom_object.parentFrame),
+ parent_joint=int(geom_object.parentJoint),
+ collision_geometry=hppfcl.Capsule(
+ geometry.radius, geometry.halfLength
+ ),
+ placement=geom_object.placement,
+ )
+ capsule.meshColor = np.array([249, 136, 126, 125]) / 255 # Red color
+ self.robot.collision_model.addGeometryObject(capsule)
+ self.robot.collision_model.removeGeometryObject(geom_object.name)
+
+ # Remove spheres associated with links
+ elif isinstance(geometry, hppfcl.Sphere) and "link" in geom_object.name:
+ self.robot.collision_model.removeGeometryObject(geom_object.name)
+
+ # Recreate collision data since the collision pairs changed
+ self.robot.collision_data = self.robot.collision_model.createData()
+
+ self.srdf_path = None
+ self.robot.q0 = pin.neutral(self.robot.model)
+ root = getModelPath(self.path)
+ self.robot.urdf = join(root, self.path, self.urdf_subpath, self.urdf_filename)
+
+
class AlexNubHandsLoader(RobotLoader):
path = "alex_description"
urdf_filename = "alex_nub_hands.urdf"
@@ -689,6 +749,7 @@ ROBOTS = {
"kinova": KinovaLoader,
"laikago": LaikagoLoader,
"panda": PandaLoader,
+ "panda_collision": PandaLoaderCollision,
"alex_nub_hands": AlexNubHandsLoader,
"alex_psyonic_hands": AlexPsyonicHandsLoader,
"alex_sake_hands": AlexSakeHandsLoader,
diff --git a/robots/panda_description/urdf/panda_collision.urdf b/robots/panda_description/urdf/panda_collision.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..67ed506a36b3c1134f8e6276888ae34a45183c6a
--- /dev/null
+++ b/robots/panda_description/urdf/panda_collision.urdf
@@ -0,0 +1,450 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- | This document was autogenerated by xacro from franka_ros/franka_description/robots/panda/panda.urdf.xacro | -->
+<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
+<!-- =================================================================================== -->
+<robot name="panda" xmlns:xacro="http://www.ros.org/wiki/xacro">
+ <link name="panda_link0">
+ <visual>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/panda_description/meshes/visual/link0.dae" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin rpy="0 1.5707963267948966 0" xyz="-0.075 0 0.06"/>
+ <geometry>
+ <cylinder length="0.03" radius="0.09"/>
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="-0.06 0 0.06"/>
+ <geometry>
+ <sphere radius="0.09"/>
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="-0.09 0 0.06"/>
+ <geometry>
+ <sphere radius="0.09"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0 0 0" xyz="-0.041018 -0.00014 0.049974"/>
+ <mass value="0.629769"/>
+ <inertia ixx="0.00315" ixy="8.2904e-07" ixz="0.00015" iyy="0.00388" iyz="8.2299e-06" izz="0.004285"/>
+ </inertial>
+ </link>
+ <link name="panda_link1">
+ <visual>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/panda_description/meshes/visual/link1.dae" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 -0.1915"/>
+ <geometry>
+ <cylinder length="0.283" radius="0.09"/>
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0 0 -0.05000000000000002"/>
+ <geometry>
+ <sphere radius="0.09"/>
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0 0 -0.33299999999999996"/>
+ <geometry>
+ <sphere radius="0.09"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.003875 0.002081 -0.04762"/>
+ <mass value="4.970684"/>
+ <inertia ixx="0.70337" ixy="-0.000139" ixz="0.006772" iyy="0.70661" iyz="0.019169" izz="0.009117"/>
+ </inertial>
+ </link>
+ <joint name="panda_joint1" type="revolute">
+ <origin rpy="0 0 0" xyz="0 0 0.333"/>
+ <parent link="panda_link0"/>
+ <child link="panda_link1"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="87.0" lower="-2.8973" upper="2.8973" velocity="2.175"/>
+ <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+ <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
+ </joint>
+ <link name="panda_link2">
+ <visual>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/panda_description/meshes/visual/link2.dae" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <cylinder length="0.12" radius="0.09"/>
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0 0 0.06"/>
+ <geometry>
+ <sphere radius="0.09"/>
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0 0 -0.06"/>
+ <geometry>
+ <sphere radius="0.09"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0 0 0" xyz="-0.003141 -0.02872 0.003495"/>
+ <mass value="0.646926"/>
+ <inertia ixx="0.007962" ixy="-0.003925" ixz="0.010254" iyy="0.02811" iyz="0.000704" izz="0.025995"/>
+ </inertial>
+ </link>
+ <joint name="panda_joint2" type="revolute">
+ <origin rpy="-1.5707963267948966 0 0" xyz="0 0 0"/>
+ <parent link="panda_link1"/>
+ <child link="panda_link2"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="87.0" lower="-1.7628" upper="1.7628" velocity="2.175"/>
+ <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
+ <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
+ </joint>
+ <link name="panda_link3">
+ <visual>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/panda_description/meshes/visual/link3.dae" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 -0.145"/>
+ <geometry>
+ <cylinder length="0.15" radius="0.09"/>
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0 0 -0.06999999999999999"/>
+ <geometry>
+ <sphere radius="0.09"/>
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0 0 -0.21999999999999997"/>
+ <geometry>
+ <sphere radius="0.09"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0 0 0" xyz="2.7518e-02 3.9252e-02 -6.6502e-02"/>
+ <mass value="3.228604"/>
+ <inertia ixx="0.037242" ixy="-0.004761" ixz="-0.011396" iyy="0.036155" iyz="-0.012805" izz="0.01083"/>
+ </inertial>
+ </link>
+ <joint name="panda_joint3" type="revolute">
+ <origin rpy="1.5707963267948966 0 0" xyz="0 -0.316 0"/>
+ <parent link="panda_link2"/>
+ <child link="panda_link3"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="87.0" lower="-2.8973" upper="2.8973" velocity="2.175"/>
+ <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+ <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
+ </joint>
+ <link name="panda_link4">
+ <visual>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/panda_description/meshes/visual/link4.dae"/>
+ </geometry>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <cylinder length="0.12" radius="0.09"/>
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0 0 0.06"/>
+ <geometry>
+ <sphere radius="0.09"/>
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0 0 -0.06"/>
+ <geometry>
+ <sphere radius="0.09"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0 0 0" xyz="-5.317e-02 1.04419e-01 2.7454e-02"/>
+ <mass value="3.587895"/>
+ <inertia ixx="0.025853" ixy="0.007796" ixz="-0.001332" iyy="0.019552" iyz="0.008641" izz="0.028323"/>
+ </inertial>
+ </link>
+ <joint name="panda_joint4" type="revolute">
+ <origin rpy="1.5707963267948966 0 0" xyz="0.0825 0 0"/>
+ <parent link="panda_link3"/>
+ <child link="panda_link4"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="87.0" lower="-3.0718" upper="-0.0698" velocity="2.175"/>
+ <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
+ <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
+ </joint>
+ <link name="panda_link5">
+ <visual>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/panda_description/meshes/visual/link5.dae" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 -0.26"/>
+ <geometry>
+ <cylinder length="0.1" radius="0.09"/>
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0 0 -0.21000000000000002"/>
+ <geometry>
+ <sphere radius="0.09"/>
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0 0 -0.31"/>
+ <geometry>
+ <sphere radius="0.09"/>
+ </geometry>
+ </collision>
+ <collision>
+ <origin rpy="0 0 0.08" xyz="0 0.08 -0.13"/>
+ <geometry>
+ <cylinder length="0.14" radius="0.055"/>
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0 0.08 -0.06"/>
+ <geometry>
+ <sphere radius="0.055"/>
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0 0.08 -0.2"/>
+ <geometry>
+ <sphere radius="0.055"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0 0 0" xyz="-1.1953e-02 4.1065e-02 -3.8437e-02"/>
+ <mass value="1.225946"/>
+ <inertia ixx="0.035549" ixy="-0.002117" ixz="-0.004037" iyy="0.029474" iyz="0.000229" izz="0.008627"/>
+ </inertial>
+ </link>
+ <joint name="panda_joint5" type="revolute">
+ <origin rpy="-1.5707963267948966 0 0" xyz="-0.0825 0.384 0"/>
+ <parent link="panda_link4"/>
+ <child link="panda_link5"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="12.0" lower="-2.8973" upper="2.8973" velocity="2.61"/>
+ <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+ <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
+ </joint>
+ <link name="panda_link6">
+ <visual>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/panda_description/meshes/visual/link6.dae" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 -0.03"/>
+ <geometry>
+ <cylinder length="0.08" radius="0.08"/>
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0 0 0.010000000000000002"/>
+ <geometry>
+ <sphere radius="0.08"/>
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0 0 -0.07"/>
+ <geometry>
+ <sphere radius="0.08"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0 0 0" xyz="6.0149e-02 -1.4117e-02 -1.0517e-02"/>
+ <mass value="1.666555"/>
+ <inertia ixx="0.001964" ixy="0.000109" ixz="-0.001158" iyy="0.004354" iyz="0.000341" izz="0.005433"/>
+ </inertial>
+ </link>
+ <joint name="panda_joint6" type="revolute">
+ <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+ <parent link="panda_link5"/>
+ <child link="panda_link6"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="12.0" lower="-0.0175" upper="3.7525" velocity="2.61"/>
+ <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
+ <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
+ </joint>
+ <link name="panda_link7">
+ <visual>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/panda_description/meshes/visual/link7.dae" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0.01"/>
+ <geometry>
+ <cylinder length="0.14" radius="0.07"/>
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0 0 0.08"/>
+ <geometry>
+ <sphere radius="0.07"/>
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0 0 -0.060000000000000005"/>
+ <geometry>
+ <sphere radius="0.07"/>
+ </geometry>
+ </collision>
+ <collision>
+ <origin rpy="0 1.5707963267948966 0" xyz="0.06 0 0.082"/>
+ <geometry>
+ <cylinder length="0.01" radius="0.06"/>
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0.065 0 0.082"/>
+ <geometry>
+ <sphere radius="0.06"/>
+ </geometry>
+ </collision>
+ <collision>
+ <origin xyz="0.055 0 0.082"/>
+ <geometry>
+ <sphere radius="0.06"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0 0 0" xyz="1.0517e-02 -4.252e-03 6.1597e-02"/>
+ <mass value="0.735522"/>
+ <inertia ixx="0.012516" ixy="-0.000428" ixz="-0.001196" iyy="0.010027" iyz="-0.000741" izz="0.004815"/>
+ </inertial>
+ </link>
+ <joint name="panda_joint7" type="revolute">
+ <origin rpy="1.5707963267948966 0 0" xyz="0.088 0 0"/>
+ <parent link="panda_link6"/>
+ <child link="panda_link7"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="12.0" lower="-2.8973" upper="2.8973" velocity="2.61"/>
+ <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+ <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
+ </joint>
+ <link name="panda_link8">
+ <inertial>
+ <mass value="0"/>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+ </inertial>
+ </link>
+ <joint name="panda_joint8" type="fixed">
+ <origin rpy="0 0 0" xyz="0 0 0.107"/>
+ <parent link="panda_link7"/>
+ <child link="panda_link8"/>
+ </joint>
+ <joint name="panda_hand_joint" type="fixed">
+ <parent link="panda_link8"/>
+ <child link="panda_hand"/>
+ <origin rpy="0 0 -0.7853981633974483" xyz="0 0 0"/>
+ </joint>
+ <link name="panda_hand">
+ <visual>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/panda_description/meshes/visual/hand.dae" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin rpy="1.57 0 0" xyz="0 0 3e-2"/>
+ <geometry>
+ <cylinder length="0.25" radius="0.04"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0 0 0" xyz="-0.01 0 0.03"/>
+ <mass value="0.73"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.0025" iyz="0" izz="0.0017"/>
+ </inertial>
+ </link>
+ <!-- Define the hand_tcp frame -->
+ <link name="panda_hand_tcp">
+ <inertial>
+ <mass value="0"/>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+ </inertial>
+ </link>
+ <joint name="panda_hand_tcp_joint" type="fixed">
+ <origin rpy="0 0 0" xyz="0 0 0.1034"/>
+ <parent link="panda_hand"/>
+ <child link="panda_hand_tcp"/>
+ </joint>
+ <link name="panda_leftfinger">
+ <visual>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/panda_description/meshes/visual/finger.dae" />
+ </geometry>
+ </visual>
+ <!-- screw mount -->
+ <collision>
+ <origin rpy="0 0 0" xyz="0 15e-3 3e-2"/>
+ <geometry>
+ <cylinder length="0.07" radius="0.015"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.015"/>
+ <inertia ixx="2.3749999999999997e-06" ixy="0" ixz="0" iyy="2.3749999999999997e-06" iyz="0" izz="7.5e-07"/>
+ </inertial>
+ </link>
+ <link name="panda_rightfinger">
+ <visual>
+ <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/panda_description/meshes/visual/finger.dae" />
+ </geometry>
+ </visual>
+ <!-- screw mount -->
+ <collision>
+ <origin rpy="0 0 0" xyz="0 -15e-3 3e-2"/>
+ <geometry>
+ <cylinder length="0.07" radius="0.015"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.015"/>
+ <inertia ixx="2.3749999999999997e-06" ixy="0" ixz="0" iyy="2.3749999999999997e-06" iyz="0" izz="7.5e-07"/>
+ </inertial>
+ </link>
+ <joint name="panda_finger_joint1" type="prismatic">
+ <parent link="panda_hand"/>
+ <child link="panda_leftfinger"/>
+ <origin rpy="0 0 0" xyz="0 0 0.0584"/>
+ <axis xyz="0 1 0"/>
+ <limit effort="100" lower="0.0" upper="0.04" velocity="0.2"/>
+ <dynamics damping="0.3"/>
+ </joint>
+ <joint name="panda_finger_joint2" type="prismatic">
+ <parent link="panda_hand"/>
+ <child link="panda_rightfinger"/>
+ <origin rpy="0 0 0" xyz="0 0 0.0584"/>
+ <axis xyz="0 -1 0"/>
+ <limit effort="100" lower="0.0" upper="0.04" velocity="0.2"/>
+ <mimic joint="panda_finger_joint1"/>
+ <dynamics damping="0.3"/>
+ </joint>
+</robot>
diff --git a/unittest/test_load.py b/unittest/test_load.py
index b497baecbbc11cfe54d658d93aaaaed94e3879c8..9b0a8b17aa7129c009d9d9a0aa9ec3011364008b 100755
--- a/unittest/test_load.py
+++ b/unittest/test_load.py
@@ -118,6 +118,9 @@ class RobotTestCase(unittest.TestCase):
def test_panda(self):
self.check("panda", 9, 9)
+ def test_panda_collision(self):
+ self.check("panda_collision", 9, 9)
+
def test_alex_nub_hands(self):
self.check("alex_nub_hands", 19, 19)