def _setupDynamicsConstraints(self,endTime,traj):
# Todo: add parallelization
# Todo: get endTime right
g = []
nicp = 1
deg = 4
p = self._dvMap.pVec()
for k in range(self.nk):
newton = Newton(LagrangePoly,self.dae,1,nicp,deg,'RADAU')
newton.setupStuff(endTime)
X0_i = self._dvMap.xVec(k)
U_i = self._U[k,:].T
# guess
if traj is None:
newton.isolver.setOutput(1,0)
else:
X = C.DMatrix([[traj.lookup(name,timestep=k,degIdx=j) for j in range(1,traj.dvMap._deg+1)] \
for name in traj.dvMap._xNames])
Z = C.DMatrix([[traj.lookup(name,timestep=k,degIdx=j) for j in range(1,traj.dvMap._deg+1)] \
for name in traj.dvMap._zNames])
newton.isolver.setOutput(C.veccat([X,Z]),0)
_, Xf_i = newton.isolver.call([X0_i,U_i,p])
X0_i_plus = self._dvMap.xVec(k+1)
g.append(Xf_i-X0_i_plus)
return g
def _setupDynamicsConstraints(self, endTime, traj):
# Todo: add parallelization
# Todo: get endTime right
g = []
nicp = 1
deg = 4
p = self._dvMap.pVec()
for k in range(self.nk):
newton = Newton(LagrangePoly, self.dae, 1, nicp, deg, 'RADAU')
newton.setupStuff(endTime)
X0_i = self._dvMap.xVec(k)
U_i = self._U[k, :].T
# guess
if traj is None:
newton.isolver.setOutput(1, 0)
else:
X = C.DMatrix([[traj.lookup(name,timestep=k,degIdx=j) for j in range(1,traj.dvMap._deg+1)] \
for name in traj.dvMap._xNames])
Z = C.DMatrix([[traj.lookup(name,timestep=k,degIdx=j) for j in range(1,traj.dvMap._deg+1)] \
for name in traj.dvMap._zNames])
newton.isolver.setOutput(C.veccat([X, Z]), 0)
_, Xf_i = newton.isolver.call([X0_i, U_i, p])
X0_i_plus = self._dvMap.xVec(k + 1)
g.append(Xf_i - X0_i_plus)
return g
def _setupDynamicsConstraints(self):
# Todo: add parallelization
# Todo: add initialization
g = []
nicp = 10
deg = 4
p = self._dvMap.pVec()
for k in range(self.nk):
newton = Newton(LagrangePoly,self.dae,1,nicp,deg,'RADAU')
endTime = 0.05
newton.setupStuff(endTime)
X0_i = self._dvMap.xVec(k)
U_i = self._dvMap.uVec(k)
_, Xf_i = newton.isolver.call([X0_i,U_i,p])
X0_i_plus = self._dvMap.xVec(k+1)
g.append(Xf_i-X0_i_plus)
return g
def _setupDynamicsConstraints(self):
# Todo: add parallelization
# Todo: add initialization
g = []
nicp = 10
deg = 4
p = self._dvMap.pVec()
for k in range(self.nk):
newton = Newton(LagrangePoly, self.dae, 1, nicp, deg, 'RADAU')
endTime = 0.05
newton.setupStuff(endTime)
X0_i = self._dvMap.xVec(k)
U_i = self._dvMap.uVec(k)
_, Xf_i = newton.isolver.call([X0_i, U_i, p])
X0_i_plus = self._dvMap.xVec(k + 1)
g.append(Xf_i - X0_i_plus)
return g
