建模接口

该章节介绍以下内容：

• OPTIMake支持的问题类型
• 如何通过OPTIMake提供的符号化建模接口定义问题
• 如何通过外部C/C++函数定义问题

问题形式

OPTIMake求解以下的优化问题：

$$\begin{equation*} \begin{split} &\quad \quad \quad \min_{v_1,\cdots,v_N} \sum_{i=1}^{N} l(v_i, p) + l_e(v_N, p) \\ &\begin{split} \text{subject to} &\quad f_s(v_1, p) = 0,\\ &\quad f(v_{i}, v_{i+1}, p) = 0,\quad i=1,\cdots,N - 1,\\ &\quad f_e(v_N, p) = 0,\\ &\quad g(v_i, p) \geq 0,\quad i=1,\cdots,N \end{split} \end{split} \end{equation*}$$

该优化问题为一个具有 $N$ 个stage的优化问题, 该优化问题可能为一个非线性非凸的优化问题. 其中:

• $v_1,\cdots,v_N \in \mathbb{R}^{n_{v}}$ 为优化变量
• $p\in \mathbb{R}^{n_{p}}$ 为参数
• $l：\mathbb{R}^{n_{v}} \times \mathbb{R}^{n_{p}} \rightarrow \mathbb{R}$ 为stage objective
• $l_e：\mathbb{R}^{n_{v}} \times \mathbb{R}^{n_{p}} \rightarrow \mathbb{R}$ 为终点处额外的objective
• $f_s：\mathbb{R}^{n_{v}} \times \mathbb{R}^{n_{p}} \rightarrow \mathbb{R}^{n_{f_s}}$ 为起点等式约束
• $f：\mathbb{R}^{n_{v}} \times \mathbb{R}^{n_{v}} \times \mathbb{R}^{n_{p}} \rightarrow \mathbb{R}^{n_{f}}$ 为等式约束
• $f_e：\mathbb{R}^{n_{v}} \times \mathbb{R}^{n_{p}} \rightarrow \mathbb{R}^{n_{f_e}}$ 为终点等式约束
• $g：\mathbb{R}^{n_{v}} \times \mathbb{R}^{n_{p}} \rightarrow \mathbb{R}^{n_{g}}$ 为不等式约束

问题定义

下面为定义问题的例子, 该例子指定了问题的名称为vehicle且具有10个stage：

Python

prob = multi_stage_problem(name='vehicle', N=10)

其中, 函数入参的定义如下：

• name: str 问题的名称, 该名称会用在生成代码的文件名, 函数名等.

• N: int 问题的stage数目, 必须大于等于1.

在完成问题名称和stage数的定义后, 下面说明如何定义优化问题中的参数, 优化变量及约束.

Note

因为上述的优化问题为一个多stage的优化问题, 等式约束与不等式约束的表达式在所有stage上都一致, 所以不需要在每个stage上都单独定义, 只需要定义一次该表达式即可 (参数与优化变量同理) .

parameter定义

parameter为在优化过程中不变的量, 由用户在调用求解前给定, 比如车身长度length, 质量mass.

OPTIMake支持两种类型的parameter：stage-independent parameter与stage-dependent parameter. stage-dependent parameter在不同stage可以有不同的值, 而stage-independent parameter在所有stage的值都一致 (可以节约存储与简化设置) .

下面为定义优化变量的例子：

Python

length = prob.parameter(name='length', stage_dependent=False)
mass = prob.parameter('mass')
xLowerBound = prob.parameter('xLowerBound', stage_dependent=True)

其中, 函数入参的定义如下：

• name: str parameter的名称.

• stage_dependent: bool, optional parameter是否为stage-dependent. 默认值为True, 表示parameter为stage-dependent.

亦或者通过list的方式定义：

Python

length, mass, xLowerBound = prob.parameters(
    ['length', 'mass', 'xLowerBound'], 
    stage_dependent=False)

优化变量定义

优化变量为在优化过程中变化的量, 比如车辆的转角控制量delta, 位置状态x, y. 在定义优化变量时, 可以同时定义优化变量的硬边界、软边界以及违反软边界时的惩罚 (见建模接口 > 约束软化了解OPTIMake中的软约束定义) .

下面为定义优化变量的例子：

Python

delta = prob.variable(name='delta', hard_lowerbound=-0.5, hard_upperbound=0.5)

