Drake 中直接搭配的非线性（非多项式）成本函数

Question

我正在尝试为滑翔机制定轨迹优化问题，我想在其中最大化平均水平速度。我将系统制定为drakesystem，状态向量由位置和速度组成。

目前，我有如下内容：

dircol = DirectCollocation(
    plant,
    context,
    num_time_samples=N,
    minimum_timestep=min_dt,
    maximum_timestep=max_dt,
)

... # other constraints etc

horisontal_pos = dircol.state()[0:2] # Only (x,y)
time = dircol.time()
dircol.AddFinalCost(-w.T.dot(horisontal_pos) / time)

据我从文档中了解到，其中 AddFinalCost() 应该用最终值替换 state() 和 time() 的所有实例。 min_dt 是非零值，w 是线性权重向量。

但是，我收到以下错误消息

Expression (...) is not a polynomial. ParseCost does not support non-polynomial expression.

这让我觉得无法添加我正在寻找的成本函数类型。有什么我遗漏的吗？

提前致谢！

Answer 1

首先，成本函数应该是一个标量，但是你是一个向量值horisontal_pos / time，它有两个包含position_x / dt和position_y / dt的条目，即一个向量作为成本。您应该改为提供标量价值成本。

其次，我不清楚为什么要将 time 分摊到最终费用中。据我了解，您希望最终位置靠近原点，所以类似于 position_x² + position_y²。代码看起来像

dircol.AddFinalCost(horisontal_pos[0]**2 + horisontal_pos[1]**2)

Answer 2

调用AddFinalCost(e)时，e是一个符号表达式，只有当e是状态的多项式函数（更准确地说，是二次函数或一个线性函数）。因此，您会看到抱怨成本不是多项式的错误。

您可以这样添加费用

def average_speed(v):
    x = v[0]
    time_steps = v[1:]
    return v[0] / np.sum(time_steps)
    
h_vars = [dircol.timestep[i] for i in range(N-1)]
dircol.AddCost(average_speed, vars=[dircol.state(N-1)[0]] + h_vars)

它使用一个函数average_speed来评估平均速度。您可以在 https://github.com/RobotLocomotion/drake/blob/e5f3c3e5f7927ef675066d97d3afac55d3481305/bindings/pydrake/solvers/test/mathematicalprogram_test.py#L590

中找到执行此操作的示例

Drake 中直接搭配的非线性（非多项式）成本函数

Nonlinear (non-polynomial) cost function with DirectCollocation in Drake

drake