Solution, exercise 1

Dateiinhalt

# Wrapper
import numpy as NP
import casadi as C
def qpsolve(H,g,lbx,ubx,A=NP.zeros((0,0)),lba=NP.zeros(0),uba=NP.zeros(0)):
  # Convert to CasADi types
  H = C.DMatrix(H)
  g = C.DMatrix(g)
  lbx = C.DMatrix(lbx)
  ubx = C.DMatrix(ubx)
  A = C.DMatrix(A)
  A = A.reshape((A.size1(),H.size1())) # Make sure matching dimensions
  lba = C.DMatrix(lba)
  uba = C.DMatrix(uba)
  
  # QP structure
  qp = C.qpStruct(h=H.sparsity(),a=A.sparsity())
  
  # Create CasADi solver instance
  if False:
    solver = C.QpSolver("qpoases",qp)
  else:
    solver = C.QpSolver("nlp",qp)
    solver.setOption("nlp_solver","ipopt")
  
  # Initialize the solver
  solver.init()
  
  # Pass problem data
  solver.setInput(H,"h")
  solver.setInput(g,"g")
  solver.setInput(A,"a")
  solver.setInput(lbx,"lbx")
  solver.setInput(ubx,"ubx")
  solver.setInput(lba,"lba")
  solver.setInput(uba,"uba")
  
  # Solver the QP
  solver.evaluate()
  
  # Return the solution
  return NP.array(solver.getOutput("x"))

from matplotlib import pylab as plt

DEBUG=False

N = 40
mi = 40.0/N
Di = 70.0*N
g0 = 9.81

# hessian
H = NP.zeros((2*N, 2*N))
for k in range(0,2*N-2):
    H[k+2, k  ] = -1.0
    H[k  , k+2] = -1.0
for k in range(0,2*N):
    H[k, k] = 2.0
H[0,0] = H[1,1] = H[-2,-2] = H[-1,-1] = 1.0
H = Di*H

if DEBUG:
    print "hessian:"
    print H

# g
g = NP.zeros((2*N))
g[1::2] = g0 * mi

if DEBUG:
    print "g:"
    print g

# box constraints
lbx = -NP.inf*NP.ones((2*N))
ubx =  NP.inf*NP.ones((2*N))
lbx[ 0] = ubx[ 0] = -2
lbx[ 1] = ubx[ 1] =  1
lbx[-2] = ubx[-2] =  2
lbx[-1] = ubx[-1] =  1

if DEBUG:
    print "lower bound:"
    print lbx
    print "upper bound:"
    print ubx

xopt0 = qpsolve(H,g,lbx,ubx)
Y0 = xopt0[0::2]
Z0 = xopt0[1::2]


# introduce ground constraints
lbx[3:-2:2] = 0.5
xopt1 = qpsolve(H,g,lbx,ubx)
Y1 = xopt1[0::2]
Z1 = xopt1[1::2]
if DEBUG:
    print "new lower bound:"
    print lbx
    print "new upper bound:"
    print ubx


# introduce slanted ground constraints
lbA = 0.5*NP.ones((N,1))
ubA = NP.inf*NP.ones((N,1))
A = NP.zeros((N,2*N))
for k in range(N):
    A[k,2*k  ] = -0.1
    A[k,2*k+1] =  1.0

if DEBUG:
    print "A:"
    print A
    print "lbA:"
    print lbA
    print "ubA:"
    print ubA

xopt2 = qpsolve(H,g,lbx,ubx,A,lbA,ubA)
Y2 = xopt2[0::2]
Z2 = xopt2[1::2]


plt.plot(Y0,Z0,'o-')
plt.plot(Y1,Z1,'ro-')
plt.plot(Y2,Z2,'go-')
ys = NP.linspace(-2,2,100)
zs = 0.5 + 0.1*ys
plt.plot(ys,zs)

plt.xlabel('y [m]')
plt.ylabel('z [m]')
plt.title('hanging chain QP')
plt.grid(True)
plt.legend(['original','z >= 0.5', 'z >= 0.5, z - 0.1y >= 0.5'],loc=9)
#savefig("test.png")
plt.show()