如何强制数学规划生成非AutoDiffXd类型决策变量及相关问题

我尝试将数学规划生成的决策变量状态向量q设置到模型的plant中，通过距离查询验证该状态是否有效。按我的理解，需要将plant和场景图连接的diagram转换为与q匹配的类型（AutoDiffXd或float），但转换为float以执行query_object.ComputeSignedDistanceQuery时出现错误：

RuntimeError: System ::_ of type drake::systems::Diagramdrake::AutoDiffXd does not support scalar conversion to type double (because System ::_::scene_graph of type drake::geometry::SceneGraphdrake::AutoDiffXd does not support scalar conversion to type double)

问题在于AutoDiffXd类型的SceneGraph无法转换为float。我想到的“最简单”解决方案是强制数学规划生成的状态向量不为AutoDiffXd类型，请问这是否可行？如果不可行，有没有其他方法可以避免将SceneGraph转换为autodiff（考虑到仅需转换plant来设置位置）？

使用extract_value处理决策变量矩阵的切片q[1, :]（此时q.dtype == AutoDiffXd为真）时，出现以下错误：

TypeError: ExtractValue(): incompatible function arguments. The following argument types are supported:

1. (auto_diff_matrix: numpy.ndarray[object[m, n]]) -> numpy.ndarray[numpy.float64[m, n]]

Invoked with: array([Variable('q(1,0)', Continuous),
Variable('q(1,1)', Continuous),
Variable('q(1,2)', Continuous), Variable('q(1,3)', Continuous),
Variable('q(1,4)', Continuous), Variable('q(1,5)', Continuous),
Variable('q(1,6)', Continuous), Variable('q(1,7)', Continuous),
Variable('q(1,8)', Continuous), Variable('q(1,9)', Continuous),
Variable('q(1,10)', Continuous), Variable('q(1,11)', Continuous),
Variable('q(1,12)', Continuous), Variable('q(1,13)', Continuous),
Variable('q(1,14)', Continuous), Variable('q(1,15)', Continuous),
Variable('q(1,16)', Continuous), Variable('q(1,17)', Continuous),
Variable('q(1,18)', Continuous), Variable('q(1,19)', Continuous),
Variable('q(1,20)', Continuous)], dtype=object)

针对问题1的解决方法 #

• 强制数学规划生成非AutoDiffXd向量不可行
  数学规划（如Drake的MathematicalProgram）依赖AutoDiffXd完成梯度计算，强制改为float会破坏优化的梯度推导流程，导致优化无法正常运行，因此该方案不可取。

• 避免转换SceneGraph的替代方案
  无需将带SceneGraph的整个Diagram转换为AutoDiffXd，而是单独创建一个double类型的Plant+SceneGraph Diagram用于距离查询：

  1. 基于相同的模型文件，重新构建double类型的Plant和SceneGraph，并组装成Diagram；
  2. 拿到数学规划输出的AutoDiffXd类型状态q后，用extract_value(q)提取对应float数值向量；
  3. 将该float向量设置到double类型的Plant中，执行距离查询即可。

此方法既保留了数学规划的AutoDiffXd计算能力，又通过独立的double类型场景图完成验证，彻底避开SceneGraph的标量转换问题。

针对问题2的解决方法 #

报错原因是传入的是决策变量（Variable）数组，而extract_value仅支持处理AutoDiffXd类型数组。数学规划中的决策变量本身并非AutoDiffXd，只有代入表达式或求解后得到的解才会是AutoDiffXd类型。

解决方式：

• 若要获取优化后的解数值：先通过result.GetSolution(q)得到AutoDiffXd类型的解向量，再对该向量的切片调用extract_value；
• 若要直接获取决策变量当前赋值：用np.array([var.GetValue() for var in q[1,:]])（需确保变量已被赋值）。

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