PyDrake中可变形体不显示及刚体附着方法问询

场景与问题描述 #

仿真场景设置 #

• 立柱（刚体）：固定于地面，半径0.3m、高度0.2m的圆柱
• 可变形圆柱：置于立柱顶部，半径0.055m、高度0.08m
• 腿部（刚体）：置于可变形圆柱顶部，半径0.3m、高度0.675m

连接需求 #

• 立柱与地面固定
• 可变形体与立柱螺栓连接（底面贴合立柱顶面）
• 腿部放置于可变形体上

现存问题 #

通过PyNgrok提供的可视化链接查看仿真时，可变形体未显示，立柱与腿部显示正常，已确认VTK模型文件无误。

额外咨询 #

如何将可变形体与刚体固定附着，而非仅保持接触？

实现代码 #

import numpy as np
import os

from pydrake.all import *

# 仿真参数设置
simulation_time = 8.0
realtime_rate = 1.0
time_step = 1e-2

# 部件尺寸参数
column_height = 0.20
joint_height = 0.08
joint_radius = 0.055
leg_height = 0.675

# 创建DiagramBuilder
builder = DiagramBuilder()

# 配置多体植物（必须使用SAP求解器支持可变形体）
plant_config = MultibodyPlantConfig()
plant_config.time_step = time_step
plant_config.discrete_contact_solver = "sap"

plant, scene_graph = AddMultibodyPlant(plant_config, builder)
parser = Parser(plant)

# 配置刚体接触属性
surface_friction = CoulombFriction(1.15, 1.15)
rigid_proximity_props = ProximityProperties()
rigid_proximity_props.AddProperty("hydroelastic", "resolution_hint", 1.0)
AddContactMaterial(friction=surface_friction, properties=rigid_proximity_props)

# 加载立柱URDF
base_column_path = "pippa_oneleg_column.urdf"
if not os.path.exists(base_column_path):
    raise FileNotFoundError(f"File not found: {base_column_path}")
column = parser.AddModels(base_column_path)

# 固定立柱到世界坐标系
colonna_model_instance = plant.GetModelInstanceByName("PIPPA_oneleg_column")
body_base = plant.GetBodyByName("Link_colonna", colonna_model_instance)
column_frame = body_base.body_frame()
plant.WeldFrames(plant.world_frame(), column_frame, RigidTransform([0, 0, 0]))

# 配置可变形体参数（钢材质）
density_ms = 7800
youngs_modulus_ms = 200e9
poisson_ratio_ms = 0.3
beta = 0.01
material_model_joint = MaterialModel.kLinearCorotated

# 加载可变形体VTK模型
joint_path = "volumetric_vtk_joint_no_triangles.vtk"
if not os.path.exists(joint_path):
    raise FileNotFoundError(f"Mesh file not found: {joint_path}")

# 设置可变形体初始位置（贴合立柱顶面）
X_WB = RigidTransform(RotationMatrix(), [0, 0, column_height])
joint_geometry = GeometryInstance(X_WB, Mesh(joint_path), "deformable_joint")

# 配置可变形体接触属性
deformable_proximity_props = ProximityProperties()
AddContactMaterial(friction=surface_friction, properties=deformable_proximity_props)
joint_geometry.set_proximity_properties(deformable_proximity_props)

# 注册可变形体
owned_deformable_model = DeformableModel(plant)
config = DeformableBodyConfig()
config.set_youngs_modulus(youngs_modulus_ms)
config.set_poissons_ratio(poisson_ratio_ms)
config.set_stiffness_damping_coefficient(beta)
config.set_mass_density(density_ms)
config.set_material_model(material_model_joint)

# 调整FEM离散化精度（优化可视化效果）
resolution_hint = 0.02
deformable_body_id = owned_deformable_model.RegisterDeformableBody(
    geometry_instance=joint_geometry,
    config=config,
    resolution_hint=resolution_hint
)
plant.AddPhysicalModel(owned_deformable_model)
deformable_model = owned_deformable_model

# PyNgrok配置
from pyngrok import ngrok
ngrok.set_auth_token("mytoken")
public_url = ngrok.connect(7000)
print(f"Meldis è disponibile al seguente link: {public_url}")

# 关键：连接可变形体顶点位置端口到SceneGraph
builder.Connect(
    deformable_model.vertex_positions_port(),
    scene_graph.get_source_configuration_port(plant.get_source_id()))

# 添加可视化器（确保在端口连接后初始化）
DrakeVisualizer().AddToBuilder(builder, scene_graph)

# 运行仿真
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
simulator = Simulator(diagram)
simulator.Initialize()
simulator.set_target_realtime_rate(realtime_rate)
simulator.AdvanceTo(simulation_time)

问题解决方案 #

一、可变形体不显示修复 #

1. 调整FEM离散化精度
   将resolution_hint从0.05调整为0.02，适配小尺寸可变形体的网格渲染需求，避免因网格过粗/过细导致不显示。
2. 修正初始位置
   原代码中可变形体初始z坐标为0.3，改为column_height（0.2），确保贴合立柱顶面，避免位置异常导致渲染失效。
3. 确认VTK模型格式
   必须使用四面体体积网格，而非表面三角网格，可通过VTK工具打开模型验证内部单元类型。
4. 调整可视化器初始化顺序
   将DrakeVisualizer添加到Builder的步骤放在端口连接之后，确保SceneGraph已加载所有数据源。

二、可变形体与刚体固定附着实现 #

通过锚定可变形体顶点到刚体的方式实现固定，在注册可变形体后添加以下代码：

# 获取场景上下文
diagram_context = diagram.CreateDefaultContext()
deformable_context = deformable_model.GetMyContextFromRoot(diagram_context)

# 获取可变形体初始顶点位置，筛选底部顶点（局部坐标系z<0.01的点）
initial_vertices = deformable_model.GetVertexPositions(deformable_context)
bottom_vertex_indices = np.where(initial_vertices[:, 2] < 0.01)[0]

# 将底部顶点锚定到立柱顶面中心
for idx in bottom_vertex_indices:
    deformable_model.AddDeformableBodyRigidAnchor(
        deformable_body_id,
        idx,
        body_base,
        Vector3(0, 0, column_height)  # 立柱顶面中心在刚体坐标系下的位置
    )

锚定多个顶点可避免单点固定导致的旋转偏移，实现类似螺栓连接的牢固固定效果。

内容的提问来源于stack exchange，提问作者Gneve