pendulum.py
Go to the documentation of this file.
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.name = name # Name in gepetto viewer
35  self.jointParent = jointParent # ID (int) of the joint
36  self.placement = placement # placement of the body wrt joint, i.e. bodyMjoint
37  def place(self,display,oMjoint):
38  oMbody = oMjoint*self.placement
39  display.place(self.name,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.viewer = Display()
58  self.visuals = []
59  self.model = pin.Model()
60  self.createPendulum(nbJoint)
61  self.data = self.model.createData()
62 
63  self.q0 = zero(self.model.nq)
64 
65  self.DT = 5e-2 # Step length
66  self.NDT = 2 # Number of Euler steps per integration (internal)
67  self.Kf = .10 # Friction coefficient
68  self.vmax = 8.0 # Max velocity (clipped if larger)
69  self.umax = 2.0 # Max torque (clipped if larger)
70  self.withSinCos = 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.matrix([0.0,0.0,length/2]).T,
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"]
88  jointId = self.model.addJoint(jointId,pin.JointModelRY(),jointPlacement,jointName)
89  self.model.appendBodyToJoint(jointId,inertia,pin.SE3.Identity())
90  try:self.viewer.viewer.gui.addSphere('world/'+prefix+'sphere'+istr, 0.15,colorred)
91  except: pass
92  self.visuals.append( Visual('world/'+prefix+'sphere'+istr,jointId,pin.SE3.Identity()) )
93  try:self.viewer.viewer.gui.addCapsule('world/'+prefix+'arm'+istr, .1,.8*length,color)
94  except:pass
95  self.visuals.append( Visual('world/'+prefix+'arm'+istr,jointId,
96  pin.SE3(eye(3),np.matrix([0.,0.,length/2]))))
97  jointPlacement = pin.SE3(eye(3),np.matrix([0.0,0.0,length]).T)
98 
99  self.model.addFrame( pin.Frame('tip',jointId,0,jointPlacement,pin.FrameType.OP_FRAME) )
100 
101  def display(self,q):
102  pin.forwardKinematics(self.model,self.data,q)
103  for visual in self.visuals:
104  visual.place( self.viewer,self.data.oMi[visual.jointParent] )
105  self.viewer.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.nv
114  @property
115  def nobs(self): return self.nx+self.withSinCos
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.nq)*2-1)
122  v0 = rand(self.nv)*2-1
123  x0 = np.vstack([q0,v0])
124  assert len(x0)==self.nx
125  self.x = x0.copy()
126  self.r = 0.0
127  return self.obs(self.x)
128 
129  def step(self,u):
130  assert(len(u)==self.nu)
131  _,self.r = self.dynamics(self.x,u)
132  return self.obs(self.x),self.r
133 
134  def obs(self,x):
135  if self.withSinCos:
136  return np.vstack([ np.vstack([np.cos(qi),np.sin(qi)]) for qi in x[:self.nq] ]
137  + [x[self.nq:]],)
138  else: return x.copy()
139 
140  def tip(self,q):
141  '''Return the altitude of pendulum tip'''
142  pin.framesKinematics(self.model,self.data,q)
143  return self.data.oMf[1].translation[2,0]
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.nq])
158  v = x[self.nq:]
159  u = np.clip(np.reshape(np.matrix(u),[self.nu,1]),-self.umax,self.umax)
160 
161 
162  DT = self.DT/self.NDT
163  for i in range(self.NDT):
164  pin.computeAllTerms(self.model,self.data,q,v)
165  M = self.data.M
166  b = self.data.nle
167  #tau = u-self.Kf*v
168  a = inv(M)*(u-self.Kf*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.display(q)
176  time.sleep(1e-4)
177 
178  x[:self.nq] = modulePi(q)
179  x[self.nq:] = np.clip(v,-self.vmax,self.vmax)
180 
181  return x,-cost
182 
183  def render(self):
184  q = self.x[:self.nq]
185  self.display(q)
186  time.sleep(self.DT/10)
187 
188 
def createPendulum(self, nbJoint, rootId=0, prefix='', jointPlacement=None)
Definition: pendulum.py:72
def nx(self)
Definition: pendulum.py:113
def reset(self, x0=None)
Definition: pendulum.py:119
def place(self, display, oMjoint)
Definition: pendulum.py:37
def render(self)
Definition: pendulum.py:183
def nobs(self)
Definition: pendulum.py:115
def nu(self)
Definition: pendulum.py:117
def obs(self, x)
Definition: pendulum.py:134
def display(self, q)
Definition: pendulum.py:101
def __init__(self, nbJoint=1)
Definition: pendulum.py:55
def nv(self)
Definition: pendulum.py:111
def tip(self, q)
Definition: pendulum.py:140
def step(self, u)
Definition: pendulum.py:129
def nq(self)
Definition: pendulum.py:109
def __init__(self, name, jointParent, placement)
Definition: pendulum.py:33
def dynamics(self, x, u, display=False)
Definition: pendulum.py:145


pinocchio
Author(s):
autogenerated on Tue Jun 1 2021 02:45:04