使用绝对值和除以决策变量的成本函数

Question

我正在尝试在 pydrake 数学程序中实现成本函数，但是每当我尝试除以决策变量并使用 abs() 时都会遇到问题。我尝试实现的简化版本如下，我尝试只包含我认为可能相关的内容。

    T = 50
    na = 3
    nq = 5

    prog = MathematicalProgram()
    h = prog.NewContinuousVariables(rows=T, cols=1, name='h')
    qd = prog.NewContinuousVariables(rows=T+1, cols=nq, name='qd')
    d = prog.NewContinuousVariables(1, name='d')
    u = prog.NewContinuousVariables(rows=T, cols=na, name='u')

    def energyCost(vars):
       assert vars.size == 2*na + 1 + 1
       split_at = [na, 2*na, 2*na + 1]
       qd, u, h, d = np.split(vars, split_at)
       return np.abs([qd.dot(u)*h/d])

    for t in range(T):
       vars = np.concatenate((qd[t, 2:], u[t,:], h[t], d))
       prog.AddCost(energyCost, vars=vars)

    initial_guess = np.empty(prog.num_vars())
    solver = SnoptSolver()
    result = solver.Solve(prog, initial_guess)

我得到的错误是：

RuntimeError                              Traceback (most recent call last)
<ipython-input-55-111da18cdce0> in <module>()
     22 initial_guess = np.empty(prog.num_vars())
     23 solver = SnoptSolver()
---> 24 result = solver.Solve(prog, initial_guess)
     25 print(f'Solution found? {result.is_success()}.')

RuntimeError: PyFunctionCost: Output must be of .ndim = 0 (scalar) and .size = 1. Got .ndim = 2 and .size = 1 instead.

据我所知，问题出在输出的维度上，但我不确定如何进行。我花了很多时间试图解决这个问题，但收效甚微。我也尝试将 np.abs 更改为 pydrake.math.abs，但随后出现以下错误：

---------------------------------------------------------------------------
TypeError                                 Traceback (most recent call last)
<ipython-input-56-c0c2f008616b> in <module>()
     22 initial_guess = np.empty(prog.num_vars())
     23 solver = SnoptSolver()
---> 24 result = solver.Solve(prog, initial_guess)
     25 print(f'Solution found? {result.is_success()}.')

<ipython-input-56-c0c2f008616b> in energyCost(vars)
     14     split_at = [na, 2*na, 2*na + 1]
     15     qd, u, h, d = np.split(vars, split_at)
---> 16     return pydrake.math.abs([qd.dot(u)*h/d])
     17 
     18 for t in range(T):

TypeError: abs(): incompatible function arguments. The following argument types are supported:
    1. (arg0: float) -> float
    2. (arg0: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
    3. (arg0: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression

Invoked with: [array([<AutoDiffXd 1.691961398933386e-257 nderiv=8>], dtype=object)]

任何帮助将不胜感激，谢谢！

Answer 1

一些观察：

您尝试使用的这种成本函数不需要强制使用 python 函数。你可以只说（即使它会按原样引发其他错误）prog.AddCost(np.abs([qd[t, 2:].dot(u[t,:])*h[t]/d])).
prog.AddCost 的参数必须是 Drake 标量表达式。因此，请确保您的 numpy 矩阵乘法 return 是一个标量。在上面的例子中，他们 return 一个 (1,1) numpy 数组。
要最小化绝对值，您需要比这更复杂的东西。在当前形式中，您传递的是不可微分的 objective 函数：求解器不太喜欢那样。假设您想最小化 abs(x)。优化中的一个标准技巧是添加一个额外的（松弛）变量，比如 s，并添加约束 s >= x、s >= -x，然后最小化 s 本身。所有这些约束和这个 objective 都是可微的和线性的。
关于 objective 除以优化变量。只要有可能，就应该避免这种情况。例如（我有 90% 的把握）如果您不提供，像 SNOPT 或 IPOPT 这样的求解器会将初始猜测设置为零。这意味着，如果您不提供自定义初始猜测，则在第一次评估约束时，求解器将除以零并且会崩溃。

Answer 2

顺便说一句，正如 Tobia 所提到的，在成本函数中划分决策变量可能会有问题。有两种方法可以避免这个问题

对您的决策变量施加一个界限，0 不包含在该界限内。例如，假设您要优化
```
min f(x) / y
```
如果您可以施加一个 y > 1 的界限，那么 SNOPT 将不会尝试使用 y=0，从而避免被零除的问题。
一个技巧是引入另一个变量作为除法的结果，然后最小化这个变量。

比如你要优化
```
min f(x) / y
```
您可以引入松弛变量 z = f(x) / y。并将这个问题表述为
```
min z
s.t f(x) - y * z = 0
```

使用绝对值和除以决策变量的成本函数

Cost function using absolute value and division by decision variables

python

drake