如何在Drake中可视化数学规划Jacobian并检查约束/变量顺序

一、可视化约束相对于优化变量的Jacobian稀疏性 #

在Drake中，你可以通过MathematicalProgram的GetJacobianSparsity方法获取约束对优化变量的Jacobian稀疏结构，再结合matplotlib绘制稀疏图，实现和CasADi类似的效果：

import matplotlib.pyplot as plt
from pydrake.solvers.mathematicalprogram import MathematicalProgram

# 假设已构建好你的MathematicalProgram实例 prog
# 获取所有约束对所有优化变量的Jacobian稀疏性
sparsity = prog.GetJacobianSparsity(prog.GetAllConstraints(), prog.GetAllVariables())
# 转换为scipy稀疏矩阵用于可视化
sparse_matrix = sparsity.ToScipyMatrix()

# 绘制稀疏图
plt.figure(figsize=(10,10), dpi=100)
plt.spy(sparse_matrix, markersize=0.5)
plt.title("Constraint Jacobian Sparsity")
plt.xlabel("Optimization Variables")
plt.ylabel("Constraints")
plt.show()

如果只需要查看特定约束或变量子集的Jacobian，只需将prog.GetAllConstraints()或prog.GetAllVariables()替换为对应的子集即可，比如[prog.GetConstraintByIndex(0)]或自定义的变量列表。

二、检查与优化变量/约束的顺序 #

1. 查看变量与约束的顺序 #

Drake中优化变量和约束的顺序严格遵循添加顺序：

• 变量顺序：按prog.NewContinuousVariables()、prog.NewBinaryVariables()等方法的调用顺序排列，后续添加的变量排在前面变量之后。通过prog.GetAllVariables()可获取完整变量列表，按顺序遍历即可确认每个变量的位置。
• 约束顺序：按prog.AddConstraint()系列方法的调用顺序排列，涵盖等式约束、不等式约束等所有约束类型。通过prog.GetAllConstraints()可获取完整约束列表，按顺序遍历查看。

你可以打印变量名称或约束信息来确认顺序：

# 打印变量顺序及名称
for idx, var in enumerate(prog.GetAllVariables()):
    print(f"Variable {idx}: {var.GetName()}")

# 打印约束顺序及信息
for idx, constraint in enumerate(prog.GetAllConstraints()):
    print(f"Constraint {idx}: {constraint}")

2. 优化顺序提升Ipopt求解效率 #

变量和约束的顺序直接影响Jacobian矩阵的带宽，Ipopt的求解效率对稀疏矩阵结构极为敏感，以下是实用优化技巧：

• 聚类相关变量：将物理关联的变量（如同一刚体的位置、速度变量）放在一起，减少Jacobian中非零元素的跨距。
• 聚类相关约束：将依赖相同变量子集的约束集中排列，比如同一动力学模块的约束放在一起。
• 优先排列稠密块：若Jacobian存在稠密子块，将对应的变量和约束集中放置，降低矩阵带宽。
• 避免零散分布：尽量避免变量/约束交替排列导致Jacobian非零元素分散，增加Ipopt计算开销。

你可以通过调整NewContinuousVariables和AddConstraint的调用顺序来重新排列，也可使用prog.ReorderVariables()手动指定变量顺序（需传入覆盖所有变量的新顺序列表）：

# 假设new_var_order是自定义的变量顺序列表
prog.ReorderVariables(new_var_order)

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