Commit 4285cd2e authored by Steve T's avatar Steve T
Browse files

problem size ok

parent bcc064c1
This diff is collapsed.
import quadprog
from numpy import array, dot, vstack, hstack, asmatrix, identity, abs
from numpy import array, dot, vstack, hstack, asmatrix, identity, abs, ones
from scipy.optimize import linprog
......@@ -63,7 +63,7 @@ def quadprog_solve_qp(P, q, G=None, h=None, C=None, d=None, verbose = False):
res = quadprog.solve_qp(qp_G, qp_a, qp_C, qp_b, meq)
return ResultData(res[0], 'opt', True, res[1])
except:
return ResultData(res[0], 'unfeasible', False, 0.)
return ResultData(-ones(P.shape[1]), 'unfeasible', False, 0.)
......
......@@ -9,7 +9,7 @@ from numpy.linalg import norm
from sl1m.planner import *
from constraints import *
from sl1m.stand_alone_scenarios.constraints import *
start = [[-3., 0.4 , 0. ], [-2.7 , 0.4, 0. ], [-2.7 , 0.1, 0. ], [-03., 0.1, 0. ], ]
......
......@@ -10,7 +10,7 @@ from sl1m.planner import *
from constraints import *
from sl1m.stand_alone_scenarios.constraints import *
floor = [ [0.16, 1., 0.], [-1.8, 1., 0.], [-1.8, -1., 0.], [0.16, -1., 0.] ]
......@@ -46,7 +46,7 @@ def gen_stair_pb():
p0 = [array([0.,0., 0.]), array([0.,0., 0.])];
res = { "p0" : p0, "c0" : None, "nphases": nphases}
phaseData = [ {"moving" : i%2, "fixed" : (i+1) % 2 , "K" : [copyKin(kinematicConstraints) for _ in range(len(surfaces[i]))], "relativeK" : [relativeConstraints[(i) % 2] for _ in range(len(surfaces[i]))], "S" : surfaces[i], "allRelativeK" : [ [[1, relativeConstraints[0]], [0, relativeConstraints[1]]] for _ in range(len(surfaces[i]))] } for i in range(nphases)]
phaseData = [ {"moving" : i%2, "fixed" : (i+1) % 2 , "K" : [copyKin(kinematicConstraints) for _ in range(len(surfaces[i]))], "relativeK" : [relativeConstraints[(i) % 2] for _ in range(len(surfaces[i]))], "S" : surfaces[i], "allRelativeK" : [ [[ (1, relativeConstraints[0])], [(0, relativeConstraints[1])]] for _ in range(len(surfaces[i]))] } for i in range(nphases)]
res ["phaseData"] = phaseData
return res
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment