parameters: (self,x,u,display=False)
return_statement: return x,-cost
docstring: 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.
docstring_summary: 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.
docstring_tokens: 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 .
function: def dynamics(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.matrix(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
function_tokens: def dynamics ( self , x , u , display = False ) : 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 . matrix ( 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