# xLowerBound与xUpperBound为已定义的parameter
x = prob.variable(
    name = 'x', 
    hard_lowerbound=xLowerBound, 
    hard_upperbound=xUpperBound,
    soft_lowerbound=-0.2,
    soft_upperbound=0.2,
    weight_soft_lowerbound=100.0, 
    weight_soft_upperbound=100.0,
    penalty_type_soft_lowerbound='quadratic',
    penalty_type_soft_upperbound='l1')
y = prob.variable('y')
theta = prob.variable('theta')

其中, 函数入参的定义如下：

• name: 优化变量的名称, string类型
• hard_lowerbound (optional): 优化变量的硬下界, 可以为常数或关于参数$p$的表达式, 默认值为-inf, 表示无下界
• hard_upperbound (optional): 优化变量的硬上界, 可以为常数或关于参数$p$的表达式, 默认值为inf, 表示无上界
• soft_lowerbound (optional): 优化变量的软下界, 可以为常数或关于参数$p$的表达式, 默认值为-inf, 表示无下界
• soft_upperbound (optional): 优化变量的软上界, 可以为常数或关于参数$p$的表达式, 默认值为inf, 表示无上界
• weight_soft_lowerbound (optional): 优化变量的软下界的惩罚权重, 必须为非负, 可以为常数或关于参数$p$的表达式, 默认值为0.0, 表示无惩罚
• weight_soft_upperbound (optional): 优化变量的软上界的惩罚权重, 必须为非负, 可以为常数或关于参数$p$的表达式, 默认值为0.0, 表示无惩罚
• penalty_type_soft_lowerbound (optional): 优化变量的软下界的惩罚类型, 可选值为'quadratic'或'l1', 默认值为'quadratic'
• penalty_type_soft_upperbound (optional): 优化变量的软上界的惩罚类型, 可选值为'quadratic'或'l1', 默认值为'quadratic'

objective定义

objective为需要最小化的目标函数. OPTIMake支持以下类型的objective定义：

• general_objective：通用类型, 通过表达式直接定义
• least_square_objective：least square类型
• external_general_objective：通用类型, 通过外部C/C++函数定义

在完成objective定义后, 通过以下接口最小化该objective：

Python

prob.objective(obj)

同理, 通过以下接口加入对最后一个点的objective：

Python

prob.end_objective(obj)

Note

• 只能调用一次prob.objective或prob.end_objective设置objective

general_objective

下面为通过表达式直接定义objective的例子：

Python

# wyref, wphi为已定义的parameter
obj = general_objective(wxref * (x * sin(phi) - xref)**2 + wyref * (y - yref)**2 + wphi * phi**2 + 0.01 * delta**2 + 0.1 * v**2)
prob.objective(obj)

least_square_objective

当objective为以下形式时, 可通过least square接口定义, 其中$r_j(v, p)$为残差项, $w_j(p)$为其对应的权重项. \begin{equation*} l(v_i, p) = \frac{1}{2}\sum_{j}w_j(p) r^2_j(v_i, p) \end{equation*}

下面为通过least square接口定义objective的例子：

Python

r = [x * sin(phi) - xref, y - yref, phi, delta, v]
w = [wxref, wyref, wphi, 0.01, 0.1]
obj = least_square_objective(residuals=r, weights=w)
prob.objective(obj)

其中, least_square_objective接口的函数入参的定义如下：

• residuals: 残差项, 对应$r(v, p)$, 类型为list
• weights (optional): 残差权重, 对应$w(p)$, 可以为常数或关于参数$p$的表达式的list, 其默认值为全为1.0的list

Note

当objective通过least square接口定义时, 其Hessian通过Gauss-Newton方式近似计算.

external_general_objective

通过外部C/C++函数定义的general_objective. 在建模时, 可以指定其Hessian的稀疏性加速运算.

下面为通过external_general_objective接口定义objective的例子：

Python

obj_spy_hess = \
[[1, 0, 0, 0],
 [0, 1, 0, 0],
 [0, 0, 1, 0],
 [0, 0, 0, 1]]
obj = external_general_objective(sparsity_hessian=obj_spy_hess)
prob.objective(obj)

其中, external_general_objective接口的入参如下：

• sparsity_hessian (optional): objective的Hessian的sparsity pattern, 维度为$n_{v} \times n_{v}$的对称矩阵, 0代表稀疏, 1代表稠密；该参数为optional参数, 其默认值为全1.0

