Drake::IK::AddMinimumDistanceLowerBoundConstraint未按预期工作问题排查

背景 #

我基于Drake开发UR5机械臂（搭配3D打印铲头和探针）的优化式逆运动学，核心需求：

• 过滤相邻连杆间的碰撞对
• 施加最小距离下界约束，确保机器人连杆间保持安全间距

碰撞过滤实现代码 #

def __exclude_collision_pairs(scene_graph: SceneGraph) -> None:
    # add collision filter to the scene graph.
    collision_filter_manager = scene_graph.collision_filter_manager()
    scene_graph_inspector = scene_graph.model_inspector()

    frame_name_to_id = dict()
    for frame_id in scene_graph_inspector.GetAllFrameIds():
        frame_name_to_id[scene_graph_inspector.GetName(frame_id)] = frame_id

    ground_geo_set = GeometrySet(frame_name_to_id['UR5::ground'])
    base_link_geo_set = GeometrySet(frame_name_to_id['UR5::base_link'])
    shoulder_link_geo_set = GeometrySet(frame_name_to_id['UR5::shoulder_link'])
    upperarm_link_geo_set = GeometrySet(frame_name_to_id['UR5::upperarm_link'])
    forearm_link_geo_set = GeometrySet(frame_name_to_id['UR5::forearm_link'])
    wrist1_link_geo_set = GeometrySet(frame_name_to_id['UR5::wrist1_link'])
    wrist2_link_geo_set = GeometrySet(frame_name_to_id['UR5::wrist2_link'])
    EE_link_geo_set = GeometrySet(frame_name_to_id['UR5::EE_link'])
    scoop_link_geo_set = GeometrySet(frame_name_to_id['UR5::scoop_link'])
    probe_link_geo_set = GeometrySet(frame_name_to_id['UR5::probe_link'])

    collision_filter_declaration = CollisionFilterDeclaration()
    collision_filter_declaration.ExcludeWithin(ground_geo_set)
    collision_filter_declaration.ExcludeWithin(base_link_geo_set)
    collision_filter_declaration.ExcludeWithin(shoulder_link_geo_set)
    collision_filter_declaration.ExcludeWithin(upperarm_link_geo_set)
    collision_filter_declaration.ExcludeWithin(forearm_link_geo_set)
    collision_filter_declaration.ExcludeWithin(wrist1_link_geo_set)
    collision_filter_declaration.ExcludeWithin(wrist2_link_geo_set)
    collision_filter_declaration.ExcludeWithin(EE_link_geo_set)
    collision_filter_declaration.ExcludeWithin(scoop_link_geo_set)
    collision_filter_declaration.ExcludeWithin(probe_link_geo_set)
    collision_filter_declaration.ExcludeBetween(ground_geo_set, base_link_geo_set)
    collision_filter_declaration.ExcludeBetween(base_link_geo_set, shoulder_link_geo_set)
    collision_filter_declaration.ExcludeBetween(shoulder_link_geo_set, upperarm_link_geo_set)
    collision_filter_declaration.ExcludeBetween(upperarm_link_geo_set, forearm_link_geo_set)
    collision_filter_declaration.ExcludeBetween(forearm_link_geo_set, wrist1_link_geo_set)
    collision_filter_declaration.ExcludeBetween(wrist1_link_geo_set, wrist2_link_geo_set)
    collision_filter_declaration.ExcludeBetween(wrist2_link_geo_set, EE_link_geo_set)
    collision_filter_declaration.ExcludeBetween(wrist2_link_geo_set, scoop_link_geo_set)
    collision_filter_declaration.ExcludeBetween(EE_link_geo_set, scoop_link_geo_set)
    collision_filter_declaration.ExcludeBetween(probe_link_geo_set, scoop_link_geo_set)
    collision_filter_manager.Apply(collision_filter_declaration)

IK实现代码 #

builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0)
parser = Parser(plant)
robot = parser.AddModels(self._bot_urdf_path)
__exclude_collision_pairs(scene_graph)  
plant.Finalize()
diagram = builder.Build()
ik_context = diagram.CreateDefaultContext()
ik_plant_context = plant.GetMyMutableContextFromRoot(ik_context)
ee_frame = plant.GetFrameByName(self._bot_params['ee_link_name'])
ik = InverseKinematics(plant, ik_plant_context)
# ik.AddMinimumDistanceLowerBoundConstraint(bound=0.0001, influence_distance_offset=0.0005) # hard-coded
ik.prog().AddBoundingBoxConstraint(plant.GetPositionLowerLimits(),  # [-2pi, 2pi]
    plant.GetPositionUpperLimits(), ik.q()) 
if init_guess is None:
    init_guess = self.GetConfiguration()
ik.prog().SetInitialGuess(ik.q(), init_guess)
ik.prog().AddQuadraticCost(
    (ik.q() - init_guess).dot(np.diag(weights)).dot(ik.q() - init_guess))
ik.AddPositionConstraint(ee_frame, [0, 0, 0], plant.world_frame(), 
    translation - 1e-5 * np.ones(3), translation + 1e-5 * np.ones(3))
ik.AddOrientationConstraint(ee_frame, RotationMatrix(), plant.world_frame(), 
                            RotationMatrix(orientation), 1e-5)
result = Solve(ik.prog())
if kVerbose and result.get_solver_id().name() != 'SNOPT':
    print("ScoopingBot::__inverse_kinematics -- SNOPT is not available")
if kVerbose and not result.is_success():
    print("ScoopingBot::__inverse_kinematics -- IK failed")
    print(result.GetInfeasibleConstraintNames(ik.prog()))
    return False, None
return True, result.GetSolution(ik.q())

问题现象 #

已确认碰撞过滤逻辑生效，但启用AddMinimumDistanceLowerBoundConstraint后（即使将bound设为0），出现以下异常：

• 情况1：最小距离约束被违反
• 情况2：约束未被违反，但末端位姿约束必须放宽至1e-2（1cm误差，无法接受）

移除该约束后，IK运行完全正常，位姿约束仅需1e-3~1e-5的放宽；若将bound设为-100，约束可正常生效且无碰撞问题。

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