在pydrake中使用ComputeSignedDistancePairClosestPoints时的奇怪不一致性

Question

我用德雷克为一个腿型机器人做步行步态。要做到这一点，我需要一种方法来确定什么时候适合施加接触力。为此，我在机器人的爪子(脚)和作为地板(世界物体)的盒子之间使用ComputeSignedDistancePairClosestPoints：

# Relevant geometry ids

Worldbody_id = inspector.GetAllGeometryIds()[body_list.index('WorldBody')]
FL_claw_id = inspector.GetAllGeometryIds()[body_list.index('Link_FL_3')]
FR_claw_id = inspector.GetAllGeometryIds()[body_list.index('Link_FR_3')]
ML_claw_id = inspector.GetAllGeometryIds()[body_list.index('Link_ML_3')]
MR_claw_id = inspector.GetAllGeometryIds()[body_list.index('Link_MR_3')]
BL_claw_id = inspector.GetAllGeometryIds()[body_list.index('Link_BL_3')]
BR_claw_id = inspector.GetAllGeometryIds()[body_list.index('Link_BR_3')]

Claw_id = [FL_claw_id, FR_claw_id, ML_claw_id, MR_claw_id, BL_claw_id, BR_claw_id]
# Claw_id[0] = FL_claw_id
# Claw_id[1] = FR_claw_id
# Claw_id[2] = ML_claw_id
# Claw_id[3] = MR_claw_id
# Claw_id[4] = BL_claw_id
# Claw_id[5] = BR_claw_id
print(Claw_id)

作为一项测试，我做了以下工作：

Distances = np.zeros(5)
for i in range(6):
    print(Distances)
    Distances[i] = query_object.ComputeSignedDistancePairClosestPoints(Worldbody_id, Claw_id[i]).distance

不幸的是，这会产生以下错误：

[0. 0. 0. 0. 0.]
[-0.04070674  0.          0.          0.          0.        ]
---------------------------------------------------------------------------
RuntimeError                              Traceback (most recent call last)
<ipython-input-11-077874ed3da0> in <module>
      2 for i in range(6):
      3     print(Distances)
----> 4     Distances[i] = query_object.ComputeSignedDistancePairClosestPoints(Worldbody_id, Claw_id[i]).distance
      5 
      6 

RuntimeError: The geometry given by id 63 does not reference a geometry that can be used in a signed distance query

这意味着对应于FR_Claw_id的几何使用的几何类型与对应于FL_Claw_id的几何类型不同。这是尽管事实，他们都是网格(obj文件)。

有人知道我没看到什么吗？

使用具有更简单几何学(只有框)的urdf，这个问题仍然存在(这个urdf是手工制作的，所以中间数据可能是错误的)。

下面是使用的代码：

    # Imports required for this programme
import numpy as np
import os

from pydrake.common import FindResourceOrThrow, temp_directory
from pydrake.geometry import (
    MeshcatVisualizerCpp,
    MeshcatVisualizerParams,
    Role,
    StartMeshcat,
)
from pydrake.solvers.mathematicalprogram import MathematicalProgram
from pydrake.math import RigidTransform, RollPitchYaw
from pydrake.multibody.meshcat import JointSliders
from pydrake.multibody.parsing import Parser
from pydrake.multibody.plant import AddMultibodyPlantSceneGraph
from pydrake.multibody.plant import MultibodyPlant
from pydrake.multibody.optimization import StaticEquilibriumProblem
from pydrake.multibody.tree import Joint
from pydrake.systems.analysis import Simulator
from pydrake.systems.framework import DiagramBuilder
from pydrake.all import *

hexapod_file = "/home/remi/catkin_ws/src/hexapod/SCAWeR.urdf"
builder = DiagramBuilder()

# Note: the time_step here is chosen arbitrarily.
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.001)

# Adding a ground plane
normal_W = [0, 0, 1]
point_W = [0, 0, 10]

surface_friction = CoulombFriction(
  0.8 # static friction
 ,0.3)# dynamic friction

# A box for the ground geometry.
plant.RegisterCollisionGeometry(
    plant.world_body(), HalfSpace.MakePose(normal_W, point_W),
    Box(100, 100, 1), "collision", surface_friction)

# Load the file into the plant/scene_graph.
parser = Parser(plant)
parser.package_map().PopulateFromFolder("/home/remi/catkin_ws/src/hexapod")
parser.AddModelFromFile(hexapod_file)
plant.Finalize()

# create context for plant
plant_ad = plant.ToAutoDiffXd()
context_ad = plant_ad.CreateDefaultContext()

# creating an inspector
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
scene_graph_context = scene_graph.GetMyContextFromRoot(diagram_context) 
query_object = scene_graph.get_query_output_port().Eval(scene_graph_context)
inspector = query_object.inspector()