等式约束定义

等式约束描述了相邻stage之间优化变量的等式关系, OPTIMake支持以下类型的等式约束：

• differential_equation：微分方程
• discrete_equation：离散方程
• external_discrete_equation：离散方程, 通过外部C/C++函数定义

在完成约束定义后, 通过以下接口添加该约束：

Python

prob.equality(eq, weight_soft, penalty_type)

其中, equality接口的函数入参的定义如下 (约束软化详见 建模接口 > 约束软化) ：

• eq: 等式约束, 类型为differential_equation, discrete_equation, external_discrete_equation
• weight_soft: 该约束软化时的惩罚权重list, 必须为非负, 可以为常数或关于参数$p$的表达式, 默认值为全inf, 表示硬约束
• penalty_type: 该约束软化的惩罚类型list, 可选值为'none', 'quadratic', 'l1', 默认值为全'none', 表示硬约束

Note

• 当约束类型为external_discrete_equation或differential_equation (discretization_method为'irk2'或'irk4') 时, 只能调用一次prob.equality设置约束
• 当约束为其他类型时, 可调用多次prob.equality添加多个约束

differential_equation

differential_equation定义了$\dot x = h(v,p)$的形式的等式约束, 需要指定离散步长与离散方法将其离散化.

下面为通过differential_equation接口定义等式约束的例子：

Python

# ts, length为已定义的parameter
eq = differential_equation(
    state=[x, y, phi], 
    state_dot=[v * cos(phi), v * sin(phi), v * tan(delta) / length], 
    stepsize=ts, 
    discretization_method='forward_euler')
prob.equality(eq)

其中, differential_equation接口的函数入参的定义如下：

• state: 状态量, 对应$x$, 类型为优化变量的list
• state_dot: 状态微分量, 对应$h(v,p)$, 类型为list
• stepsize: 离散步长, 可以为常数或关于参数$p$的表达式

• discretization_method: 离散方法, 可选值为：

  • 'forward_euler'：显式欧拉
  • 'erk4'：显式四阶龙格库塔
  • 'backward_euler'：隐式欧拉
  • 'trapezoid'：梯形积分 (trapezoidal rule)
  • 'irk2'：隐式二阶龙格库塔 (midpoint rule)
  • 'irk4'：隐式四阶龙格库塔

discrete_equation

discrete equation为 $h_{next}(v_{i+1}, p_{i+1}) = h_{this}(v_i, p)$, 下面为通过discrete equation接口定义等式约束的例子 (使用forward_euler方法的离散系统) ：

Python

# ts, length为已定义的parameter
eq = discrete_equation(
    expr_this_stage=[
        x + ts * v * cos(phi),
        y + ts * v * sin(phi),
        phi + ts * v * tan(delta) / length],
    expr_next_stage=[x, y, phi])
prob.equality(eq)

其中, discrete equation接口的入参如下：

• expr_this_stage: 当前stage的函数表达式, 对应 $h_{this}(v_i, p)$, 类型为list
• expr_next_stage: 下一个stage的函数表达式, 对应 $h_{next}(v_{i+1}, p_{i+1})$, 类型为list

Note

若只需要描述当前stage内优化变量的等式关系, 可将expr_this_stage或expr_next_stage设置为全零.

external_discrete_equation

通过外部C/C++函数定义的discrete_equation, 其形式为$h_{next}(v_{i+1}, p_{i+1}) = h_{this}(v_i, p)$. 在建模时, 可以指定其Jacobian & Hessian的稀疏性加速运算.

下面为external_discrete_equation的例子：

Python

eq_spy_jac_this = \
[[1, 0, 1, 1],
 [0, 1, 1, 1],
 [0, 0, 1, 1]]

eq_spy_jac_next = \
[[1, 0, 0, 0],
 [0, 1, 0, 0],
 [0, 0, 1, 0]]

eq_spy_hess_this = \
[[0, 0, 0, 0],
 [0, 0, 0, 0],
 [0, 0, 1, 1],
 [0, 0, 1, 1]]

eq_spy_hess_next = \
[[0, 0, 0, 0],
 [0, 0, 0, 0],
 [0, 0, 0, 0],
 [0, 0, 0, 0]]

