pinocchio  3.1.0 A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
pendulum.py
1 '''
2 Create a simulation environment for a N-pendulum.
3 Example of use:
4
5 env = Pendulum(N)
6 env.reset()
7
8 for i in range(1000):
9  env.step(zero(env.nu))
10  env.render()
11
12 '''
13
14
15 from pinocchio.utils import *
16 from pinocchio.explog import exp,log
17 from numpy.linalg import pinv,norm
18 import pinocchio as pin
19 import gepetto.corbaserver
20 from display import Display
21 from numpy.linalg import pinv,norm,inv
22 import time
23
24
25 class Visual:
26  '''
27  Class representing one 3D mesh of the robot, to be attached to a joint. The class contains:
28  * the name of the 3D objects inside Gepetto viewer.
29  * the ID of the joint in the kinematic tree to which the body is attached.
30  * the placement of the body with respect to the joint frame.
31  This class is only used in the list Robot.visuals (see below).
32  '''
33  def __init__(self,name,jointParent,placement):
34  self.namename = name # Name in gepetto viewer
35  self.jointParentjointParent = jointParent # ID (int) of the joint
36  self.placementplacement = placement # placement of the body wrt joint, i.e. bodyMjoint
37  def place(self,display,oMjoint):
38  oMbody = oMjoint*self.placementplacement
39  display.place(self.namename,oMbody,False)
40
41 class Pendulum:
42  '''
43  Define a class Robot with 7DOF (shoulder=3 + elbow=1 + wrist=3).
44  The configuration is nq=7. The velocity is the same.
45  The members of the class are:
46  * viewer: a display encapsulating a gepetto viewer client to create 3D objects and place them.
47  * model: the kinematic tree of the robot.
48  * data: the temporary variables to be used by the kinematic algorithms.
49  * visuals: the list of all the 'visual' 3D objects to render the robot, each element of the list being
50  an object Visual (see above).
51
52  See tp1.py for an example of use.
53  '''
54
55  def __init__(self,nbJoint=1):
56  '''Create a Pinocchio model of a N-pendulum, with N the argument <nbJoint>.'''
57  self.viewerviewer = Display()
58  self.visualsvisuals = []
59  self.modelmodel = pin.Model()
60  self.createPendulumcreatePendulum(nbJoint)
62
63  self.q0q0 = zero(self.modelmodel.nq)
64
65  self.DTDT = 5e-2 # Step length
66  self.NDTNDT = 2 # Number of Euler steps per integration (internal)
67  self.KfKf = .10 # Friction coefficient
68  self.vmaxvmax = 8.0 # Max velocity (clipped if larger)
69  self.umaxumax = 2.0 # Max torque (clipped if larger)
70  self.withSinCoswithSinCos = False # If true, state is [cos(q),sin(q),qdot], else [q,qdot]
71
72  def createPendulum(self,nbJoint,rootId=0,prefix='',jointPlacement=None):
73  color = [red,green,blue,transparency] = [1,1,0.78,1.0]
74  colorred = [1.0,0.0,0.0,1.0]
75
76  jointId = rootId
77  jointPlacement = jointPlacement if jointPlacement!=None else pin.SE3.Identity()
78  length = 1.0
79  mass = length
80  inertia = pin.Inertia(mass,
81  np.array([0.0,0.0,length/2]),
82  mass/5*np.diagflat([ 1e-2,length**2, 1e-2 ]) )
83
84  for i in range(nbJoint):
85  istr = str(i)
86  name = prefix+"joint"+istr
87  jointName,bodyName = [name+"_joint",name+"_body"]
89  self.modelmodel.appendBodyToJoint(jointId,inertia,pin.SE3.Identity())
91  except: pass
92  self.visualsvisuals.append( Visual('world/'+prefix+'sphere'+istr,jointId,pin.SE3.Identity()) )
94  except:pass
95  self.visualsvisuals.append( Visual('world/'+prefix+'arm'+istr,jointId,
96  pin.SE3(eye(3),np.array([0.,0.,length/2]))))
97  jointPlacement = pin.SE3(eye(3),np.array([0.0,0.0,length]))
98
100
101  def display(self,q):
103  for visual in self.visualsvisuals:
105  self.viewerviewer.viewer.gui.refresh()
106
107
108  @property
109  def nq(self): return self.model.nq
110  @property
111  def nv(self): return self.model.nv
112  @property
113  def nx(self): return self.nq+self.nvnv
114  @property
115  def nobs(self): return self.nx+self.withSinCoswithSinCos
116  @property
117  def nu(self): return self.nv
118
119  def reset(self,x0=None):
120  if x0 is None:
121  q0 = np.pi*(rand(self.nqnq)*2-1)
122  v0 = rand(self.nvnv)*2-1
123  x0 = np.vstack([q0,v0])
124  assert len(x0)==self.nxnx
125  self.xx = x0.copy()
126  self.rr = 0.0
127  return self.obsobs(self.xx)
128
129  def step(self,u):
130  assert(len(u)==self.nunu)
131  _,self.rr = self.dynamicsdynamics(self.xx,u)
132  return self.obsobs(self.xx),self.rr
133
134  def obs(self,x):
135  if self.withSinCoswithSinCos:
136  return np.vstack([ np.vstack([np.cos(qi),np.sin(qi)]) for qi in x[:self.nqnq] ]
137  + [x[self.nqnq:]],)
138  else: return x.copy()
139
140  def tip(self,q):
141  '''Return the altitude of pendulum tip'''
144
145  def dynamics(self,x,u,display=False):
146  '''
147  Dynamic function: x,u -> xnext=f(x,y).
148  Put the result in x (the initial value is destroyed).
149  Also compute the cost of making this step.
150  Return x for convenience along with the cost.
151  '''
152
153  modulePi = lambda th: (th+np.pi)%(2*np.pi)-np.pi
154  sumsq = lambda x : np.sum(np.square(x))
155
156  cost = 0.0
157  q = modulePi(x[:self.nqnq])
158  v = x[self.nqnq:]
159  u = np.clip(np.reshape(np.array(u),[self.nunu,1]),-self.umaxumax,self.umaxumax)
160
161
162  DT = self.DTDT/self.NDTNDT
163  for i in range(self.NDTNDT):
167  #tau = u-self.Kf*v
168  a = inv(M)*(u-self.KfKf*v-b)
169
170  v += a*DT
171  q += v*DT
172  cost += (sumsq(q) + 1e-1*sumsq(v) + 1e-3*sumsq(u))*DT
173
174  if display:
175  self.displaydisplay(q)
176  time.sleep(1e-4)
177
178  x[:self.nqnq] = modulePi(q)
179  x[self.nqnq:] = np.clip(v,-self.vmaxvmax,self.vmaxvmax)
180
181  return x,-cost
182
183  def render(self):
184  q = self.xx[:self.nqnq]
185  self.displaydisplay(q)
186  time.sleep(self.DTDT/10)
def obs(self, x)
Definition: pendulum.py:134
def createPendulum(self, nbJoint, rootId=0, prefix='', jointPlacement=None)
Definition: pendulum.py:72
def __init__(self, nbJoint=1)
Definition: pendulum.py:55
def nx(self)
Definition: pendulum.py:113
def display(self, q)
Definition: pendulum.py:101
def dynamics(self, x, u, display=False)
Definition: pendulum.py:145
def nv(self)
Definition: pendulum.py:111
def nq(self)
Definition: pendulum.py:109
def tip(self, q)
Definition: pendulum.py:140
def nu(self)
Definition: pendulum.py:117