ocp.py
Go to the documentation of this file.
1 '''
2 Example of optimal control resolution by direct optimization of a single trajectory.
3 '''
4 
5 from pendulum import Pendulum
6 from scipy.optimize import *
7 from pinocchio.utils import *
8 import pinocchio as pin
9 from numpy.linalg import norm
10 import time
11 import signal
12 import matplotlib.pyplot as plt
13 
14 env = Pendulum(1)
15 NSTEPS = 50
16 x0 = env.reset().copy()
17 
18 def cost(U):
19  '''Cost for a trajectory starting at state X0 with control U'''
20  env.reset(x0)
21  csum = 0.0
22  for t in range(NSTEPS):
23  u = U[env.nu*t:env.nu*(t+1)] # Control at time step <t>
24  _,r = env.step(u) # Pendulum step, with reward r
25  csum += r # Cumulative sum
26  return -csum # Returns cost ie negative reward
27 
28 def display(U,verbose=False):
29  '''Display the trajectory on Gepetto viewer.'''
30  x = x0.copy()
31  if verbose: print "U = ", " ".join(map(lambda u:'%.1f'%u,np.asarray(U).flatten()))
32  for i in range(len(U)/env.nu):
33  env.dynamics(x,U[env.nu*i:env.nu*(i+1)],True)
34  env.display(x)
35  time.sleep(5e-2)
36  if verbose: print "X%d"%i,x.T
37 
38 class CallBack:
39  '''Call back function used to follow optimizer steps.'''
40  def __init__(self):
41  self.iter = 0
42  self.withdisplay = False
43  self.h_rwd = []
44  def __call__(self,U):
45  print 'Iteration ',self.iter, " ".join(map(lambda u:'%.1f'%u,np.asarray(U).flatten()))
46  self.iter += 1
47  self.U = U.copy()
48  self.h_rwd.append(cost(U))
49  if self.withdisplay: r = display(U) # Display if CTRL-Z has been pressed.
50  def setWithDisplay(self,boolean = None):
51  self.withdisplay = not self.withdisplay if boolean is None else boolean
52 
53 callback = CallBack()
54 signal.signal(signal.SIGTSTP, lambda x,y:callback.setWithDisplay())
55 
56 ### --- OCP resolution
57 U0 = zero(NSTEPS*env.nu)-env.umax # Initial guess for the control trajectory.
58 bounds = [ [-env.umax,env.umax], ]*env.nu*NSTEPS # Set control bounds to environment umax.
59 
60 # Start BFGS optimization routine
61 U,c,info = fmin_l_bfgs_b(cost,x0=U0,callback=callback,
62  approx_grad=True,bounds=bounds)
63 
64 # When done, display the trajectory in Gepetto-viewer
65 display(U,True)
66 
67 plt.plot(callback.h_rwd)
68 plt.show()
69 
withdisplay
Definition: ocp.py:42
def __init__(self)
Definition: ocp.py:40
def __call__(self, U)
Definition: ocp.py:44
def display(U, verbose=False)
Definition: ocp.py:28
def setWithDisplay(self, boolean=None)
Definition: ocp.py:50
void copy(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &origin, DataTpl< Scalar, Options, JointCollectionTpl > &dest, KinematicLevel kinematic_level)
Copy part of the data from origin to dest. Template parameter can be used to select at which differen...
Definition: copy.hpp:52
cost
Definition: ocp.py:61


pinocchio
Author(s):
autogenerated on Tue Jun 1 2021 02:45:04