# Compiling a list of the names of the bodies in the world (same order as geometry id list)
body_list = []
for geometry_id in inspector.GetAllGeometryIds():
    body = plant.GetBodyFromFrameId(inspector.GetFrameId(geometry_id))
    body_list.append(body.name())
    
# Relevant geometry ids
Worldbody_id = inspector.GetAllGeometryIds()[body_list.index('WorldBody')]
FL_claw_id = inspector.GetAllGeometryIds()[body_list.index('leg_l1_3')]
FR_claw_id = inspector.GetAllGeometryIds()[body_list.index('leg_r1_3')]
ML_claw_id = inspector.GetAllGeometryIds()[body_list.index('leg_l2_3')]
MR_claw_id = inspector.GetAllGeometryIds()[body_list.index('leg_r2_3')]
BL_claw_id = inspector.GetAllGeometryIds()[body_list.index('leg_l3_3')]
BR_claw_id = inspector.GetAllGeometryIds()[body_list.index('leg_r3_3')]

# Assigning the claw_ids to a list for ease of use
Claw_id = [FL_claw_id, FR_claw_id, ML_claw_id, MR_claw_id, BL_claw_id, BR_claw_id]
# Claw_id[0] = FL_claw_id
# Claw_id[1] = FR_claw_id
# Claw_id[2] = ML_claw_id
# Claw_id[3] = MR_claw_id
# Claw_id[4] = BL_claw_id
# Claw_id[5] = BR_claw_id

# Assigning the relevant geometries for ease of use
Claw_name = ['leg_l1_3', 'leg_r1_3', 'leg_l2_3', 'leg_r2_3', 'leg_l3_3', 'leg_r3_3']

def GetNameFromGeometryID(ID):
    return plant.GetBodyFromFrameId(inspector.GetFrameId(ID)).name()

def SignedDistance(name):
    dList = query_object.ComputeSignedDistancePairwiseClosestPoints()
    idW = Worldbody_id
    for i in range(len(dList)):
        idB = dList[i].id_B
        idA = dList[i].id_A
        if (idW == dList[i].id_A) and (name == GetNameFromGeometryID(idB)):
            distance = dList[i]
            return distance
        elif (idW == dList[i].id_B) and (name == GetNameFromGeometryID(idA)):
            distance = dList[i]
            return distance

这里定义的自定义函数SignedDistance使用ComputeSignedDistancePairwiseClosestPoints作为代理来查找我需要的距离：

Distances = np.zeros(6)
for i in range(len(Claw_name)):
    Distances[i] = SignedDistance(Claw_name[i]).distance
    
        
print(Distances)

奇怪的是，这与使用ComputeSignedDistancePairClosestPoints不同：

Distances = np.zeros(6)
for i in range(len(Claw_id)):
    Distances[i] = query_object.ComputeSignedDistancePairClosestPoints(Worldbody_id, Claw_id[i])
    
print(Distances)

下面是我使用的urdf：

