如何提高轨迹优化结果

Question

我想用Drake中的Atlas机器人模型做轨迹优化。休闲 litterdog example and this repository, I tried the jump motion optimization here。基本上，我更改了 in_stance 序列，添加了 q 的 Initial guess，并添加了不同的 q_cost.

但似乎很难得到最优解，SNOPT输出为：

 SNOPTA EXIT  10 -- the problem appears to be infeasible
 SNOPTA INFO  13 -- nonlinear infeasibilities minimized

这里有一个video来展示优化后的轨迹。

我有一些想法，探索改进解决方案的方法：

使用简化模型。我觉得这个问题可能更容易解决。
仔细查看每个约束。例如，在跳跃的情况下，我认为最好约束 x 和 z 轴 angular_momentum 而让 y 轴 angular_momentum 不受约束。
为所有变量添加更好的初始猜测。
将参考轨迹添加到成本中以指导求解器。
添加一些成本来稳定腿的飞行阶段。

不知道该先做哪一个，还是漏掉了重要的东西导致问题无法解决？
谢谢你的建议！

感谢@Hongkai Dai 的快速回复。我检查了 matlab 版本并在 here 和 testJump.m.

找到了讨论

我使用 InverseKinematics class 添加了 q 的初步猜测。也会尝试添加接触力轨迹和CoM轨迹初始猜测。

而在 testJump.m 中，这是您的初步猜测吗？不是运动轨迹初始猜测吧？

x_seed = zeros(cdfkp.num_vars,1);
x_seed(cdfkp.h_inds) = 0.06;
x_seed(cdfkp.q_inds(:)) = reshape(bsxfun(@times,qstar,ones(1,nT)),[],1);
x_seed(cdfkp.com_inds(:)) = reshape(bsxfun(@times,com_star,ones(1,nT)),[],1);
x_seed(cdfkp.lambda_inds{1}(:)) = reshape(bsxfun(@times,1/num_edges*ones(num_edges,4,1),ones(1,1,nT)),[],1);
x_seed(cdfkp.lambda_inds{2}(:)) = reshape(bsxfun(@times,1/num_edges*ones(num_edges,4,1),ones(1,1,nT)),[],1);

Answer 1

更好的初始猜测有很大帮助。您可以尝试先求解运动轨迹（不考虑力），然后使用运动轨迹初始化轨迹优化变量。
当优化不可行时，您可以检查在不可行的解决方案处违反了哪些约束。使用 function result.GetInfeasibleConstraints(prog)，它 returns 违反约束的列表，然后你可以尝试 relax/remove 约束。您可以调用 prog.RemoveConstraint() 函数来移除约束，或者使用 constraint.UpdateLowerBound()、constraint.UpdateUpperBound().

对于 Atlas 跳跃运动，我们有一个 Matlab 实现。如果你查看旧的 matlab version of Drake, then in the folder drake/matlab/solvers/trajectoryOptimization/dev/testJump.m, it implements trajectory optimization for Atlas jumping up motion, using the idea in this paper

在testJump.m中，我没有使用运动轨迹作为初始猜测。我计算了机器人站在地面上的初始姿势，接触力均匀分布在每条腿上。

testJump.m中还有其他细微差别：

我按质量 * g 缩放了接触力的决策变量。所以接触力的决策变量值大致在1的数量级。在当前版本的Drake中，您可以尝试MathematicalProgram中的SetVariableScaling()方法。
在testJump.m中，浮基的方向用欧拉角表示。在当前版本的 Drake 中，浮动基方向由单位四元数表示。四元数的时间积分比欧拉角复杂得多。单位四元数可以试试QuaternionEulerIntegrationConstraint，其余关节可以照常使用Euler/Midpoint积分。

如何提高轨迹优化结果

How to improve the trajectory optimization results

drake