eq = external_discrete_equation(
    dim = 3, 
    sparsity_jacobian_this_stage=eq_spy_jac_this,
    sparsity_jacobian_next_stage=eq_spy_jac_next,
    sparsity_hessian_this_stage=eq_spy_hess_this,
    sparsity_hessian_next_stage=eq_spy_hess_next)
prob.equality(eq)

其中, external_discrete_equation接口的入参如下：

• dim：该约束的维度
• sparsity_jacobian_this_stage (optional): $h_{this}(v_i, p)$的Jacoboian的sparsity pattern, 维度为dim$\times n_{v}$的矩阵, 0代表稀疏, 1代表稠密；其默认值为全1.0
• sparsity_jacobian_next_stage (optional): $h_{next}(v_{i+1}, p_{i+1})$的Jacoboian的sparsity pattern, 维度为dim$\times n_{v}$的矩阵, 0代表稀疏, 1代表稠密；其默认值为全1.0
• sparsity_hessian_this_stage (optional): $y^T h_{this}(v_i, p)$的Hessian的sparsity pattern (y为维度为dim$\times 1$的拉格朗日乘子) , 维度为$n_{v} \times n_{v}$的对称矩阵, 0代表稀疏, 1代表稠密；其默认值为全1.0
• sparsity_hessian_next_stage (optional): $y^T h_{next}(v_{i+1}, p_{i+1})$的Hessian的sparsity pattern, 维度为$n_{v} \times n_{v}$的对称矩阵, 0代表稀疏, 1代表稠密；其默认值为全1.0

不等式约束定义

OPTIMake支持以下类型的不等式约束：

• general_inequality：通用类型, 通过表达式直接定义
• external_general_inequality：通用类型, 通过外部C/C++函数定义

在完成约束定义后, 通过以下接口添加该约束：

Python

prob.inequality(ineq, weight_soft, penalty_type)

其中, inequality接口的函数入参的定义如下 (约束软化下详见 建模接口 > 约束软化) ：

• ineq: 等式约束, 类型为general_inequality, external_general_inequality
• weight_soft: 该约束软化时的惩罚权重list, 必须为非负, 可以为常数或关于参数$p$的表达式, 默认值为全inf, 表示硬约束
• penalty_type: 该约束软化的惩罚类型list, 可选值为'none', 'quadratic', 'l1', 默认值为全'none', 表示硬约束

Note

• 当约束类型为external_general_inequality时, 只能调用一次prob.inequality设置约束
• 当约束为其他类型时, 可调用多次prob.inequality添加多个约束

general_inequality

下面为通过general_inequality定义不等式约束的例子：

Python

# -1.0 <= x + y <= 1.0
ineq = general_inequality(
    expr  =  [x + y, x + y],
    sign  =  ['>=',  '<='],
    bound =  [-1.0,  1.0])
prob.inequality(ineq)

其中, general_inequality接口的入参如下：

• expr: 不等式约束的表达式的list
• sign: 不等式约束的符号, 可选值为">=", "<="的list
• bound: 不等式约束的边界, 可以为常数或关于参数$p$的表达式的list

当所有的不等式约束都具备一样的sign和bound时, 可简化sign和bound的定义. 如以上的不等式约束也可通过以下实现:

Python

# -1.0 <= x + y <= 1.0
ineq = general_inequality(
    expr  =  [x + y, -(x + y)],
    sign  =  '<=',
    bound =  1.0)
prob.inequality(ineq)

external_general_inequality

通过外部C/C++函数定义的general_inequality, 其表达式为$g(v,p) \geq 0$. 在建模时, 可以指定其Jacobian & Hessian的稀疏性加速运算.

下面为external_general_inequality的例子：

Python

ineq_spy_jac = \
[[1, 1, 0, 0],
 [1, 1, 0, 0]]

ineq_spy_hess = \
[[0, 0, 0, 0],
 [0, 0, 0, 0],
 [0, 0, 0, 0],
 [0, 0, 0, 0]]

ineq = external_general_inequality(
    dim = 2,
    sparsity_jacobian=ineq_spy_jac,
    sparsity_hessian=ineq_spy_hess)
prob.inequality(ineq)

其中, external_general_inequality接口的入参如下：