<?xml version="1.0" encoding="utf-8"?>
<robot name="scawer">
    <!-- Defining materials -->
    <material name="mat_body">
        <color rgba="0.1 0.1 0.4 1.0"/>
    </material>

    <material name="mat_link_0">
        <color rgba="0.35 0.4 0.35 1.0"/>
    </material>

    <material name="mat_link_1">
        <color rgba="0.5 0.35 0.35 1.0"/>
    </material>

    <material name="mat_link_2">
        <color rgba="0.35 0.35 0.5 1.0"/>
    </material>

    <material name="mat_link_3">
        <color rgba="0.25 0.25 0.25 1.0"/>
    </material>
    <!-- End Defining materials -->

    <link name="body">
        <visual>
            <geometry>
                <box size="0.2 0.1 0.05"/>
            </geometry>
            <material name="mat_body"/>
        </visual>

        <collision>
            <geometry>
                <box size="0.2 0.1 0.05"/>
            </geometry>
        </collision>

        <inertial>
            <mass value="0.8"/>
            <inertia ixx="8.33e-4" ixy="0.0" ixz="0.0" iyy="2.83e-3" iyz="0.0" izz="3.33e-3"/>
        </inertial>
    </link>

    <!-- Leg R1 Begin -->
    <link name="leg_r1_0">
        <visual>
            <geometry>
                <box size="0.015 0.02 0.015"/>
            </geometry>
            <origin xyz="0.0 -0.01 0.0"/>
            <material name="mat_link_0"/>
        </visual>

        <collision>
            <geometry>
                <box size="0.015 0.02 0.015"/>
            </geometry>
            <origin xyz="0.0 -0.01 0.0"/>
        </collision>

        <inertial>
            <mass value="0.05"/>
            <inertia ixx="2.604e-6" ixy="0.0" ixz="0.0" iyy="1.875e-6" iyz="0.0" izz="2.604e-6"/>
        </inertial>
    </link>

    <link name="leg_r1_1">
        <visual>
            <geometry>
                <box size="0.015 0.045 0.015"/>
            </geometry>
            <origin xyz="0.0 -0.0225 0.0"/>
            <material name="mat_link_1"/>
        </visual>

        <collision>
            <geometry>
                <box size="0.015 0.045 0.015"/>
            </geometry>
            <origin xyz="0.0 -0.0225 0.0"/>
        </collision>

        <inertial>
            <mass value="0.05"/>
            <inertia ixx="9.375e-6" ixy="0.0" ixz="0.0" iyy="1.875e-6" iyz="0.0" izz="9.375e-6"/>
        </inertial>
    </link>

    <link name="leg_r1_2">
        <visual>
            <geometry>
                <box size="0.015 0.045 0.015"/>
            </geometry>
            <origin xyz="0.0 -0.0225 0.0"/>
            <material name="mat_link_2"/>
        </visual>

        <collision>
            <geometry>
                <box size="0.015 0.045 0.015"/>
            </geometry>
            <origin xyz="0.0 -0.0225 0.0"/>
        </collision>

        <inertial>
            <mass value="0.05"/>
            <inertia ixx="9.375e-6" ixy="0.0" ixz="0.0" iyy="1.875e-6" iyz="0.0" izz="9.375e-6"/>
        </inertial>
    </link>

    <link name="leg_r1_3">
        <visual>
            <geometry>
                <box size="0.015 0.09 0.015"/>
            </geometry>
            <origin xyz="0.0 -0.045 0.0"/>
            <material name="mat_link_3"/>
        </visual>

        <collision>
            <geometry>
                <box size="0.015 0.09 0.015"/>
            </geometry>
            <origin xyz="0.0 -0.045 0.0"/>
        </collision>

        <inertial>
            <mass value="0.05"/>
            <inertia ixx="3.46875e-5" ixy="0.0" ixz="0.0" iyy="1.875e-6" iyz="0.0" izz="3.46875e-5"/>
        </inertial>
    </link>

    <joint name="r1_j0" type="revolute">
        <origin xyz="0.1 -0.05 0"/> <!--rpy="0.0 0.0 0.0"/>-->
        <parent link="body"/>
        <child link="leg_r1_0"/>
        <axis xyz="0.0 0.0 1.0"/>
        <limit lower="-1.0471975512" upper="2.09439510239" effort="0.0" velocity="0.0"/>
    </joint>
    
    <joint name="r1_j1" type="revolute">
        <origin xyz="0 -0.02 0"/> <!-- rpy="0.0 0.0 0.0"/> -->
        <parent link="leg_r1_0"/>
        <child link="leg_r1_1"/>
        <axis xyz="1.0 0.0 0.0"/>
        <limit lower="-2.09439510239" upper="2.09439510239" effort="0.0" velocity="0.0"/>
    </joint>
    
    <joint name="r1_j2" type="revolute">
        <origin xyz="0 -0.045 0"/> <!-- rpy="0.0 0.0 0.0"/> -->
        <parent link="leg_r1_1"/>
        <child link="leg_r1_2"/>
        <axis xyz="0.0 -1.0 0.0"/>
        <limit lower="-1.57" upper="1.57" effort="0.0" velocity="0.0"/>
    </joint>

    <joint name="r1_j3" type="revolute">
        <origin xyz="0 -0.045 0"/> <!-- rpy="0.0 0.0 0.0"/> -->
        <parent link="leg_r1_2"/>
        <child link="leg_r1_3"/>
        <axis xyz="1.0 0.0 0.0"/>
        <limit lower="-2.356" upper="2.356" effort="0.0" velocity="0.0"/>
    </joint>
    <!-- Leg R1 End -->

    <!-- Leg L1 Begin -->
    <link name="leg_l1_0">
        <visual>
            <geometry>
                <box size="0.015 0.02 0.015"/>
            </geometry>
            <origin xyz="0.0 0.01 0.0"/>
            <material name="mat_link_0"/>
        </visual>

        <collision>
            <geometry>
                <box size="0.015 0.02 0.015"/>
            </geometry>
            <origin xyz="0.0 0.01 0.0"/>
        </collision>

        <inertial>
            <mass value="0.05"/>
            <inertia ixx="2.604e-6" ixy="0.0" ixz="0.0" iyy="1.875e-6" iyz="0.0" izz="2.604e-6"/>
        </inertial>
    </link>

    <link name="leg_l1_1">
        <visual>
            <geometry>
                <box size="0.015 0.045 0.015"/>
            </geometry>
            <origin xyz="0.0 0.0225 0.0"/>
            <material name="mat_link_1"/>
        </visual>

        <collision>
            <geometry>
                <box size="0.015 0.045 0.015"/>
            </geometry>
            <origin xyz="0.0 0.0225 0.0"/>
        </collision>

        <inertial>
            <mass value="0.05"/>
            <inertia ixx="9.375e-6" ixy="0.0" ixz="0.0" iyy="1.875e-6" iyz="0.0" izz="9.375e-6"/>
        </inertial>
    </link>

    <link name="leg_l1_2">
        <visual>
            <geometry>
                <box size="0.015 0.045 0.015"/>
            </geometry>
            <origin xyz="0.0 0.0225 0.0"/>
            <material name="mat_link_2"/>
        </visual>

        <collision>
            <geometry>
                <box size="0.015 0.045 0.015"/>
            </geometry>
            <origin xyz="0.0 0.0225 0.0"/>
        </collision>

        <inertial>
            <mass value="0.05"/>
            <inertia ixx="9.375e-6" ixy="0.0" ixz="0.0" iyy="1.875e-6" iyz="0.0" izz="9.375e-6"/>
        </inertial>
    </link>

    <link name="leg_l1_3">
        <visual>
            <geometry>
                <box size="0.015 0.09 0.015"/>
            </geometry>
            <origin xyz="0.0 0.045 0.0"/>
            <material name="mat_link_3"/>
        </visual>

        <collision>
            <geometry>
                <box size="0.015 0.09 0.015"/>
            </geometry>
            <origin xyz="0.0 0.045 0.0"/>
        </collision>

        <inertial>
            <mass value="0.05"/>
            <inertia ixx="3.46875e-5" ixy="0.0" ixz="0.0" iyy="1.875e-6" iyz="0.0" izz="3.46875e-5"/>
        </inertial>
    </link>

    <joint name="l1_j0" type="revolute">
        <origin xyz="0.1 0.05 0"/> <!-- rpy="3.1416 0.0 0.0"/>-->
        <parent link="body"/>
        <child link="leg_l1_0"/>
        <axis xyz="0.0 0.0 -1.0"/>
        <limit lower="-1.0471975512" upper="2.09439510239" effort="0.0" velocity="0.0"/>
    </joint>
    
    <joint name="l1_j1" type="revolute">
        <origin xyz="0 0.02 0"/> <!-- rpy="0.0 0.0 0.0"/> -->
        <parent link="leg_l1_0"/>
        <child link="leg_l1_1"/>
        <axis xyz="-1.0 0.0 0.0"/>
        <limit lower="-2.09439510239" upper="2.09439510239" effort="0.0" velocity="0.0"/>
    </joint>
    
    <joint name="l1_j2" type="revolute">
        <origin xyz="0 0.045 0"/> <!-- rpy="0.0 0.0 0.0"/> -->
        <parent link="leg_l1_1"/>
        <child link="leg_l1_2"/>
        <axis xyz="0.0 -1.0 0.0"/>
        <limit lower="-1.57" upper="1.57" effort="0.0" velocity="0.0"/>
    </joint>

    <joint name="l1_j3" type="revolute">
        <origin xyz="0 0.045 0"/> <!-- rpy="0.0 0.0 0.0"/> -->
        <parent link="leg_l1_2"/>
        <child link="leg_l1_3"/>
        <axis xyz="-1.0 0.0 0.0"/>
        <limit lower="-2.356" upper="2.356" effort="0.0" velocity="0.0"/>
    </joint>
    <!-- Leg L1 End -->

    <!-- Leg R2 Begin -->
    <link name="leg_r2_0">
        <visual>
            <geometry>
                <box size="0.015 0.02 0.015"/>
            </geometry>
            <origin xyz="0.0 -0.01 0.0"/>
            <material name="mat_link_0"/>
        </visual>

        <collision>
            <geometry>
                <box size="0.015 0.02 0.015"/>
            </geometry>
            <origin xyz="0.0 -0.01 0.0"/>
        </collision>

        <inertial>
            <mass value="0.05"/>
            <inertia ixx="2.604e-6" ixy="0.0" ixz="0.0" iyy="1.875e-6" iyz="0.0" izz="2.604e-6"/>
        </inertial>
    </link>

    <link name="leg_r2_1">
        <visual>
            <geometry>
                <box size="0.015 0.045 0.015"/>
            </geometry>
            <origin xyz="0.0 -0.0225 0.0"/>
            <material name="mat_link_1"/>
        </visual>

        <collision>
            <geometry>
                <box size="0.015 0.045 0.015"/>
            </geometry>
            <origin xyz="0.0 -0.0225 0.0"/>
        </collision>

        <inertial>
            <mass value="0.05"/>
            <inertia ixx="9.375e-6" ixy="0.0" ixz="0.0" iyy="1.875e-6" iyz="0.0" izz="9.375e-6"/>
        </inertial>
    </link>

    <link name="leg_r2_2">
        <visual>
            <geometry>
                <box size="0.015 0.045 0.015"/>
            </geometry>
            <origin xyz="0.0 -0.0225 0.0"/>
            <material name="mat_link_2"/>
        </visual>

        <collision>
            <geometry>
                <box size="0.015 0.045 0.015"/>
            </geometry>
            <origin xyz="0.0 -0.0225 0.0"/>
        </collision>

        <inertial>
            <mass value="0.05"/>
            <inertia ixx="9.375e-6" ixy="0.0" ixz="0.0" iyy="1.875e-6" iyz="0.0" izz="9.375e-6"/>
        </inertial>
    </link>

    <link name="leg_r2_3">
        <visual>
            <geometry>
                <box size="0.015 0.09 0.015"/>
            </geometry>
            <origin xyz="0.0 -0.045 0.0"/>
            <material name="mat_link_3"/>
        </visual>

        <collision>
            <geometry>
                <box size="0.015 0.09 0.015"/>
            </geometry>
            <origin xyz="0.0 -0.045 0.0"/>
        </collision>

        <inertial>
            <mass value="0.05"/>
            <inertia ixx="3.46875e-5" ixy="0.0" ixz="0.0" iyy="1.875e-6" iyz="0.0" izz="3.46875e-5"/>
        </inertial>
    </link>

    <joint name="r2_j0" type="revolute">
        <origin xyz="0 -0.05 0"/> <!--rpy="0.0 0.0 0.0"/>-->
        <parent link="body"/>
        <child link="leg_r2_0"/>
        <axis xyz="0.0 0.0 1.0"/>
        <limit lower="-1.0471975512" upper="1.047197551" effort="0.0" velocity="0.0"/>
    </joint>
    
    <joint name="r2_j1" type="revolute">
        <origin xyz="0 -0.02 0"/> <!-- rpy="0.0 0.0 0.0"/> -->
        <parent link="leg_r2_0"/>
        <child link="leg_r2_1"/>
        <axis xyz="1.0 0.0 0.0"/>
        <limit lower="-2.09439510239" upper="2.09439510239" effort="0.0" velocity="0.0"/>
    </joint>
    
    <joint name="r2_j2" type="revolute">
        <origin xyz="0 -0.045 0"/> <!-- rpy="0.0 0.0 0.0"/> -->
        <parent link="leg_r2_1"/>
        <child link="leg_r2_2"/>
        <axis xyz="0.0 -1.0 0.0"/>
        <limit lower="-1.57" upper="1.57" effort="0.0" velocity="0.0"/>
    </joint>

    <joint name="r2_j3" type="revolute">
        <origin xyz="0 -0.045 0"/> <!-- rpy="0.0 0.0 0.0"/> -->
        <parent link="leg_r2_2"/>
        <child link="leg_r2_3"/>
        <axis xyz="1.0 0.0 0.0"/>
        <limit lower="-2.356" upper="2.356" effort="0.0" velocity="0.0"/>
    </joint>
    <!-- Leg R2 End -->

    <!-- Leg L2 Begin -->
    <link name="leg_l2_0">
        <visual>
            <geometry>
                <box size="0.015 0.02 0.015"/>
            </geometry>
            <origin xyz="0.0 0.01 0.0"/>
            <material name="mat_link_0"/>
        </visual>

        <collision>
            <geometry>
                <box size="0.015 0.02 0.015"/>
            </geometry>
            <origin xyz="0.0 0.01 0.0"/>
        </collision>

        <inertial>
            <mass value="0.05"/>
            <inertia ixx="2.604e-6" ixy="0.0" ixz="0.0" iyy="1.875e-6" iyz="0.0" izz="2.604e-6"/>
        </inertial>
    </link>

    <link name="leg_l2_1">
        <visual>
            <geometry>
                <box size="0.015 0.045 0.015"/>
            </geometry>
            <origin xyz="0.0 0.0225 0.0"/>
            <material name="mat_link_1"/>
        </visual>

        <collision>
            <geometry>
                <box size="0.015 0.045 0.015"/>
            </geometry>
            <origin xyz="0.0 0.0225 0.0"/>
        </collision>

        <inertial>
            <mass value="0.05"/>
            <inertia ixx="9.375e-6" ixy="0.0" ixz="0.0" iyy="1.875e-6" iyz="0.0" izz="9.375e-6"/>
        </inertial>
    </link>

    <link name="leg_l2_2">
        <visual>
            <geometry>
                <box size="0.015 0.045 0.015"/>
            </geometry>
            <origin xyz="0.0 0.0225 0.0"/>
            <material name="mat_link_2"/>
        </visual>

        <collision>
            <geometry>
                <box size="0.015 0.045 0.015"/>
            </geometry>
            <origin xyz="0.0 0.0225 0.0"/>
        </collision>

        <inertial>
            <mass value="0.05"/>
            <inertia ixx="9.375e-6" ixy="0.0" ixz="0.0" iyy="1.875e-6" iyz="0.0" izz="9.375e-6"/>
        </inertial>
    </link>

    <link name="leg_l2_3">
        <visual>
            <geometry>
                <box size="0.015 0.09 0.015"/>
            </geometry>
            <origin xyz="0.0 0.045 0.0"/>
            <material name="mat_link_3"/>
        </visual>

        <collision>
            <geometry>
                <box size="0.015 0.09 0.015"/>
            </geometry>
            <origin xyz="0.0 0.045 0.0"/>
        </collision>

        <inertial>
            <mass value="0.05"/>
            <inertia ixx="3.46875e-5" ixy="0.0" ixz="0.0" iyy="1.875e-6" iyz="0.0" izz="3.46875e-5"/>
        </inertial>
    </link>

    <joint name="l2_j0" type="revolute">
        <origin xyz="0 0.05 0"/> <!-- rpy="3.1416 0.0 0.0"/>-->
        <parent link="body"/>
        <child link="leg_l2_0"/>
        <axis xyz="0.0 0.0 -1.0"/>
        <limit lower="-1.0471975512" upper="1.047197551" effort="0.0" velocity="0.0"/>
    </joint>
    
    <joint name="l2_j1" type="revolute">
        <origin xyz="0 0.02 0"/> <!-- rpy="0.0 0.0 0.0"/> -->
        <parent link="leg_l2_0"/>
        <child link="leg_l2_1"/>
        <axis xyz="-1.0 0.0 0.0"/>
        <limit lower="-2.09439510239" upper="2.09439510239" effort="0.0" velocity="0.0"/>
    </joint>
    
    <joint name="l2_j2" type="revolute">
        <origin xyz="0 0.045 0"/> <!-- rpy="0.0 0.0 0.0"/> -->
        <parent link="leg_l2_1"/>
        <child link="leg_l2_2"/>
        <axis xyz="0.0 -1.0 0.0"/>
        <limit lower="-1.57" upper="1.57" effort="0.0" velocity="0.0"/>
    </joint>

    <joint name="l2_j3" type="revolute">
        <origin xyz="0 0.045 0"/> <!-- rpy="0.0 0.0 0.0"/> -->
        <parent link="leg_l2_2"/>
        <child link="leg_l2_3"/>
        <axis xyz="-1.0 0.0 0.0"/>
        <limit lower="-2.356" upper="2.356" effort="0.0" velocity="0.0"/>
    </joint>
    <!-- Leg L2 End -->

    <!-- Leg R3 Begin -->
    <link name="leg_r3_0">
        <visual>
            <geometry>
                <box size="0.015 0.02 0.015"/>
            </geometry>
            <origin xyz="0.0 -0.01 0.0"/>
            <material name="mat_link_0"/>
        </visual>

        <collision>
            <geometry>
                <box size="0.015 0.02 0.015"/>
            </geometry>
            <origin xyz="0.0 -0.01 0.0"/>
        </collision>

        <inertial>
            <mass value="0.05"/>
            <inertia ixx="2.604e-6" ixy="0.0" ixz="0.0" iyy="1.875e-6" iyz="0.0" izz="2.604e-6"/>
        </inertial>
    </link>

    <link name="leg_r3_1">
        <visual>
            <geometry>
                <box size="0.015 0.045 0.015"/>
            </geometry>
            <origin xyz="0.0 -0.0225 0.0"/>
            <material name="mat_link_1"/>
        </visual>

        <collision>
            <geometry>
                <box size="0.015 0.045 0.015"/>
            </geometry>
            <origin xyz="0.0 -0.0225 0.0"/>
        </collision>

        <inertial>
            <mass value="0.05"/>
            <inertia ixx="9.375e-6" ixy="0.0" ixz="0.0" iyy="1.875e-6" iyz="0.0" izz="9.375e-6"/>
        </inertial>
    </link>

    <link name="leg_r3_2">
        <visual>
            <geometry>
                <box size="0.015 0.045 0.015"/>
            </geometry>
            <origin xyz="0.0 -0.0225 0.0"/>
            <material name="mat_link_2"/>
        </visual>

        <collision>
            <geometry>
                <box size="0.015 0.045 0.015"/>
            </geometry>
            <origin xyz="0.0 -0.0225 0.0"/>
        </collision>

        <inertial>
            <mass value="0.05"/>
            <inertia ixx="9.375e-6" ixy="0.0" ixz="0.0" iyy="1.875e-6" iyz="0.0" izz="9.375e-6"/>
        </inertial>
    </link>

    <link name="leg_r3_3">
        <visual>
            <geometry>
                <box size="0.015 0.09 0.015"/>
            </geometry>
            <origin xyz="0.0 -0.045 0.0"/>
            <material name="mat_link_3"/>
        </visual>

        <collision>
            <geometry>
                <box size="0.015 0.09 0.015"/>
            </geometry>
            <origin xyz="0.0 -0.045 0.0"/>
        </collision>

        <inertial>
            <mass value="0.05"/>
            <inertia ixx="3.46875e-5" ixy="0.0" ixz="0.0" iyy="1.875e-6" iyz="0.0" izz="3.46875e-5"/>
        </inertial>
    </link>

    <joint name="r3_j0" type="revolute">
        <origin xyz="-0.1 -0.05 0"/> <!--rpy="0.0 0.0 0.0"/>-->
        <parent link="body"/>
        <child link="leg_r3_0"/>
        <axis xyz="0.0 0.0 1.0"/>
        <limit lower="-2.09439510239" upper="1.0471975512" effort="0.0" velocity="0.0"/>
    </joint>
    
    <joint name="r3_j1" type="revolute">
        <origin xyz="0 -0.02 0"/> <!-- rpy="0.0 0.0 0.0"/> -->
        <parent link="leg_r3_0"/>
        <child link="leg_r3_1"/>
        <axis xyz="1.0 0.0 0.0"/>
        <limit lower="-2.09439510239" upper="2.09439510239" effort="0.0" velocity="0.0"/>
    </joint>
    
    <joint name="r3_j2" type="revolute">
        <origin xyz="0 -0.045 0"/> <!-- rpy="0.0 0.0 0.0"/> -->
        <parent link="leg_r3_1"/>
        <child link="leg_r3_2"/>
        <axis xyz="0.0 -1.0 0.0"/>
        <limit lower="-1.57" upper="1.57" effort="0.0" velocity="0.0"/>
    </joint>

    <joint name="r3_j3" type="revolute">
        <origin xyz="0 -0.045 0"/> <!-- rpy="0.0 0.0 0.0"/> -->
        <parent link="leg_r3_2"/>
        <child link="leg_r3_3"/>
        <axis xyz="1.0 0.0 0.0"/>
        <limit lower="-2.356" upper="2.356" effort="0.0" velocity="0.0"/>
    </joint>
    <!-- Leg R3 End -->

    <!-- Leg L3 Begin -->
    <link name="leg_l3_0">
        <visual>
            <geometry>
                <box size="0.015 0.02 0.015"/>
            </geometry>
            <origin xyz="0.0 0.01 0.0"/>
            <material name="mat_link_0"/>
        </visual>

        <collision>
            <geometry>
                <box size="0.015 0.02 0.015"/>
            </geometry>
            <origin xyz="0.0 0.01 0.0"/>
        </collision>

        <inertial>
            <mass value="0.05"/>
            <inertia ixx="2.604e-6" ixy="0.0" ixz="0.0" iyy="1.875e-6" iyz="0.0" izz="2.604e-6"/>
        </inertial>
    </link>

    <link name="leg_l3_1">
        <visual>
            <geometry>
                <box size="0.015 0.045 0.015"/>
            </geometry>
            <origin xyz="0.0 0.0225 0.0"/>
            <material name="mat_link_1"/>
        </visual>

        <collision>
            <geometry>
                <box size="0.015 0.045 0.015"/>
            </geometry>
            <origin xyz="0.0 0.0225 0.0"/>
        </collision>

        <inertial>
            <mass value="0.05"/>
            <inertia ixx="9.375e-6" ixy="0.0" ixz="0.0" iyy="1.875e-6" iyz="0.0" izz="9.375e-6"/>
        </inertial>
    </link>

    <link name="leg_l3_2">
        <visual>
            <geometry>
                <box size="0.015 0.045 0.015"/>
            </geometry>
            <origin xyz="0.0 0.0225 0.0"/>
            <material name="mat_link_2"/>
        </visual>

        <collision>
            <geometry>
                <box size="0.015 0.045 0.015"/>
            </geometry>
            <origin xyz="0.0 0.0225 0.0"/>
        </collision>

        <inertial>
            <mass value="0.05"/>
            <inertia ixx="9.375e-6" ixy="0.0" ixz="0.0" iyy="1.875e-6" iyz="0.0" izz="9.375e-6"/>
        </inertial>
    </link>

    <link name="leg_l3_3">
        <visual>
            <geometry>
                <box size="0.015 0.09 0.015"/>
            </geometry>
            <origin xyz="0.0 0.045 0.0"/>
            <material name="mat_link_3"/>
        </visual>

        <collision>
            <geometry>
                <box size="0.015 0.09 0.015"/>
            </geometry>
            <origin xyz="0.0 0.045 0.0"/>
        </collision>

        <inertial>
            <mass value="0.05"/>
            <inertia ixx="3.46875e-5" ixy="0.0" ixz="0.0" iyy="1.875e-6" iyz="0.0" izz="3.46875e-5"/>
        </inertial>
    </link>

    <joint name="l3_j0" type="revolute">
        <origin xyz="-0.1 0.05 0"/> <!-- rpy="3.1416 0.0 0.0"/>-->
        <parent link="body"/>
        <child link="leg_l3_0"/>
        <axis xyz="0.0 0.0 -1.0"/>
        <limit lower="-2.09439510239" upper="1.0471975512" effort="0.0" velocity="0.0"/>
    </joint>
    
    <joint name="l3_j1" type="revolute">
        <origin xyz="0 0.02 0"/> <!-- rpy="0.0 0.0 0.0"/> -->
        <parent link="leg_l3_0"/>
        <child link="leg_l3_1"/>
        <axis xyz="-1.0 0.0 0.0"/>
        <limit lower="-2.09439510239" upper="2.09439510239" effort="0.0" velocity="0.0"/>
    </joint>
    
    <joint name="l3_j2" type="revolute">
        <origin xyz="0 0.045 0"/> <!-- rpy="0.0 0.0 0.0"/> -->
        <parent link="leg_l3_1"/>
        <child link="leg_l3_2"/>
        <axis xyz="0.0 -1.0 0.0"/>
        <limit lower="-1.57" upper="1.57" effort="0.0" velocity="0.0"/>
    </joint>

    <joint name="l3_j3" type="revolute">
        <origin xyz="0 0.045 0"/> <!-- rpy="0.0 0.0 0.0"/> -->
        <parent link="leg_l3_2"/>
        <child link="leg_l3_3"/>
        <axis xyz="-1.0 0.0 0.0"/>
        <limit lower="-2.356" upper="2.356" effort="0.0" velocity="0.0"/>
    </joint>
    <!-- Leg L3 1End -->
