dx = x2 - x1
dq = (dx[:self.NQ] + PI) % PPI - PI
dv = dx[self.NQ:]
return np.concatenate([dq, dv])
env = Pendulum(2, length=.5, mass=3.0, armature=.2, withDisplay=True)
env.withSinCos = False # State is dim-3: (cosq,sinq,qdot) ...
env.vmax = 100.
env.Kf = np.diagflat([0.2, 2.])
env.modulo = False
env.DT = 0.15
env.NDT = 1
env.umax = np.matrix([5., 10.]).T
env.qlow[1] = -np.pi
env.qup = np.matrix([2 * np.pi, np.pi]).T
env.qlow = -env.qup
env.vup = np.matrix([
3,
] * 2).T
env.vlow = -env.vup
env.xmax = np.matrix([3 * np.pi, np.pi, 8, 8]).T
env.xmin = -env.xmax
acado = AcadoConnect(acadoBinDir + "connect_double_pendulum",
datadir=acadoTxtPath)
config(acado, 'connect', env)
acado.setDims(env.nq, env.nv)
def doplot():
plt.clf()
for idx, i in enumerate([2, 10, 50]): #NACADO_ITER:
#plt.figure()
plt.subplot(1, 3, idx + 1)
treatCheckData(results[i]['extra'],
'After %d OCP iteration' % i) #'Refine in %d iter'%i)
plt.legend(
['Ground truth', 'Policy approx', 'Trajectory approx', 'Cold start'],
loc=4)
plt.subplot(1, 3, 1)
plt.ylabel('Cost')
saveCurrentFigure('warmstart')
'''
add_tx = lambda c,tx: CheckData(x0=c.x0,idx=c.idx,t0=c.t0,tu=c.tu,tj=c.tj,tx=tx)
acado.setDims(3,3)
for niter in [50]:# 2, 10, 50]:
acado.iter = niter
for i,c in enumerate(results[niter]['extra']):
print niter,c.x0.T
try: success = acado.run(c.x0,zero(6))
except: success = False
if success:
tx = acado.cost()
if tx<c.t0: tx=np.inf
results[niter]['extra'][i] = c._replace(tx=tx)
else:
results[niter]['extra'][i] = c._replace(tx=np.inf)