def __init__(self):
acado = self.acado = AcadoConnect(model=env.model)
acado.setTimeInterval(1.0)
acado.options['steps'] = 10
acado.options['shift'] = 0
acado.options['iter'] = 20
acado.options['friction'] = 0.2
acado.setTimeInterval(1.0)
self.threshold = 1e-3
def __init__(self):
acado = self.acado = AcadoConnect(acadoPath,
model=env.model,datadir=acadoTxtPath)
acado.setTimeInterval(1.0)
acado.options['steps'] = 25
acado.options['shift'] = 0
acado.options['iter'] = 20
acado.options['friction'] = \
"{0:f} {1:f}".format([env.Kf,]*2) if isinstance(env.Kf,float) \
else "{0:f} {1:f}".format(*env.Kf.diagonal())
acado.options['umax'] = "%.2f %.2f" % tuple([x for x in env.umax])
acado.options['armature'] = env.armature
acado.setTimeInterval(1.5)
self.threshold = 1e-3
self.acado.setup_async()
self.idxBest = None # Store the index of the best previous trial
for hi, ci in zip(hist[-1], colors):
plt.plot(len(hist), hi, ci)
plt.draw()
if track: return hist
# --- OPTIMIZE -----------------------------------------------------
# --- OPTIMIZE -----------------------------------------------------
# --- OPTIMIZE -----------------------------------------------------
# --- SETUP ACADO
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
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 states(self, jobid=None):
X = AcadoConnect.states(self, jobid)
return X[:, [0, 1, 2, 3, 4, 6, 7, 8, 9, 10]]
def __init__(self, *args, **kwargs):
AcadoConnect.__init__(self, *args, **kwargs)
X13 = zero([X.shape[0], self.NQ + self.NV + 3])
X13[:, :self.NQ] = X[:, :self.NQ]
X13[:, self.NQ + 1:self.NQ + 1 + self.NV] = X[:, self.NQ:self.NQ +
self.NV]
np.savetxt(self.stateFile('i', jobid),
np.vstack([T / T[-1], X.T]).T)
if U is not None and 'icontrol' in self.options:
np.savetxt(self.controlFile('i', jobid),
np.vstack([T / T[-1], U.T]).T)
return X, U, T
# def run(self,x0,x1,*args,**kwargs):
# jobid = self.run_async(x0,x1,*args,**kwargs)
# try:
# return self.join(jobid,x0,x1,timeout = 10)
# except:
# return False
dataRootPath = 'data/planner/quadcopterobs'
env = Quadcopter(withDisplay=False)
env.sphericalObstacle = True
env.obstacleSize = 1.
env.obstaclePosition = np.matrix([2., 2., 0.]).T
#acado = AcadoQuadConnect(acadoBinDir + "connect_quadcopter",
acado = AcadoConnect(acadoBinDir + "connect_quadcopter", datadir=acadoTxtPath)
config(acado, 'connect', env)
acado.setDims(env.nq, env.nv)
return False
def states(self):
return self.acado.states()
def controls(self):
return self.acado.controls()
def cost(self):
return self.acado.opttime()
def times(self):
return self.acado.times()
def time(self):
return self.acado.opttime()
class BicopterStateDiff:
'''Given two states (of the bicopter) returns the delta (Lie exponential)
to go from x1 to x2.'''
def __call__(self, x1, x2):
return x2 - x1
# ---
env = Bicopter(withDisplay=False)
acado = AcadoConnect(acadoBinDir + "connect_bicopter", datadir=acadoTxtPath)
config(acado, 'connect', env)
NX = env.nobs # ... training converges with q,qdot with 2x more neurones.
NU = env.nu # Control is dim-1: joint torque
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 [1] = +np.pi
acado = AcadoConnect(acadoPath,
model=env.model,
datadir=acadoTxtPath)
config(acado,'connect',env)
# --- PRM ---
# --- PRM ---
# --- PRM ---
prm = PRM(GraphPendulumDouble(),
sampler = env.reset,
checker = lambda x:True,
nearestNeighbor = NearestNeighbor(DistanceSO3([1,.1])),
connect = ConnectAcado(acado))
if LOAD_PRM:
prm.graph.load(dataRootPath)