</robot>

Answer 1

(编辑)

随着事情的发展，有几个问题。这是值得保留的，而不是每个人，因为每一个都是一个绊脚石，简单地找出之间的距离身体。

• 最初的问题是:使用查询函数ComputeSignedDistancePairClosest不支持的网格，该查询不支持方框网格。
  • 很容易解决，交换一半的空间为盒子(为地面)。

• 新的问题(在8/28/2022发布的代码中)是，您正在获取所有几何图形的ids。
  • 在您的urdf中，您的身体被定义为<collision>和<visual>标记。这意味着每个物体都有两个(或更多)的几何图形。但并不是所有这些都可以用于远程查询--这就是您遇到的情况。您询问了一个关于几何学的距离查询，例如，它有一个插图角色。
  • 下面，我提供了一个代码片段，说明如何更正确地完成这个任务。编码风格是你我的混合体。对此我很抱歉。

...
# creating an inspector
diagram = builder.Build()

query_object = scene_graph.get_query_output_port().Eval(scene_graph_context)
inspector = query_object.inspector()

# The names of the bodies we care about -- we'll test their distance to the
# ground box.
Claw_name = ['leg_l1_3', 'leg_r1_3', 'leg_l2_3', 'leg_r2_3', 'leg_l3_3',
             'leg_r3_3']
body_geometries = {}
for body_name in Claw_name:
    f_id = plant.GetBodyFrameIdOrThrow(plant.GetBodyByName(body_name).index())
    # For distance queries, only get geometries with the proximity role.
    body_geometries[body_name] = inspector.GetGeometries(f_id, Role.kProximity)

# Get the GeometryId for the ground box.
world_ids = inspector.GetGeometries(scene_graph.world_frame_id(),
                                    Role.kProximity)
assert(len(world_ids) == 1)
Worldbody_id = world_ids[0]

assert(len(body_geometries) == 6)
Distances = np.zeros(6)
i = 0
for body_name in Claw_name:
    g_ids = body_geometries[body_name]
    assert(len(g_ids) == 1)
    Distances[i] = query_object.ComputeSignedDistancePairClosestPoints(
        Worldbody_id, g_ids[0]).distance
    i += 1
print(Distances)