def METHOD_NAME(self,x,u,display=False):
    '''
    Dynamic function: x,u -> xnext=f(x,y).
    Put the result in x (the initial value is destroyed). 
    Also compute the cost of making this step.
    Return x for convenience along with the cost.
    '''
    modulePi = lambda th: (th+np.pi)%(2*np.pi)-np.pi
    sumsq    = lambda x : np.sum(np.square(x))
    cost = 0.0
    q = modulePi(x[:self.nq])
    v = x[self.nq:]
    u = np.clip(np.reshape(np.array(u),[self.nu,1]),-self.umax,self.umax)
    DT = self.DT/self.NDT
    for i in range(self.NDT):
        pin.computeAllTerms(self.model,self.data,q,v)
        M   = self.data.M
        b   = self.data.nle
        #tau = u-self.Kf*v
        a   = inv(M)*(u-self.Kf*v-b)
        v    += a*DT
        q    += v*DT
        cost += (sumsq(q) + 1e-1*sumsq(v) + 1e-3*sumsq(u))*DT
        if display:
            self.display(q)
            time.sleep(1e-4)
    x[:self.nq] = modulePi(q)
    x[self.nq:] = np.clip(v,-self.vmax,self.vmax)
    return x,-cost