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