I'm using drake to generate a walking gait for a legged robot. To do this, I need a way to determine when it's appropriate to apply contact forces. For this I'm using ComputeSignedDistancePairClosestPoints between the claws (feet) of the robot and a box which I'm using as the floor (world body):
# 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)
As a test I'm doing the following:
Distances = np.zeros(5)
for i in range(6):
print(Distances)
Distances[i] = query_object.ComputeSignedDistancePairClosestPoints(Worldbody_id, Claw_id[i]).distance
Unfortunately this produces the following error:
[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
This implies that the geometry type used by the geometry corresponding to the FR_Claw_id differs from the one corresponding to FL_Claw_id. This is despite the fact that they're both meshes (obj files).
Does anybody know what I'm not seeing?
Using a urdf with much simpler geometry (only boxes), this problem persists (this urdf was made by hand, so the intertial data is probably wrong).
Here's the code used:
# 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
Thhe custom function defined here, SignedDistance, uses ComputeSignedDistancePairwiseClosestPoints as a proxy to find the distances I need:
Distances = np.zeros(6)
for i in range(len(Claw_name)):
Distances[i] = SignedDistance(Claw_name[i]).distance
print(Distances)
Curiously this works as opposed to using ComputeSignedDistancePairClosestPoints:
Distances = np.zeros(6)
for i in range(len(Claw_id)):
Distances[i] = query_object.ComputeSignedDistancePairClosestPoints(Worldbody_id, Claw_id[i])
print(Distances)
Here's the urdf I used:
<?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>
(Edited)
There were several issues as things evolved. It's worth keeping not of each of them as each served as a stumbling block to simply finding out distances between bodies.
<collision> and <visual> tags. That means you have two (or more) geometries associated with each body. But not all of them can be used for distance queries -- which is what you ran into. You asked a distance query about a geometry that had, for example, an illustration role....
# 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)