from specpath import acadoBinDir, acadoTxtPath
from acado_connect import AcadoConnect
acado = AcadoConnect(acadoBinDir + "connect_bicopter", datadir=acadoTxtPath)
acado.NQ = NQ
acado.NV = NV
acado.options['thetamax'] = np.pi / 2
acado.options['printlevel'] = 1
if 'shift' in acado.options: del acado.options['shift']
if env is not None:
acado.options['umax'] = "%.2f %.2f" % tuple([x for x in env.umax])
#if 'icontrol' in acado.options: del acado.options['icontrol']
acado.debug(False)
acado.iter = 80
acado.options['steps'] = 20
# --- ROLL OUT
def trajFromTraj(x0=None, x1=None, withPlot=None, **plotargs):
x0 = x0.T if x0 is not None else env.sample().T
x1 = x1.T if x1 is not None else env.sample().T
x = np.hstack([x0, x1])
X = nets.sess.run(nets.ptrajx.policy, feed_dict={nets.ptrajx.x: x})
X = np.reshape(X, [max(X.shape) / NX, NX])
U = nets.sess.run(nets.ptraju.policy, feed_dict={nets.ptraju.x: x})
U = np.reshape(U, [max(U.shape) / NU, NU])
T = nets.sess.run(nets.value.policy, feed_dict={nets.value.x: x})[0, 0]
if withPlot is not None: plt.plot(X[:, 0], X[:, 1], withPlot, **plotargs)
from specpath import acadoBinDir, acadoTxtPath
from acado_connect import AcadoConnect
acado = AcadoConnect(acadoBinDir + "connect_bicopter", datadir=acadoTxtPath)
acado.NQ = NQ
acado.NV = NV
acado.options['thetamax'] = np.pi / 2
acado.options['printlevel'] = 1
if 'shift' in acado.options: del acado.options['shift']
if env is not None:
acado.options['umax'] = "%.2f %.2f" % tuple([x for x in env.umax])
#if 'icontrol' in acado.options: del acado.options['icontrol']
acado.debug(False)
acado.iter = 80
acado.options['steps'] = 20
# --- NET+ACADO
def optNet(x0, net=trajFromU, withPlot=False, color='r', **plotargs):
X, U, T = net(x0)
if withPlot:
plt.plot(X[:, 0], X[:, 1], '--', color=color, **plotargs)
#ts = np.arange(0.,1.,1./X.shape[0])
ts = np.arange(0., X.shape[0]) / (X.shape[0] - 1)
np.savetxt(acado.stateFile('i'), np.vstack([ts, X.T]).T)
np.savetxt(acado.controlFile('i'), np.vstack([ts, U.T]).T)