Question

我用sympy通过拉格朗日推导了我的3连杆机器人的运动方程。结果，以theta_dot_dot = f(theta, theta_dot)和cos的形式得出的运动方程形式（sin）非常复杂。然后，我lambdified与drake一起使用的函数，将所有sympy.sin和sympy.cos替换为drake.sin，drake.cos。

可以在毫秒范围内有效地对最终函数进行数值评估（即给定theta, theta_dot，找到theta_dot_dot）。

然后我尝试使用直接转录进行轨迹优化。注意我没有使用DirectTranscription库，而是手动添加了必要的约束。

约束大致添加如下：

for i in range(NUM_TIME_STEPS-1):
    print("Adding constraints for t = " + str(i))
    tau = mp.NewContinuousVariables(3, "tau_%d" % i)

    next_state = mp.NewContinuousVariables(8, "state_%d" % (i+1))

    for j in range(8):
        mp.AddConstraint(next_state[j] <= (state_over_time[i] + TIME_INTERVAL*derivs(state_over_time[i], tau))[j])
        mp.AddConstraint(next_state[j] >= (state_over_time[i] + TIME_INTERVAL*derivs(state_over_time[i], tau))[j])

    state_over_time[i+1] = next_state
    tau_over_time[i] = tau

我现在面临的问题是，在每次添加约束的迭代中，我发现我的内存使用量增加了约70-100MB。这意味着在程序由于内存不足而崩溃之前，我的时间步数不能超过50。

我想知道如何使机器人的轨迹优化工作。显然，我可以尝试简化（手动或其他方式）运动方程式...但是我还能尝试其他方法吗？约束占用这么多内存是否正常？我在这里做错什么吗？

Answer 1

您正在通过复杂的方程式来推导德雷克的符号。使其更好是一个好目标，但您可能想通过对AddConstraint使用其他重载来避免它：

AddConstraint(your_method, lb, ub, vars)

https://drake.mit.edu/pydrake/pydrake.solvers.mathematicalprogram.html?highlight=addconstraint#pydrake.solvers.mathematicalprogram.MathematicalProgram.AddConstraint

这将使用您的python代码作为函数指针，并且应使用autodiff而不是符号。

使用sympy方程进行直接转录时内存使用率很高

1 个答案: