我想在Drake中使用Atlas机器人模型进行轨迹优化。除了垃圾狗的例子和这个仓库，我在这里尝试了跳跃运动优化。基本上，我改变了in_stance序列，添加了q的Initial guess，并添加了不同的q_cost。
但是似乎很难得到最优解，SNOPT输出是:
SNOPTA EXIT 10 -- the problem appears to be infeasible
SNOPTA INFO 13 -- nonlinear infeasibilities minimized
这里有一个视频来展示优化的轨迹。
我有一些想法来探索改进解决方案的方法:
- 使用简化模型。我认为这个问题可能更容易解决。
- 仔细查看每个约束。例如，在跳跃的情况下，我认为最好约束
x和z轴的angular_动量，而不约束y轴的angular_动量。 - 为所有变量添加一个更好的初始猜测。
- 将参考轨迹添加到成本中以指导求解器。
- 增加一些成本来稳定腿的飞行阶段。
我不确定我应该先做哪一个，还是我错过了一些重要的东西，导致问题无法解决?
谢谢你的建议!
感谢您的快速回复@戴宏凯。我检查了matlab版本，并在这里和testJump.m找到了讨论。
我通过使用InverseKinematics类添加了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);
- 更好的初步猜测有很大帮助。你可以尝试先求解一个运动学轨迹(不考虑力)，然后用运动学轨迹初始化轨迹优化变量。
- 当优化不可行时，可以检查在不可行解处违反了哪些约束。使用
result.GetInfeasibleConstraints(prog)函数，它返回违反约束的列表，然后您可以尝试放松/删除约束。您可以调用prog.RemoveConstraint()函数来删除约束，或者使用constraint.UpdateLowerBound(),constraint.UpdateUpperBound()来放松约束边界。
对于Atlas跳跃运动，我们确实有一个Matlab实现。如果你检查旧的matlab版本的Drake，然后在文件夹Drake/matlab/solvers/trajectoryOptimization/dev/testJump。m，利用本文的思想
实现Atlas跳跃运动的轨迹优化。在testJump。m，我没有用运动学轨迹作为最初的猜测。我计算了机器人站在地面上的初始姿态，接触力均匀分布在每条腿上。
在testjumpm:
中还有其他细微差别- 我将接触力的决策变量按质量* g进行缩放，因此接触力的决策变量值大致为1的数量级。在当前版本的Drake中，可以在
MathematicalProgram中尝试SetVariableScaling()方法。 在testJump - 。m，浮基的方向用欧拉角表示。在当前版本的Drake中，浮点基方向由单位四元数表示。四元数的时间积分比欧拉角要复杂得多。你可以尝试QuaternionEulerIntegrationConstraint的单位四元数，其余的关节可以像往常一样使用欧拉/中点积分。