Trajectory Optimization

def optimize_double_integrator(N):
  # Discrete-time approximation of the double integrator.
  dt = 0.01
  A = np.eye(2) + dt * np.mat("0 1; 0 0")
  B = dt * np.mat("0; 1")

  prog = MathematicalProgram()

  # Create decision variables
  u = prog.NewContinuousVariables(1, N-1, "u")
  x = prog.NewContinuousVariables(2, N, "x")

  # Add constraints
  x0 = [-2, 0]
  prog.AddBoundingBoxConstraint(x0, x0, x[:, 0])
  for n in range(N - 1):
    prog.AddConstraint(eq(x[:, n + 1], A.dot(x[:, n]) + B.dot(u[:, n])))
    prog.AddBoundingBoxConstraint(-1, 1, u[:, n])
    prog.AddQuadraticCost(u[0,n]**2, True)
  xf = [0, 0]
  prog.AddBoundingBoxConstraint(xf, xf, x[:, N - 1])

  result = Solve(prog)