• dim：该约束的维度
• sparsity_jacobian (optional): $g(v, p)$的Jacoboian的sparsity pattern, 维度为dim$\times n_{v}$的矩阵, 0代表稀疏, 1代表稠密；其默认值为全1.0
• sparsity_hessian (optional): $z^T g(v, p)$的Hessian的sparsity pattern (z为维度为dim$\times 1$的拉格朗日乘子) , 维度为$n_{v} \times n_{v}$的对称矩阵, 0代表稀疏, 1代表稠密；其默认值为全1.0

起点与终点约束定义

起点约束描述了第一个优化变量 $v_1$ 的等式约束, 比如在车辆轨迹规划问题中的车辆初始状态约束.

终点约束描述了最后一个优化变量 $v_N$ 的等式约束, 比如在火箭着陆轨迹规划问题中的末端零速度约束.

OPTIMake支持以下类型的等式约束：

• general_equality：通用类型, 通过表达式直接定义
• external_general_equality：通用类型, 通过外部C/C++函数定义

在完成约束定义后, 通过以下接口添加起点与终点约束：

Python

prob.start_equality(eq, weight_soft, penalty_type)
prob.end_equality(eq, weight_soft, penalty_type)

其中, start_equality/end_equality接口的函数入参的定义如下 (约束软化下详见 建模接口 > 约束软化) ：

• eq: 等式约束, 类型为general_equality, external_general_equality
• weight_soft: 该约束软化时的惩罚权重list, 必须为非负, 可以为常数或关于参数$p$的表达式, 默认值为全inf, 表示硬约束
• penalty_type: 该约束软化的惩罚类型list, 可选值为'none', 'quadratic', 'l1', 默认值为全'none', 表示硬约束

Note

• 当约束类型为external_general_equality时, 只能调用一次prob.start_equality/end_equality设置约束
• 当约束为其他类型时, 可调用多次prob.start_equality/end_equality添加多个约束

general_equality

下面为通过general_equality定义起点与终点约束的例子：

Python

# x0, y0, phi0为已定义的parameter
start_eq = general_equality(expr = [x - x0, y - y0, phi - phi0])
prob.start_equality(start_eq)

# xN, yN, phiN为已定义的parameter
end_eq = general_equality(expr = [x - xN, y - yN, phi - phiN])
prob.end_equality(end_eq)

其中, 函数入参的定义如下：

• expr: 约束的表达式的list

external_general_equality

下面为通过external_general_equality定义起点与终点约束的例子：

Python

eq_spy_jac = \
[[1, 0, 0, 0],
 [0, 1, 0, 0],
 [0, 0, 1, 0]]
eq_spy_hess = \
[[0, 0, 0, 0],
 [0, 0, 0, 0],
 [0, 0, 0, 0],
 [0, 0, 0, 0]]

start_eq = external_general_equality(
    dim = 3, 
    sparsity_jacobian=seq_spy_jac, 
    sparsity_hessian=seq_spy_hess)

end_eq = external_general_equality(
    dim = 3, 
    sparsity_jacobian=seq_spy_jac, 
    sparsity_hessian=seq_spy_hess)

其中, external_general_equality接口的入参如下：

• dim：该约束的维度
• sparsity_jacobian (optional): $f(v, p)$的Jacoboian的sparsity pattern, 维度为dim$\times n_{v}$的矩阵, 0代表稀疏, 1代表稠密；其默认值为全1.0
• sparsity_hessian (optional): $y^T f(v, p)$的Hessian的sparsity pattern (z为维度为dim$\times 1$的拉格朗日乘子) , 维度为$n_{v} \times n_{v}$的对称矩阵, 0代表稀疏, 1代表稠密；其默认值为全1.0

外部C/C++函数支持

上述章节主要介绍了如何通过符号表达式显式地定义问题, 当优化问题不能通过符号定义时 (例如约束为神经网络或者复杂的机器人动力学模型时) , 可通过外部C/C++函数直接定义.

OPTIMake通过external接口支持以下函数的C/C++定义：

• objective
• inequality
• equality
• start_equality
• end_equality

在定义外部函数时, 需要提供该函数以及其Jacobian实现. 在某些场景中 (根据option中hessian_approximation的配置), 需要提供Hessian实现:

hessian_approximation配置	外部objective是否提供Hessian	外部约束是否提供Hessian
'gauss-newton'	是	否
'exact'	是	是
'bfgs'	否	否

在使用external接口定义objective或约束为外部函数后, 代码生成功能会自动生成该external函数的C/C++模板.

