Commit 7331bce4 by stevet

### removed qp dependency

parent fbe8cb60
 from __future__ import print_function import quadprog from numpy import array, dot, hstack, vstack from scipy.optimize import linprog def quadprog_solve_qp(P, q, G=None, h=None, C=None, d=None, verbose=False): """ min (1/2)x' P x + q' x subject to G x <= h subject to C x = d """ # qp_G = .5 * (P + P.T) # make sure P is symmetric qp_G = .5 * (P + P.T) # make sure P is symmetric qp_a = -q qp_C = None qp_b = None meq = 0 if C is not None: if G is not None: qp_C = -vstack([C, G]).T qp_b = -hstack([d, h]) else: qp_C = -C.transpose() qp_b = -d meq = C.shape[0] elif G is not None: # no equality constraint qp_C = -G.T qp_b = -h res = quadprog.solve_qp(qp_G, qp_a, qp_C, qp_b, meq) if verbose: return res # print('qp status ', res) return res[0] def to_least_square(A, b): "least square form of ||Ax-b||**2" return dot(A.T, A), -dot(A.T, b) def solve_least_square(A, b, G=None, h=None, C=None, d=None): """ min ||Ax-b||**2 subject to G x <= h subject to C x = d """ P, q = to_least_square(A, b) # P = dot(A.T, A) # q = -dot(A.T, b).reshape(b.shape[0]) return quadprog_solve_qp(P, q, G, h, C, d) def solve_lp(q, G=None, h=None, C=None, d=None): """ min q' x subject to G x <= h subject to C x = d """ res = linprog(q, A_ub=G, b_ub=h, A_eq=C, b_eq=d, bounds=[(-100000., 10000.) for _ in range(q.shape[0])], method='interior-point', callback=None, options={'presolve': True}) return res if __name__ == '__main__': A = array([[1., 2., 0.], [-8., 3., 2.], [0., 1., 1.]]) b = array([3., 2., 3.]) P = dot(A.T, A) q = 2 * dot(b, A).reshape((3, )) G = array([[1., 2., 1.], [2., 0., 1.], [-1., 2., -1.]]) h = array([3., 2., -2.]).reshape((3, )) res2 = solve_least_square(A, b, G, h) res1 = quadprog_solve_qp(P, q, G, h) print(res1) print(res2)
 ... @@ -238,7 +238,37 @@ ... @@ -238,7 +238,37 @@ "metadata": {}, "metadata": {}, "outputs": [], "outputs": [], "source": [ "source": [ "from qp import quadprog_solve_qp\n", "import quadprog\n", "from numpy import array, hstack, vstack\n", "\n", "def quadprog_solve_qp(P, q, G=None, h=None, C=None, d=None, verbose=False):\n", " \"\"\"\n", " min (1/2)x' P x + q' x\n", " subject to G x <= h\n", " subject to C x = d\n", " \"\"\"\n", " # qp_G = .5 * (P + P.T) # make sure P is symmetric\n", " qp_G = .5 * (P + P.T) # make sure P is symmetric\n", " qp_a = -q\n", " qp_C = None\n", " qp_b = None\n", " meq = 0\n", " if C is not None:\n", " if G is not None:\n", " qp_C = -vstack([C, G]).T\n", " qp_b = -hstack([d, h])\n", " else:\n", " qp_C = -C.transpose()\n", " qp_b = -d\n", " meq = C.shape[0]\n", " elif G is not None: # no equality constraint\n", " qp_C = -G.T\n", " qp_b = -h\n", " res = quadprog.solve_qp(qp_G, qp_a, qp_C, qp_b, meq)\n", " if verbose:\n", " return res\n", " # print('qp status ', res)\n", " return res[0]\n", "\n", "\n", "res = quadprog_solve_qp(A, b)" "res = quadprog_solve_qp(A, b)" ] ] ... @@ -683,9 +713,21 @@ ... @@ -683,9 +713,21 @@ }, }, { { "cell_type": "code", "cell_type": "code", "execution_count": null, "execution_count": 21, "metadata": {}, "metadata": {}, "outputs": [], "outputs": [ { "ename": "TypeError", "evalue": "No to_python (by-value) converter found for C++ type: curves::piecewise_curve, curves::linear_variable >", "output_type": "error", "traceback": [ "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", "\u001b[0;31mTypeError\u001b[0m Traceback (most recent call last)", "\u001b[0;32m\u001b[0m in \u001b[0;36m\u001b[0;34m()\u001b[0m\n\u001b[1;32m 1\u001b[0m \u001b[0;31m#first, split the variable curve\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m----> 2\u001b[0;31m \u001b[0mpiecewiseCurve\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mvariableBezier\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0msplit\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0marray\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m0.4\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;36m0.8\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mT\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m 3\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 4\u001b[0m \u001b[0mconstrainedCurve\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mpiecewiseCurve\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mcurve_at_index\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;36m1\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 5\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n", "\u001b[0;31mTypeError\u001b[0m: No to_python (by-value) converter found for C++ type: curves::piecewise_curve, curves::linear_variable >" ] } ], "source": [ "source": [ "#first, split the variable curve\n", "#first, split the variable curve\n", "piecewiseCurve = variableBezier.split(array([[0.4, 0.8]]).T)\n", "piecewiseCurve = variableBezier.split(array([[0.4, 0.8]]).T)\n", ... ...