Tips：持续完善文档，记录每一次优化与踩坑体会，有助于后续论文撰写与项目迭代！

毕业设计每周工作总结

未来24周计划

26.05.2025-30.05.2025

02.06.2025-06.06.2025

09.06.2025-13.06.2025

16.06.2025-20.06.2025

23.06.2025-27.06.2025

30.06.2025-04.07.2025

周任务（07.06.2025 – 07.11.2025）

本周思考

  1. 优先级：先把 APGD 插到现有的 LCP 建模流程里，实现对接；
  2. 可配置性：把 APGD 加入求解器枚举与下拉选项，方便在运行时自由切换；
  3. 后续准备：完成 QP 建模（基于 Chrono 风格），为并行化、性能优化打基础。

Data : 2025-07-10

TASK:求解器集成

APGD 求解器集成步骤

  1. 源码接入
    • 将 RBDSolverAPGD.h/cpp 添加到项目中，确保它们参与编译；
    • 在 RBDClusterLagrangeMultipliers.cpp 中加入： ```cpp #include “RBDSolverAPGD.h”
    • 执行全量 Rebuild，确保无编译或链接错误。
  2. 扩展求解器枚举

在 RBDScene.h 中：

enum CONSTRAINTSOLVERTYPE {
  CST_LAGRANGEDANTZIG,
  CST_LAGRANGEGS,
  CST_LAGRANGEAUTO,
  CST_LAGRANGEINTERACTIVEDANTZIG,
  CST_LAGRANGEINTERACTIVEGS,
  CST_LAGRANGEINTERACTIVEAUTO,
  CST_LAGRANGEDANTZIGEXP,
  CST_IMPULSEBASED,
  CST_PENALTY,
  CST_SEQUENTIALIMPULSE,
  CST_LAGRANGEAPGD   // ← 新增 APGD
};
  1. 集群工厂中开启 APGD

在 RBDScene.cpp 的 createNewCluster() 方法里：

else if (t == CST_LAGRANGEAPGD) {
  // 为非交互式 Lagrange 乘子集群
  return new RBDClusterLagrangeMultipliers(this);
  // 如需调试/单步，可改为：
  // return new RBDClusterLagrangeMultipliersInteractive(this);
}
  1. 在 doTimeStep() 中植入 APGD 支持

在 RBDClusterLagrangeMultipliers::doTimeStep(…) 的 solver 选择 switch 中添加：

case RBDScene::CST_LAGRANGEAPGD:
  myLcp = new RBDLcpAPGD(
    matA.rows(),                   // LCP 维度
    numberEqualityConstraints,     // nub
    myScene->numberIterations(),   // maxIters
    myScene->getTol(),             // tol（需在 RBDScene 中暴露）
    myScene->getAccel()            // accel（需在 RBDScene 中暴露）
  );
  break;

其余分支保持不变，并做好失败时增加 CFM 或退化到 GS 的回退策略。

  1. 界面 & 配置同步

5.1 Qt 下拉菜单

在 VSPluginRBDynamXOptions.cpp：

constraintSolverValues.append({
  "apgd",
  tr("Lagrange Multipliers, APGD")
});

5.2 读取并应用设置

在 MainSimStateExtension::slotSyncSettings() 中：

else if (solverName.toLower() == "apgd")
  myRBDScene->setConstraintSolverType(
    VSLibRBDynamX::RBDScene::CST_LAGRANGEAPGD
  );

下一步 & 建议

2025-07-11

TASK:测试对比

将新增的对默认的lagrangmultipliers的求解器（加Dantzig）效果做对比：

问题猜测：

同一个单摆模型，用默认的 Lagrange‐multiplier+Dantzig/GS 求解器能正常振动， 但用你接进去的 APGD 解算器就直接自由落体”，

APGD效果视频：

基本上说明你的 APGD 分支在求解约束力的时候返回了全零（或者极小）的 λ， 导致根本没把摆杆的杆约束力施加上去，物体直接受重力加速度下落。

单摆的 holonomic equality constraint（等式约束）写作：

\[g(\mathbf{q}) = \bigl\lVert\,\mathbf{x}_{\text{质点}} - \mathbf{x}_{\text{支点}}\bigr\rVert - L = 0.\]

周任务（14.06.2025-18.07.2025）

2025.07.14

TASK :调试定位问题原因

目前对于问题的设想是：

最后计算出来的结果（即最终的解向量 $x$ 和 $w$）

换到一个 Qt 容器里才能用 qDebug() 一次性打印，最常见的做法是先构造一个 QVector，然后 append 每个分量，最后再打印：

//调试信息
// 计算误差 ‖xnew - xprev‖∞
        err = 0;
        for (int i = 0; i < n; ++i)
            err = std::max(err, std::abs(xnew[i] - xprev[i]));
        // ——— 调试打印，每 10 步看看 λ0、λrod、err ———
        if (k % 30 == 0) {
            qDebug() << "iter" << k
                << " LA[0]=" << xprev[0]
                //<< " λrod=" << xprev[rodIndex]   // 请确保 rodIndex 已定义为“杆长约束”在向量中的下标
                << " err=" << err;
        }


        if (err < tol) break;

LA[0] 就是你第 0 号约束（也就是单摆的杆长等式约束）的拉格朗日乘子——物理上它对应的是杆上的张力 𝑇 这是单摆的初始值： 单摆初始值 第一次迭代（iter 0）LA[0]≈0：因为你从水平方向（或某个初始角度）“松手”那一刻，杆几乎不需要任何张力来维持摆球的位置，所以乘子近乎 0。

随着迭代推进，LA[0] 慢慢上升并收敛到 ~0.11：那就是 APGD 算出来的、在该时刻保持杆长不变所需的张力大小。

err 下降到很小，说明解在收敛。

值很正常

杆约束力公式

单摆杆上的张力 (T)（即第 0 号拉格朗日乘子 (\lambda_0)）满足：

\[T = \lambda_0 = m\!\left(\frac{v^2}{L} + g \cos\theta\right)\]

这是单摆完全脱离后的值： 单摆完全脱离后的debug值

原因猜测：具体为什么会“完全脱离”？

为什么不一开始就拉住？

问题确定，开始求解

方法1. 开启 Warm-start：

所析，第一步迭代时 λ₀ 被投影回 0，完全没有初始张力。 在 APGD 创建后调用：

solver.EnableWarmStart(true);

这样会使用上一步结束时的 λ 值作为新步的初始猜测，从而在刚开始迭代时就能提供一定的张力缓冲。
我需要重新添加一个函数，使他满足这个功能

完成了这个函数后需要在RBDClusterLagrangeMultipliers.cpp启动

	case (RBDScene::CST_LAGRANGEAPGD):
		{
			// APGD 求解器
			myLcp = new RBDLcpAPGD(
				matA.rows(),                   // LCP 维度
				numberEqualityConstraints,     // 均等约束数量 nub
				myScene->numberIterations(),   // 最大迭代次数
				1e-6,                          // 收敛容忍度 tol
				0.9                            // Nesterov 加速系数 accel
			);
			// **这里启用 warm-start**
			myLcp->EnableWarmStart(true);
			break;
		}
		default:

很复杂！！！！！！！

完成，但是结果并不理想。

这个把上一个时间步计算的最优结果带入并不违背APGD算法，伪码里的 γ₀ 本来就是一个“外部给定”的初始猜测（Algorithm 1 的输入里就写了 γ₀）。 它并没有硬性要求把 γ₀ 设为 0，零向量只是最常见也最简单的 choice。

所以第一种方法暂时失败，可以作为之后优化的一种尝试。

控制变量，在进行第二个方法前关掉第一个方法

2025.07.15

TASK:尝试不同方法

方法2. 调整最大迭代次数和容忍误差

// 先关闭 warm-start
solver.EnableWarmStart(false);

// 再设置迭代次数和容忍度

// 在这VSPluginRBDynamXMainSimStateExtension.cpp更改
// 所以我直接选择快速暴力的解法，直接写死

// 1) 用派生类指针接收 new 出来的对象
			RBDLcpAPGD* apgd = new RBDLcpAPGD(
				matA.rows(),                 // LCP 维度
				numberEqualityConstraints,   // nub
				500, // maxIters
				1e-10,                        // tol
				0.9                          // accel
			);

暂时暴力求解，直接写死。

效果

1.关闭warmStart，调整迭代次数从100到500：

通过观察debug的值： Only500iters 发现最初的迭代次数并不满足500次，所以判断时过早满足了容忍值，所以提前结束了迭代，所以，我们选择更新他的精度 为1e-10

2.关闭warmStart，调整迭代次数从100到500，并tol = 1e-10 :

可以发现效果好一些，没有出现上面的视频的左端受力的自由落体，贴近一点实际情况，但是因为精度很高， 所以仿真速度会慢很多。 观察Qdegug的值可以看到： Iters500+Tol1e 10

通过对上面的对比，我觉得最最开始第一时间步长的计算值很重要。

所以我猜测初始值不应该从零开始，是所以我在第三次尝试打开warmstart来进行初始值的优化

3.开启warmStart，调整迭代次数从100到500，并tol = 1e-10 : 发现效果和关闭warmStart，调整迭代次数从100到500一摸一样，所以我这边个人的判断是应该把LA[O]的值设置为1开始， 就像伪代码中所写的，所以我进行更改初始值为1.

// warm-start 相关
if (!first_step) {
    if (auto* apgd = dynamic_cast<RBDLcpAPGD*>(myLcp)) {
        apgd->SetLambda(prev_lambda);
    }
}
else {
    // 第一次也要初始化，改成全 1 向量
    if (auto* apgd = dynamic_cast<RBDLcpAPGD*>(myLcp)) {
        prev_lambda.assign(matA.rows(), 1.0);  // ← 由 0.0 改成 1.0
        apgd->SetLambda(prev_lambda);
    }
    first_step = false;
}

效果没有变化，虽然初始值是1，但是在第一个时间不长中，计算出来的最优解还是很小，所以导致没有约束力？？

既然方法2失败，我们继续使用方法3

方法3. 缩小时间步长

即便 λ 再准，一步积分时间太大也会把摆球拉得太远，约束一时跟不上，

修改源代码，加入double Lk, tk, theta;

在修改这个的时候我发现，我之前的步长设置和加速因子的设置是个定值，并不符合原来的算法设计， 所以我选择去重新完善cpp源代码查看效果。 设置还是基于尝试3：开启warmStart，调整迭代次数从100到500，并tol = 1e-10 ， 这次速度快很多，但是不断重试 CFM 并打印 “solveMsg”。

这是一种「边调参边重试」的鲁棒性增强策略——如果 APGD（或其它 LCP 求解器）第一次没解出来，就不断往矩阵对角线上加大 CFM（相当于松弛约束），直到能得到一个可行解；如果 CFM 太大还不行，就直接用零力近似，保证仿真继续跑下去。 Add New Para

从日志看，APGD 在第 0 步时用 γ₀＝1 开始，err＝1 正常；到了第 30 步，它竟然把 LA[0] 跑到 −0.07，err 一下子飙到 6.9e7，然后更后面直接爆到 1e174 这种天文数字，最后落到你的那段 solveMsg 循环里不停重试 CFM 并打印 “solveMsg”。

根本原因

你在 solve() 里把伪码中的 back-tracking line-search 那一块暂时用

bool cond = true;
if (cond) break;

给“短路”掉了，导致：
1.永远只做一次投影:
不管当前步长 t=1/Lk 到底是否合适，代码都“认为”下降条件满足，马上就跳出 while(true)。
2.步长没收敛调整:
因为从没进入那段 L_temp *=2; t=1/L_temp; 的循环，t 一直保持初始的 1/L₀——而这个 t 可能对你的 A 矩阵根本不适合，第一次迭代就可能一步跨得太大，造成 xnew 远远偏离可行区间，再往后每步都在“极度震荡”或指数爆炸。
3.最终 solve() 失败:
当 err 无限放大之后，solve() 返回 false，控制流就跑到 while (!myLcp->solve() || !myLcp->validSolution()) 里，不停加 CFM（也没用），最终就退化到零解或卡在“solveMsg”里。

解决办法 ：你需要把那段 cond = true; if (cond) break; 换成真正的能量下降检测

fine，改完直接卡死，真是个天才！！！ 应该是走进了一个死循环，得不出结果就一直在尝试。现在给这个循环最大的尝试次数，让他20次后跳出。

不卡死了，但是效果还是不行，所以我在想，是不是摩擦力的原因，而且他一直报错： “APGD backtrack reached BT_MAX, force accept” 就说明 Armijo 条件始终不满足，所以每次都翻倍 L_temp 到上限才强制跳出。

所以我需要重新看一下它整个的LCP的建模过程，加一下debug

	// —— 上面已经填充好了 frictionNormalIndices 和 addFriction ——

		// ===== 在这里插入调试打印 =====
		qDebug() << ">>> frictionNormalIndices:";
		for (int i = 0; i < frictionNormalIndices.size(); ++i)
			qDebug() << i << ":" << frictionNormalIndices[i];

		qDebug() << ">>> addFriction:";
		for (int i = 0; i < addFriction.size(); ++i)
			qDebug() << i << ":" << addFriction[i];
		// ===== 调试打印结束 =====

Friction Debug 这两个容器的 .size() 根本是 0，摩擦相关的数组根本没分配过长度（它们的 size() 一直是 0）， 那我暂时不想更改它的这个建模方式，我想先不用摩擦力，我的apgd文件需要更改。
专门为APGD写一个函数关闭所有的摩擦建模投影。

apgd->EnableFriction(false);    // <— 这一行，关闭摩擦

新思路：去参考一下正常的迭代求解器，去看他计算出来的约束力的大小，从而去确定到底计算错误在哪里？？

方法4. 优化步长（α）与预调度因子

方法5. 使用 Baumgarte 或 约束柔性化

方法4.5暂时放弃尝试

2025.07.16

TASK:更换实验思路

按照昨天预设的实验思路，今天首先去看被的正常运行的求解器所得到的正确结果是怎样的。
所以对Dantzig求解器进行调试。

 #endif
	delete[] schlupf;
	//delete[] rightSide;


// 1) Print out the current working directory (where the file will land)
qDebug() << "[RBDLcpDantzig] Current working directory:" 
         << QDir::currentPath();

// 2) Dump λ (x vector) to “dantzig_lambda.dat”
{
    QFile file("dantzig_lambda.dat");
    if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) {
        qWarning() 
          << "[RBDLcpDantzig] ERROR: could not open " 
          << "dantzig_lambda.dat" 
          << " for writing";
    }
    else {
        QTextStream out(&file);
        out.setRealNumberPrecision(6);
        out.setRealNumberNotation(QTextStream::FixedNotation);
        for (int i = 0; i < x->size(); ++i) {
            out << (*x)[i] << "\n";
        }
        file.close();
        qDebug() 
          << "[RBDLcpDantzig] Successfully wrote" 
          << x->size() 
          << "lambda values to dantzig_lambda.dat";
    }
}

// 3) (Optional) Still print them in the debug console
{
    QString xs; xs.reserve(x->size()*16);
    for (int i = 0; i < x->size(); ++i)
        xs += QString(" %1").arg((*x)[i], 0, 'f', 6);
    qDebug() << "[RBDLcpDantzig] Final x:" << xs;
}


	delete[] myLo;
	myLo = 0;

把APGD和Dantzig都存下来做对比

Run 约束分量 Dantzig λ APGD λ 差值 (APGD–Dantzig)
0 λ₀ (下界) –0.028331 0.028331 0.056662
1 λ₁ (上界) –0.000000 –0.000000 0.000000

从刚才贴出的两组数据看，APGD 和 Dantzig 对同一约束（单摆长度约束上下界）算出的 λ₀／λ₁ 是完全相反的：

为什么会有上下界？

物理上，你的单摆杆长必须 刚好等于 某个长度 L。把这个 “ 𝑔(𝑞)=0和-g(q)=0”（等式）转成互补格式时，通常写成：这两条分别保证“不短于”和“不长于”。

如何修正：

1.保证初始可行：

“每次新开一次仿真，就删掉上一次仿真产生的日志文件；但在同一次仿真内部的多个步长调用中，不重复删除”：使用flag：

	static bool first = true;
	if (first) {
		first = false;
		QFile::remove("dantzig_lambda.dat");
	}

你的物理模型里，Dantzig 那边的 b 实际上是 −b APGD ​ ，或者你在 APGD 里不小心把符号搞反了，那么肯定会出现一模一样大小但符号相反的 λ。

 grad[i] = sum - b[i];       // ← 改成 sum - b[i]
    f_y += 0.5 * yk[i] * rowSum - yk[i] * b[i];
    …
    f_x += 0.5 * xnew[i] * rowSum - xnew[i] * b[i];

2025.07.17

TASK:如下表。

现在遇到的问题计算得到的力为0，排除一切后去debug是不是步长计算的错误：

不是步长的问题，是一些参数初始化，使用了默认值，初始化的问题。

NEW ISSUE:现在前期已经正常摆动，但是还是在从右边摆动到左边会出现脱离，效果请看视频：

现在开始寻找这个结果出现的原因：

猜测：约束力算歪了，导致球体被“甩”出铰链。

Index APGD λ Dantzig λ
0 −7.6286 −0.1942
1 8.9401 0.0823
2–5 ~0 0, 0, 0, −0.001

APGD 算出的 λ 过大，导致“铰链”瞬间被“甩”开——约束力根本把摆杆拉偏离了长度约束。

我在想，我是不把RHS 符号用反了（梯度公式写错），因为可能是和chrono的建模设计是相反的，修改一下试试。

排除这个问题，没错，但是代码跑飞了，可能是迭代？？

ok，成功解决！！！

将杆子的迭代次数增加后，可以计算得到正确的结果，所以现在APGD基于LCP无摩擦力的建模下已经成功运行

从100次提高到500次

效果如下视频：

现在尝试添加摩擦力并进行其他关节等式约束的情境下测试。

按我对理论的理解，目前LCP建模加上摩擦力并不影响QP建模的结果，因为无外乎是从系统矩阵中提取出来两个参数，也就是补矩阵和 右端项用构建QP建模，从而计算最佳的结果。

任务定义

所以从现在来看：目前的工作可以从新定义为两部分：