调用AddFixedConstraint()触发Segmentation fault错误求助

错误现象 #

调用AddFixedConstraint()时触发段错误，错误日志如下：

[2024-02-19 00:26:09.610] [console] [warning] DRAKE DEPRECATED: Use MultibodyPlantConfig::discrete_contact_approximation instead of MultibodyPlantConfig::discrete_contact_solver. The deprecated code will be removed from Drake on or after 2024-04-01.
HEY
Segmentation fault (core dumped)

其中"HEY"为自定义打印语句，确认错误发生在AddFixedConstraint()调用处。

问题代码 #

///////////////////////
/// Configure Plant ///
///////////////////////
drake::systems::DiagramBuilder<double> builder;

MultibodyPlantConfig plant_config;
plant_config.time_step = time_step;
plant_config.discrete_contact_solver = "sap";

auto [plant, scene_graph] = AddMultibodyPlant(plant_config, &builder);


////////////////////////
/// SETUP PLANE GROUND ///
////////////////////////
ProximityProperties rigid_proximity_props;
const CoulombFriction<double> surface_friction(1.15, 1.15);
AddContactMaterial({}, {}, surface_friction, &rigid_proximity_props);
rigid_proximity_props.AddProperty(drake::geometry::internal::kHydroGroup, drake::geometry::internal::kRezHint, 1.0);

Box ground{2, 2, 2};
const RigidTransformd X_WG(Eigen::Vector3d{0, 0, -1});
plant.RegisterCollisionGeometry(plant.world_body(), X_WG, ground, "ground_collision", rigid_proximity_props);
IllustrationProperties illustration_props;
illustration_props.AddProperty("phong", "diffuse", Vector4d(0.7, 0.5, 0.4, 0.8));
plant.RegisterVisualGeometry(plant.world_body(), X_WG, ground, "ground_visual", std::move(illustration_props));


/////////////////////////////
/// SETUP DEFORMABLE OBJECT ///
/////////////////////////////
auto owned_deformable_model = std::make_unique<DeformableModel<double>>(&plant);

DeformableBodyConfig<double> deformable_config;
deformable_config.set_youngs_modulus(E);
deformable_config.set_poissons_ratio(nu);
deformable_config.set_mass_density(density);
deformable_config.set_stiffness_damping_coefficient(stiffness_beta);

const std::string deformable_vtk = "{MY WORKSPACE PATH}/src/drake_controller/resource/pipe.vtk";
const double scale = 0.01; // 0.65
auto deformable_mesh = std::make_unique<Mesh>(deformable_vtk, scale);
const double kL = 0.09 * scale;

const RigidTransformd X_WB(drake::Vector3<double>(0.0, 0.0, 0.0));
auto torus_instance = std::make_unique<GeometryInstance>( X_WB, std::move(deformable_mesh), "deformable_object");

/* Minimumly required proximity properties for deformable bodies: A valid Coulomb friction coefficient. */
ProximityProperties deformable_proximity_props;
AddContactMaterial({}, {}, surface_friction, &deformable_proximity_props);
torus_instance->set_proximity_properties(deformable_proximity_props);

const double unused_resolution_hint = 1.0;
auto deformable_body_id = owned_deformable_model->RegisterDeformableBody(std::move(torus_instance), deformable_config, unused_resolution_hint);
DeformableModel<double>* deformable_model = owned_deformable_model.get();

plant.AddPhysicalModel(std::move(owned_deformable_model));

////////////////////////
/// SETUP RIGID OBJECT ///
////////////////////////
Parser parser(&plant);

auto peg_idx = parser.AddModels("{MY WORKSPACE PATH}/src/drake_controller/resource/1mm_cylinder.sdf");
const drake::multibody::RigidBody<double>& peg_body = plant.GetBodyByName("cylinder_body");
const drake::multibody::Frame<double>& peg_frame = plant.GetFrameByName("cylinder_top_center");

RigidTransformd W_X_PEG(Eigen::Vector3d{0,0,0});
plant.SetDefaultFreeBodyPose(peg_body, W_X_PEG);
AddFloatingRpyJoint(&plant, const_cast<drake::multibody::RigidBodyFrame<double>&>(peg_frame.body().body_frame()), peg_idx.at(0));

drake::math::RigidTransform<double> RIGID_X_DEFORMABLE(drake::Vector3<double>(0,0,0.02));
drake::math::RigidTransform<double> RIGID_X_GEOMETRY;
drake::geometry::Cylinder peg_mesh(0.048,0.04);
drake::geometry::Shape* peg_shape = peg_mesh.Clone().get();
std::cout << "HEY" << std::endl;
auto multibody_constraint_id = deformable_model->AddFixedConstraint(deformable_body_id, peg_body, RIGID_X_DEFORMABLE, *peg_shape, RIGID_X_GEOMETRY.SetIdentity());
std::cout << "HEY" << std::endl;

//////////////////////////////////
/// FINALIZE OBJECT REGISTRATION ///
//////////////////////////////////
plant.set_gravity_enabled(peg_idx.at(0), false);
plant.Finalize();

已排查情况 #

已确认排除Drake文档中AddFixedConstraint()的前4种异常触发条件：

• 已注册对应ID的可变形体
• 刚体与可变形体属于同一个MultibodyPlant
• 使用的Cylinder形状属于支持的类型
• 调用函数时Plant尚未Finalize

仅未排查第5种情况：没有可变形体顶点位于约束形状内部，导致未添加任何约束

问题分析与解决方案 #

1. 核心问题：悬空指针导致段错误 #

代码中peg_mesh.Clone().get()会创建临时std::unique_ptr<Shape>，调用get()获取裸指针后，临时智能指针立即销毁，导致peg_shape成为悬空指针。AddFixedConstraint()访问该指针指向的内存时触发段错误。

修复方式：
直接传递peg_mesh的引用，或保存Clone()的结果到智能指针中：

// 方式1：直接使用Cylinder对象的引用
auto multibody_constraint_id = deformable_model->AddFixedConstraint(
    deformable_body_id, peg_body, RIGID_X_DEFORMABLE, 
    peg_mesh, RIGID_X_GEOMETRY.SetIdentity()
);

// 方式2：保存Clone的结果
auto peg_shape = peg_mesh.Clone();
auto multibody_constraint_id = deformable_model->AddFixedConstraint(
    deformable_body_id, peg_body, RIGID_X_DEFORMABLE, 
    *peg_shape, RIGID_X_GEOMETRY.SetIdentity()
);

2. 验证约束形状与可变形体的位置重叠 #

检查RIGID_X_DEFORMABLE的变换参数，确保圆柱形状覆盖可变形管道的顶点。如果没有顶点位于约束形状内，会触发第5种异常，需调整变换参数或形状尺寸，保证两者有重叠区域。

3. 移除不必要的const_cast #

AddFloatingRpyJoint的参数不应使用const_cast强制转换const Frame，正确添加浮动关节的方式：

plant.AddFloatingRpyJoint(
    "peg_joint", plant.world_body(), 
    peg_body.body_frame(), 
    W_X_PEG.rotation(), W_X_PEG.translation()
);

4. 处理废弃API警告 #

将plant_config.discrete_contact_solver = "sap";替换为：

plant_config.discrete_contact_approximation = DiscreteContactApproximation::kSap;

避免后续版本的兼容性问题。

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