title: optimal control using scipy optimize
body: <p>I am trying to solve an optimal control problem where the cost function is J = x^T Q x + u^T R u subject to the x_dot = A x + B u and bounds for x and u. I know that there are some solvers like cvxpy, yalimp etc which could do this but I would like to do it myself to get a better ideas about the coding and possible addition of some other parameters in future.
  I am attaching the code that I have written. It runs but returns the same value for all the time steps. I have stacked x and u as a single vector. I dont know if this is the right way to do it. I think the code could be written in a better/efficient way. All suggestions are welcome and really appreciate for any help in advance</p>
  
  <p>Ash</p>
  
  <pre><code>import numpy as np
  import sympy as sp
  import scipy.optimize as opt
  import matplotlib.pyplot as plt
  
  # Optimal Control Problem
  # Cost, J = x.transpose() * Q * x + u.transpose() * R * u
  # x_dot = A*x + B*u
  # x_min &lt; x &lt; x_max
  # u_min &lt; x &lt; u_max
  
  
  class mpc_opt():
  
      def __init__(self):
          self.Q = sp.diag(0.5, 1, 0)  # state penalty matrix, Q
          self.R = sp.eye(2) # input penalty matrix
          self.A = sp.Matrix([[-0.79, -0.3, -0.1],[0.5, 0.82, 1.23], [0.52, -0.3, -0.5]])  # state matrix 
          self.B = sp.Matrix([[-2.04, -0.21], [-1.28, 2.75], [0.29, -1.41]])  # input matrix
  
          self.t = np.linspace(0, 1, 30)
  
  
      # reference trajectory  ## static!!!
      def ref_trajectory(self, i):  # y = 3*sin(2*pi*omega*t)
          # y = 3 * np.sin(2*np.pi*self.omega*self.t[i])
          x_ref = sp.Matrix([0, 1, 0])
          return x_ref
          # return sp.Matrix(([[self.t[i]], [y], [0]]))
  
      def cost_function(self, U, *args):
          t = args
          nx, nu = self.A.shape[-1], self.B.shape[-1]
          x0 = U[0:nx]
          u = U[nx:nx+nu]
          u = u.reshape(len(u), -1)
          x0 = x0.reshape(len(x0), -1)
          x1 = self.A * x0 + self.B * u
          # q = [x1[0], x1[1]]
          # pos = self.end_effec_pose(q)
          traj_ref = self.ref_trajectory(t)
          pos_error = x1 - traj_ref
          cost = pos_error.transpose() * self.Q * pos_error + u.transpose() * self.R * u
          return cost
  
      def cost_gradient(self, U, *args):
          t = args
          nx, nu = self.A.shape[-1], self.B.shape[-1]
          x0 = U[0:nx]
          u = U[nx:nx + nu]
          u = u.reshape(len(u), -1)
          x0 = x0.reshape(len(x0), -1)
          x1 = self.A * x0 + self.B * u
          traj_ref = self.ref_trajectory(t)
          pos_error = x1 - traj_ref
          temp1 = self.Q * pos_error
          cost_gradient = temp1.col_join(self.R * u)
          return cost_gradient
  
  
      def optimise(self, u0, t):
          umin = [-2., -3.]
          umax = [2., 3.]
          xmin = [-10., -9., -8.]
          xmax = [10., 9., 8.]
          bounds = ((xmin[0], xmax[0]), (xmin[1], xmax[1]), (xmin[2], xmax[2]), (umin[0], umax[0]), (umin[1], umax[1]))
  
          U = opt.minimize(self.cost_function, u0, args=(t), method='SLSQP', bounds=bounds, jac=self.cost_gradient,
                           options={'maxiter': 200, 'disp': True})
          U = U.x
          return U
  
  
  if __name__ == '__main__':
      mpc = mpc_opt()
      x0, u0, = sp.Matrix([[0.1], [0.02], [0.05]]), sp.Matrix([[0.4], [0.2]])
      X, U = sp.zeros(len(x0), len(mpc.t)), sp.zeros(len(u0), len(mpc.t))
      U0 = sp.Matrix([x0, u0])
      nx, nu = mpc.A.shape[-1], mpc.B.shape[-1]
      for i in range(len(mpc.t)):
          print('i = :', i)
          result = mpc.optimise(U0, i)
          x0 = result[0:nx]
          u = result[nx:nx + nu]
          u = u.reshape(len(u), -1)
          x0 = x0.reshape(len(x0), -1)
          U[:, i], X[:, i] = u0, x0
          # x0 = mpc.A * x0 + mpc.B * u
          U0 = result
  
  plt.plot(X[0, :], '--r')
  plt.plot(X[1, :], '--b')
  plt.plot(X[2, :], '*r')
  plt.show()
  </code></pre>