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