以下为objective的外部C/C++函数的模板：

C

void vehicle_l(
    const Vehicle_SolverInfo *_solverinfo,
    const Vehicle_Problem *_prob,
    const double _var[4],
    const double _scaling_factor[1],
    double _l[1],
    double _jac_l[4],
    double _hess[16])
{
    if (_l != NULL) {
    /* define objective here */
    }

    if (_jac_l != NULL) {
    /* define Jacobian here */
    }

    if (_hess != NULL) {
    /* define Hessian increment here */
    }
}

其中, C/C++函数入参的定义如下：

• _solverinfo: solver info的结构体, 记录了该函数被调用的thread_id, stage以及迭代
• _prob: 存储了该问题的参数
• _var: 该stage的优化变量
• _scaling_factor: objective的scaling factor, 需要在_l, _jac_l, _hess_l上乘以该factor
• _l: objective的函数值
• _jac_l: objective的Jacobian函数值
• _hess: 将objective的Hessian通过增量方式加入 (_hess += _hess_l)

Note

1. 在计算C/C++函数中的_hess时, objective或约束函数的Hessian通过增量而不是赋值计算, 即: _hess += _hess_l

2. 优化问题中函数在求解过程中的调用顺序为: l, le, fs, fe, f, g

3. 矩阵通过列优先存储

约束软化

OPTIMake中内置了对约束软化的支持, 通过该接口能够减少内存占用与计算时间.

约束软化可分为对等式约束的软化与对不等式约束的软化, OPTIMake在以下接口中提供了约束软化支持：

• 优化变量上下限 (不等式约束)
• inequality (不等式约束)
• equality (等式约束)
• start_equality (等式约束)
• end_equality (等式约束)

软化的惩罚类型支持：

• quadratic
• l1

上面的问题定义章节中介绍了如何使用这些接口软化约束, 我们以以下的通用问题形式介绍OPTIMake中约束软化的问题设定. 需要注意的是, 下面的$p$代表slack变量而非参数.

$$\begin{equation*} \begin{split} \quad \quad &\quad \min_{v} l(v) \\ &\begin{split} \text{s.t.} \quad f(v) &= 0,\\ \quad g(v) &\geq 0 \end{split} \end{split} \end{equation*}$$

等式约束软化

当软化的惩罚类型为'quadratic'时, 软化问题的定义如下 ($w_i$ 为惩罚权重, $p$为约束软化的slack变量)：

$$\begin{equation*} \begin{split} \quad \quad &\quad \min_{v,p} l(v) + \sum_{i}\frac{1}{2}w_i p_i^2 \\ &\begin{split} \text{s.t.} \quad f(v) &= p,\\ \quad g(v) &\geq 0 \end{split} \end{split} \end{equation*}$$

当软化的惩罚类型为'l1'时, 软化问题的定义如下 ($w_i$ 为惩罚权重, $p,n$为约束软化的slack变量)：

$$\begin{equation*} \begin{split} \quad \quad &\quad \min_{v,p,n} l(v) + \sum_{i}w_i (p_i + n_i) \\ &\begin{split} \text{s.t.} \quad f(v) &= p - n,\\ \quad g(v) &\geq 0, \\ \quad p &\geq 0,\\ \quad n &\geq 0 \end{split} \end{split} \end{equation*}$$

不等式约束软化

当软化的惩罚类型为'quadratic'时, 软化问题的定义如下 ($w_i$ 为惩罚权重, $p$为约束软化的slack变量)：

$$\begin{equation*} \begin{split} \quad \quad &\quad \min_{v,p} l(v) + \sum_{i}\frac{1}{2}w_i p_i^2 \\ &\begin{split} \text{s.t.} \quad f(v) &= 0,\\ \quad g(v) + p &\geq 0 \end{split} \end{split} \end{equation*}$$

当软化的惩罚类型为'l1'时, 软化问题的定义如下 ($w_i$ 为惩罚权重, $p$为约束软化的slack变量)：

$$\begin{equation*} \begin{split} \quad \quad &\quad \min_{v,p,n} l(v) + \sum_{i}w_i p_i \\ &\begin{split} \text{s.t.} \quad f(v) &= 0,\\ \quad g(v) + p &\geq 0, \\ \quad p &\geq 0 \end{split} \end{split} \end{equation*}$$