pinocchio  3.0.0 A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
dpendulum.py
1 from pendulum import Pendulum
2 import numpy as np
3 from numpy import pi
4 import time
5
6 p = Pendulum(1)
7
8 NQ = 20 # Discretization steps for position
9 NV = 19 # Discretization steps for velocity
10 VMAX = 5 # Max velocity (v in [-vmax,vmax])
11 NU = 9 # Discretization steps for torque
12 UMAX = 5 # Max torque (u in [-umax,umax])
13 DT = 3e-1
14
15 DQ = 2*pi/NQ
16 DV = 2.0*(VMAX)/NV
17 DU = 2.0*(UMAX)/NU
18
19 # Continuous to discrete
20 def c2dq(q):
21  q = (q+pi)%(2*pi)
22  return int(round(q/DQ)) % NQ
23
24 def c2dv(v):
25  v = np.clip(v,-VMAX+1e-3,VMAX-1e-3)
26  return int(np.floor((v+VMAX)/DV))
27
28 def c2du(u):
29  u = np.clip(u,-UMAX+1e-3,UMAX-1e-3)
30  return int(np.floor((u+UMAX)/DU))
31
32 def c2d(qv):
33  '''From continuous to discrete.'''
34  return c2dq(qv[0]),c2dv(qv[1])
35
36 # Discrete to continuous
37 def d2cq(iq):
38  iq = np.clip(iq,0,NQ-1)
39  return iq*DQ - pi
40
41 def d2cv(iv):
42  iv = np.clip(iv,0,NV-1) - (NV-1)/2
43  return iv*DV
44
45 def d2cu(iu):
46  iu = np.clip(iu,0,NU-1) - (NU-1)/2
47  return iu*DU
48
49 def d2c(iqv):
50  '''From discrete to continuous'''
51  return d2cq(iqv[0]),d2cv(iqv[1])
52
53 def x2i(x): return x[0]+x[1]*NQ
54 def i2x(i): return [ i%NQ, i/NQ ]
55
56 # --- PENDULUM
57
58 class DPendulum:
59  def __init__(self):
60  self.pendulumpendulum = Pendulum(1)
61  self.pendulumpendulum.DT = DT
62  self.pendulumpendulum.NDT = 5
63
64  @property
65  def nqv(self): return [NQ,NV]
66  @property
67  def nx(self): return NQ*NV
68  @property
69  def nu(self): return NU
70  @property
71  def goal(self): return x2i(c2d([0.,0.]))
72
73  def reset(self,x=None):
74  if x is None:
75  x = [ np.random.randint(0,NQ), np.random.randint(0,NV) ]
76  else: x = i2x(x)
77  assert(len(x)==2)
78  self.xx = x
79  return x2i(self.xx)
80
81  def step(self,iu):
82  self.xx = self.dynamicsdynamics(self.xx,iu)
83  reward = 1 if x2i(self.xx)==self.goalgoal else 0
84  return x2i(self.xx),reward
85
86  def render(self):
87  q = d2cq(self.xx[0])
88  self.pendulumpendulum.display(np.array([q,]))
89  time.sleep(self.pendulumpendulum.DT)
90
91  def dynamics(self,ix,iu):
92  x = np.array(d2c (ix))
93  u = d2cu(iu)
94
95  self.xc,_ = self.pendulumpendulum.dynamics(x,u)
96  return c2d(x.T.tolist()[0])
97
98
99 '''
100 env = DPendulum()
101
102 print env.reset(x2i([14,11]))
103 hq = []
104 hv = []
105 hqc = []
106 hvc = []
107 u = 0
108 for i in range(100):
109  ix,r=env.step(u)
110  q,v = i2x(ix)
111  env.render()
112  if d2cv(v)==0.0: u = MAXU-1 if u==0 else 0
113  hq.append( d2cq(env.x[0]) )
114  hv.append( d2cv(env.x[1]) )
115  hqc.append( env.xc[0,0] )
116  hvc.append( env.xc[1,0] )
117
118 '''
119
120 '''
121
122
123 EPS = 1e-3
124 q = 0.0
125 v = -VMAX
126 hq = []
127 hv = []
128 hiq = []
129 hiv = []
130 hqa = []
131 hva = []
132
133 while q<2*pi:
134  hq.append(q)
135  iq = c2dq(q)
136  hiq.append(iq)
137  hqa.append(d2cq(iq))
138  q += EPS
139 while v<VMAX:
140  iv = c2dv(v)
141  hv.append(v)
142  hiv.append(iv)
143  hva.append(d2cv(iv))
144  v += EPS
145
146
147
148
149 '''
def dynamics(self, ix, iu)
Definition: dpendulum.py:91
def goal(self)
Definition: dpendulum.py:71