variable_stepsize_custom_constraints.py 文件源码

python
阅读 19 收藏 0 点赞 0 评论 0

项目:toppra 作者: hungpham2511 项目源码 文件源码
def evaluate_constraint(expressions, ss):
    """Evaluate the canonical constraints `expressions` over the
    discretized interval `ss`.

    Parameters
    ----------

    Returns
    -------

    """
    m = len(expressions)
    N = len(ss) - 1
    a = np.zeros((N + 1, m))
    b = np.zeros((N + 1, m))
    c = np.zeros((N + 1, m))
    for index, e in enumerate(expressions):
        if e.rel_op == "<=":
            canon_form = e.lhs - e.rhs
        elif e.rel_op == ">=":
            canon_form = e.rhs - e.lhs
        else:
            raise ValueError("Relation Operation need to be `<=` or `>=`.")

        f = sympy.lambdify(sympy.symbols('u, x, s'), canon_form)
        for i in xrange(N + 1):
            a[i, index] = f(1, 0, ss[i]) - f(0, 0, ss[i])
            b[i, index] = f(0, 1, ss[i]) - f(0, 0, ss[i])
            c[i, index] = f(0, 0, ss[i])
    return a, b, c


# Define symbolic expression and evaluation
评论列表
文章目录


问题


面经


文章

微信
公众号

扫码关注公众号