|
44
|
1 from PyQt4.QtGui import *
|
|
|
2 from PyQt4.QtCore import *
|
|
|
3 from PyQt4.QtOpenGL import QGLWidget
|
|
|
4 from OpenGL.GL import *
|
|
|
5 from OpenGL.GLU import gluPerspective
|
|
|
6 import sys
|
|
|
7 from random import random
|
|
|
8 from math import pi, cos, sin, fabs, sqrt
|
|
|
9 from numpy import mat, array, ones, zeros, eye
|
|
|
10 from numpy.linalg import norm
|
|
|
11 import numpy as np
|
|
|
12 import time
|
|
|
13 import scipy.integrate
|
|
|
14
|
|
|
15 """
|
|
|
16 Test script that lets a dice bounce.
|
|
|
17 Converted from 20-sim equations into python code.
|
|
|
18
|
|
|
19 20-sim website:
|
|
|
20 http://www.20sim.com
|
|
|
21
|
|
|
22 """
|
|
|
23 def drawCube(w):
|
|
|
24 glBegin(GL_QUADS) # Start Drawing The Cube
|
|
|
25 glColor3f(0.0,1.0,0.0) # Set The Color To Blue
|
|
|
26 glVertex3f( w, w,-w) # Top Right Of The Quad (Top)
|
|
|
27 glVertex3f(-w, w,-w) # Top Left Of The Quad (Top)
|
|
|
28 glVertex3f(-w, w, w) # Bottom Left Of The Quad (Top)
|
|
|
29 glVertex3f( w, w, w) # Bottom Right Of The Quad (Top)
|
|
|
30
|
|
|
31 glColor3f(1.0,0.5,0.0) # Set The Color To Orange
|
|
|
32 glVertex3f( w,-w, w) # Top Right Of The Quad (Bottom)
|
|
|
33 glVertex3f(-w,-w, w) # Top Left Of The Quad (Bottom)
|
|
|
34 glVertex3f(-w,-w,-w) # Bottom Left Of The Quad (Bottom)
|
|
|
35 glVertex3f( w,-w,-w) # Bottom Right Of The Quad (Bottom)
|
|
|
36
|
|
|
37 glColor3f(1.0,0.0,0.0) # Set The Color To Red
|
|
|
38 glVertex3f( w, w, w) # Top Right Of The Quad (Front)
|
|
|
39 glVertex3f(-w, w, w) # Top Left Of The Quad (Front)
|
|
|
40 glVertex3f(-w,-w, w) # Bottom Left Of The Quad (Front)
|
|
|
41 glVertex3f( w,-w, w) # Bottom Right Of The Quad (Front)
|
|
|
42
|
|
|
43 glColor3f(1.0,1.0,0.0) # Set The Color To Yellow
|
|
|
44 glVertex3f( w,-w,-w) # Bottom Left Of The Quad (Back)
|
|
|
45 glVertex3f(-w,-w,-w) # Bottom Right Of The Quad (Back)
|
|
|
46 glVertex3f(-w, w,-w) # Top Right Of The Quad (Back)
|
|
|
47 glVertex3f( w, w,-w) # Top Left Of The Quad (Back)
|
|
|
48
|
|
|
49 glColor3f(0.0,0.0,1.0) # Set The Color To Blue
|
|
|
50 glVertex3f(-w, w, w) # Top Right Of The Quad (Left)
|
|
|
51 glVertex3f(-w, w,-w) # Top Left Of The Quad (Left)
|
|
|
52 glVertex3f(-w,-w,-w) # Bottom Left Of The Quad (Left)
|
|
|
53 glVertex3f(-w,-w, w) # Bottom Right Of The Quad (Left)
|
|
|
54
|
|
|
55 glColor3f(1.0,0.0,1.0) # Set The Color To Violet
|
|
|
56 glVertex3f( w, w,-w) # Top Right Of The Quad (Right)
|
|
|
57 glVertex3f( w, w, w) # Top Left Of The Quad (Right)
|
|
|
58 glVertex3f( w,-w, w) # Bottom Left Of The Quad (Right)
|
|
|
59 glVertex3f( w,-w,-w) # Bottom Right Of The Quad (Right)
|
|
|
60 glEnd() # Done Drawing The Quad
|
|
|
61
|
|
|
62 def drawFloor(w, h):
|
|
|
63 glBegin(GL_QUADS) # Start Drawing The Cube
|
|
|
64
|
|
|
65 glColor3f(1.0,0.5,0.0) # Set The Color To Orange
|
|
|
66 glVertex3f( w,-w,h)# Top Right Of The Quad (Bottom)
|
|
|
67 glVertex3f(-w,-w,h)# Top Left Of The Quad (Bottom)
|
|
|
68 glVertex3f(-w,w,h)# Bottom Left Of The Quad (Bottom)
|
|
|
69 glVertex3f( w,w,h)# Bottom Right Of The Quad (Bottom)
|
|
|
70 glEnd() # Done Drawing The Quad
|
|
|
71
|
|
|
72 def drawAxis():
|
|
|
73 glLineWidth(0.5)
|
|
|
74 glBegin(GL_LINES)
|
|
|
75 glColor3f(1.0, 0.0, 0.0)
|
|
|
76 glVertex3f(0,0,0)
|
|
|
77 glVertex3f(1,0,0)
|
|
|
78 glColor3f(0.0, 1.0, 0.0)
|
|
|
79 glVertex3f(0,0,0)
|
|
|
80 glVertex3f(0,1,0)
|
|
|
81 glColor3f(0.0, 0.0, 1.0)
|
|
|
82 glVertex3f(0,0,0)
|
|
|
83 glVertex3f(0,0,1)
|
|
|
84 glEnd()
|
|
|
85
|
|
|
86
|
|
|
87 def cross(A, B):
|
|
|
88 a = A.A1
|
|
|
89 b = B.A1
|
|
|
90 return mat(np.cross(a, b)).T
|
|
|
91
|
|
|
92 def skew(X):
|
|
|
93 Y = mat(zeros( (3, 3) ))
|
|
|
94 a,b,c = X.A1
|
|
|
95 Y[0,1] = -c
|
|
|
96 Y[0,2] = b
|
|
|
97 Y[1,0] = c
|
|
|
98 Y[1,2] = -a
|
|
|
99 Y[2,0] = -b
|
|
|
100 Y[2,1] = a
|
|
|
101 return Y
|
|
|
102
|
|
|
103 def adjoint(T):
|
|
|
104 W = T[0:3, 0]
|
|
|
105 V = T[3:6, 0]
|
|
|
106 a = mat(zeros( (6,6) ) )
|
|
|
107 a[0:3, 0:3] = skew(W)
|
|
|
108 a[3:6, 0:3] = skew(V)
|
|
|
109 a[3:6, 3:6] = skew(W)
|
|
|
110 return a
|
|
|
111
|
|
|
112 def Adjoint(H):
|
|
|
113 R = H[0:3, 0:3]
|
|
|
114 P = H[0:3, 3]
|
|
|
115 a = mat(zeros( (6,6) ) )
|
|
|
116 a[0:3, 0:3] = R
|
|
|
117 a[3:6, 3:6] = R
|
|
|
118 a[3:6, 0:3] = skew(P) * R
|
|
|
119 return a
|
|
|
120
|
|
|
121 def quatToR(q):
|
|
|
122 x, y, z, w = q.A1
|
|
|
123 r = mat(eye(3))
|
|
|
124 r[0,0] = 1 - (2*y**2+2*z**2)
|
|
|
125 r[0,1] = 2*x*y+2*z*w
|
|
|
126 r[0,2] = 2*x*z - 2*y*w
|
|
|
127 r[1,0] = 2*x*y-2*z*w
|
|
|
128 r[1,1] = 1 - (2*x**2 + 2*z**2)
|
|
|
129 r[1,2] = 2*y*z + 2*x*w
|
|
|
130 r[2,0] = 2*x*z+2*y*w
|
|
|
131 r[2,1] = 2*y*z - 2*x*w
|
|
|
132 r[2,2] = 1 - (2*x**2+2*y**2)
|
|
|
133 return r
|
|
|
134
|
|
|
135 def rotateAbout(axis, angle):
|
|
|
136 ax, ay, az = (axis/norm(axis)).A1
|
|
|
137 qx = ax*sin(angle/2.0)
|
|
|
138 qy = ay*sin(angle/2.0)
|
|
|
139 qz = az*sin(angle/2.0)
|
|
|
140 qw = cos(angle/2.0)
|
|
|
141 q = mat(array([qx,qy,qz,qw])).T
|
|
|
142 return q
|
|
|
143
|
|
|
144 def normalizeQuaternion(quat):
|
|
|
145 x,y,z,w = quat.A1
|
|
|
146 magnitude = sqrt(x*x + y*y + z*z + w*w)
|
|
|
147 x = x / magnitude
|
|
|
148 y = y / magnitude
|
|
|
149 z = z / magnitude
|
|
|
150 w = w / magnitude
|
|
|
151 quat[0, 0] = x
|
|
|
152 quat[1, 0] = y
|
|
|
153 quat[2, 0] = z
|
|
|
154 quat[3, 0] = w
|
|
|
155 return quat
|
|
|
156
|
|
|
157 def VTo4x4(V):
|
|
|
158 v1, v2, v3 = V.A1
|
|
|
159 return mat(array( \
|
|
|
160 [[0.0, -v3, v2, -v1], \
|
|
|
161 [ v3, 0.0, -v1, -v2], \
|
|
|
162 [-v2, v1, 0.0, -v3], \
|
|
|
163 [v1, v2, v3, 0.0] ]))
|
|
|
164
|
|
|
165 def homogeneous(R,p):
|
|
|
166 H = mat(eye(4))
|
|
|
167 H[0:3, 0:3] = R
|
|
|
168 H[0:3, 3] = p
|
|
|
169 return H
|
|
|
170
|
|
|
171 def rateOfChange(states, thetime, parameters):
|
|
|
172 quat = states[0:4, 0] # Orientation (4)
|
|
|
173 pos = states[4:7, 0] # Position (3)
|
|
|
174 P = states[7:13, 0] # Momentum (6)
|
|
|
175 massI, gravity = parameters
|
|
|
176 # Rigid body parts:
|
|
|
177 # Forward Kinematic chain:
|
|
|
178 H = homogeneous(quatToR(quat), pos) # Forward kinematics
|
|
|
179
|
|
|
180 AdjX2 = mat(eye(6)) # The connectionpoint in the real world
|
|
|
181 adjAdjX2_1 = adjoint(AdjX2[0:6,0])
|
|
|
182 adjAdjX2_2 = adjoint(AdjX2[0:6,1])
|
|
|
183 adjAdjX2_3 = adjoint(AdjX2[0:6,2])
|
|
|
184 adjAdjX2_4 = adjoint(AdjX2[0:6,3])
|
|
|
185 adjAdjX2_5 = adjoint(AdjX2[0:6,4])
|
|
|
186 adjAdjX2_6 = adjoint(AdjX2[0:6,5])
|
|
|
187 AdjInv2 = Adjoint(H.I)
|
|
|
188 M2 = AdjInv2.T * (massI * AdjInv2) # Transfor mass to base frame
|
|
|
189 MassMatrix = M2
|
|
|
190
|
|
|
191 wrenchGrav2 = mat( zeros((1,6)) )
|
|
|
192 wrenchGrav2[0, 0:3] = -cross(gravity, pos).T
|
|
|
193 wrenchGrav2[0, 3:6] = gravity.T
|
|
|
194
|
|
|
195 Bk = mat( zeros( (6,6) ))
|
|
|
196 Bk[0:3, 0:3] = skew(P[0:3, 0])
|
|
|
197 Bk[3:6, 0:3] = skew(P[3:6, 0])
|
|
|
198 Bk[0:3, 3:6] = skew(P[3:6, 0])
|
|
|
199
|
|
|
200 # TODO: do this a cholesky:
|
|
|
201 v = np.linalg.solve(MassMatrix, P) # Matrix inverse like thingy !
|
|
|
202
|
|
|
203 T2_00 = v # Calculate the relative twist!
|
|
|
204 TM2 = T2_00.T * M2
|
|
|
205 twistExternal = T2_00 # Twist van het blokje
|
|
|
206 TMSum2 = TM2
|
|
|
207
|
|
|
208 PDotBodies = mat( zeros( (6,1)) )
|
|
|
209 PDotBodies[0,0] = TMSum2 * (adjAdjX2_1 * T2_00)
|
|
|
210 PDotBodies[1,0] = TMSum2 * (adjAdjX2_2 * T2_00)
|
|
|
211 PDotBodies[2,0] = TMSum2 * (adjAdjX2_3 * T2_00)
|
|
|
212 PDotBodies[3,0] = TMSum2 * (adjAdjX2_4 * T2_00)
|
|
|
213 PDotBodies[4,0] = TMSum2 * (adjAdjX2_5 * T2_00)
|
|
|
214 PDotBodies[5,0] = TMSum2 * (adjAdjX2_6 * T2_00)
|
|
|
215
|
|
|
216 PDot = -PDotBodies - Bk * v
|
|
|
217 PDot += wrenchGrav2.T
|
|
|
218
|
|
|
219 ##### Contact wrench part:
|
|
|
220 HB_W = H # Is H-matrix van het blokje
|
|
|
221 WrenchB = mat(zeros( (1,6) ))
|
|
|
222 for px in [-0.5, 0.5]:
|
|
|
223 for py in [-0.5, 0.5]:
|
|
|
224 for pz in [-0.5, 0.5]:
|
|
|
225 HB1_B = homogeneous(mat(eye(3)), mat([px,py,pz]).T)
|
|
|
226 HB1_W = HB_W * HB1_B
|
|
|
227 HW1_W = homogeneous(mat(eye(3)), HB1_W[0:3,3])
|
|
|
228 HW_W1 = HW1_W.I
|
|
|
229 HB_W1 = HW_W1 * HB_W
|
|
|
230
|
|
|
231 AdjHB_W1 = Adjoint(HB_W1)
|
|
|
232 TB_W1_W1 = AdjHB_W1 * twistExternal
|
|
|
233 z = HB1_W[2, 3]
|
|
|
234 vx, vy, vz = TB_W1_W1[3:6, 0].A1
|
|
|
235 if z < 0:
|
|
|
236 # Contact forces:
|
|
|
237 Fx = -50.0*vx
|
|
|
238 Fy = -50.0*vy
|
|
|
239 Fz = -z*50000.0
|
|
|
240 else:
|
|
|
241 Fx = 0.0
|
|
|
242 Fy = 0.0
|
|
|
243 Fz = 0.0
|
|
|
244 # TODO: reflect impulse
|
|
|
245 WrenchW1 = mat([0,0,0,0,0,Fz])
|
|
|
246 # Transform it back:
|
|
|
247 WrenchB += (AdjHB_W1.T * WrenchW1.T).T
|
|
|
248 ##### End of contact wrench
|
|
|
249
|
|
|
250 PDot += (WrenchB * AdjInv2).T
|
|
|
251
|
|
|
252 # Position and orientation rates:
|
|
|
253 QOmega = VTo4x4(v[0:3, 0])
|
|
|
254 quatDot = 0.5 * QOmega * quat
|
|
|
255 vel = v[3:6, 0]
|
|
|
256 posDot = skew(v[0:3]) * pos + vel
|
|
|
257 # The rate vector:
|
|
|
258 rates = mat(zeros( (13,1) ))
|
|
|
259 rates[0:4, 0] = quatDot
|
|
|
260 rates[4:7, 0] = posDot
|
|
|
261 rates[7:13, 0] = PDot
|
|
|
262 return rates
|
|
|
263
|
|
|
264 def fWrapper(y, t, parameters):
|
|
|
265 y = mat(y).T
|
|
|
266 dy = rateOfChange(y, t, parameters)
|
|
|
267 return dy.T.A1
|
|
|
268
|
|
|
269 def SCIPY(endtime, dt, state, parameters):
|
|
|
270 times = np.arange(0.0, endtime, dt)
|
|
|
271 y0 = state.T.A1
|
|
|
272 res = scipy.integrate.odeint(fWrapper, y0, times, args=(parameters,))
|
|
|
273 states = []
|
|
|
274 res = res.T
|
|
|
275 r,c = res.shape
|
|
|
276 for ci in range(0,c):
|
|
|
277 states.append(mat(res[:,ci]).T)
|
|
|
278 return states
|
|
|
279
|
|
|
280 def RK4(endtime, dt, state, parameters):
|
|
|
281 t = 0.0
|
|
|
282 states = []
|
|
|
283 while t < endtime:
|
|
|
284 newstate = mat (zeros( state.shape )) # Create a new object
|
|
|
285
|
|
|
286 #### Runge Kutta integration:
|
|
|
287 k1 = rateOfChange(state, t, parameters)
|
|
|
288 k2 = rateOfChange(state + 0.5*dt*k1, t, parameters)
|
|
|
289 k3 = rateOfChange(state + 0.5*dt*k1, t, parameters)
|
|
|
290 k4 = rateOfChange(state + dt*k3, t, parameters)
|
|
|
291 newstate = state + (dt/6.0)*(k1+2*k2+2*k3+k4)
|
|
|
292
|
|
|
293 # Normalize quat:
|
|
|
294 newstate[0:4, 0] = normalizeQuaternion(newstate[0:4, 0])
|
|
|
295 states.append(newstate)
|
|
|
296
|
|
|
297 state = newstate
|
|
|
298
|
|
|
299 t += dt
|
|
|
300 print state[6,0], t, ' ', (t/endtime)*100.0, '%'
|
|
|
301 return states
|
|
|
302
|
|
|
303
|
|
|
304 def simulate(endtime, dt):
|
|
|
305 PInitial = mat( zeros((6,1)) )
|
|
|
306 posInitial = mat(array([-1.2, -1.3, 2.8])).T
|
|
|
307 quatInitial = rotateAbout(mat(array([0.2, 1.0, 0.4])).T, 0.5)
|
|
|
308 # Parameters:
|
|
|
309 gravity = mat( array([0,0,-9.81]) ).T
|
|
|
310 massI = mat(eye(6)) * 1.01 # Mass matrix
|
|
|
311 parameters = (massI, gravity)
|
|
|
312
|
|
|
313 # The state vector!
|
|
|
314 state = mat(zeros( (13,1) ))
|
|
|
315 state[0:4, 0] = quatInitial
|
|
|
316 state[4:7, 0] = posInitial
|
|
|
317 state[7:13, 0] = PInitial
|
|
|
318
|
|
|
319 return SCIPY(endtime, dt, state, parameters)
|
|
|
320
|
|
|
321 class W(QGLWidget):
|
|
|
322 time = 0.0
|
|
|
323 index = 0
|
|
|
324 def __init__(self, states, dt, parent=None):
|
|
|
325 super(W, self).__init__(parent)
|
|
|
326 self.firstRun = True
|
|
|
327 self.savePNGS = False
|
|
|
328 self.dt = dt
|
|
|
329 self.resize(500,500)
|
|
|
330 self.states = states
|
|
|
331 self.UP()
|
|
|
332 t = QTimer(self)
|
|
|
333 t.timeout.connect(self.UP)
|
|
|
334 t.start(self.dt*1000.0)
|
|
|
335
|
|
|
336 def UP(self):
|
|
|
337 self.time += self.dt
|
|
|
338 if self.states:
|
|
|
339 state = self.states[self.index]
|
|
|
340 self.Dicequat = state[0:4, 0]
|
|
|
341 self.Dicepos = state[4:7, 0]
|
|
|
342 self.index += 1
|
|
|
343 if self.index == len(self.states):
|
|
|
344 self.index = 0
|
|
|
345 self.firstRun = False
|
|
|
346 # Paint:
|
|
|
347 self.update()
|
|
|
348 if self.firstRun:
|
|
|
349 # Create png images for the movie:
|
|
|
350 if self.savePNGS:
|
|
|
351 pm = self.renderPixmap()
|
|
|
352 pm.save('image'+str(self.index)+'.png')
|
|
|
353
|
|
|
354 def initializeGL(self):
|
|
|
355 glClearColor(0.0, 0.5, 0.0, 1.0)
|
|
|
356 glEnable(GL_DEPTH_TEST)
|
|
|
357 glDepthFunc(GL_LESS)
|
|
|
358 glShadeModel(GL_SMOOTH)
|
|
|
359 def resizeGL(self,w,h):
|
|
|
360 glViewport(0, 0, w, h)
|
|
|
361 glMatrixMode(GL_PROJECTION)
|
|
|
362 glLoadIdentity()
|
|
|
363 gluPerspective(45.0, float(w)/float(h), 0.1, 100.0)
|
|
|
364 glMatrixMode(GL_MODELVIEW)
|
|
|
365 def paintGL(self):
|
|
|
366 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) # Clear buffers
|
|
|
367 glLoadIdentity() # Reset The View
|
|
|
368
|
|
|
369 glLoadIdentity()
|
|
|
370 glTranslatef(0.0,-2.0,-10.0) # Move Left And Into The Screen
|
|
|
371 glRotatef(-90.0, 1.0, 0.0, 0.0)
|
|
|
372 drawFloor(2.0, 0.0)
|
|
|
373 drawAxis()
|
|
|
374 self.renderText(1.0, 0.0, 0.0, 'X')
|
|
|
375 self.renderText(0.0, 1.0, 0.0, 'Y')
|
|
|
376 self.renderText(0.0, 0.0, 1.0, 'Z')
|
|
|
377
|
|
|
378 self.renderText(0.0,0.0,1.2,str(self.time))
|
|
|
379
|
|
|
380 x,y,z = self.Dicepos.A1
|
|
|
381 R = quatToR(self.Dicequat)
|
|
|
382
|
|
|
383 glTranslatef(x, y, z)
|
|
|
384 # Trick to rotate the openGL matrix:
|
|
|
385 r = R.A1
|
|
|
386 rotR = (r[0], r[3], r[6], 0.0,
|
|
|
387 r[1], r[4], r[7], 0.0,
|
|
|
388 r[2], r[5], r[8], 0.0,
|
|
|
389 0.0, 0.0, 0.0, 1.0)
|
|
|
390 glMultMatrixd(rotR)
|
|
|
391
|
|
|
392 drawCube(0.6)
|
|
|
393
|
|
|
394 et = 20.0
|
|
|
395 dt = 0.04
|
|
|
396 print 'starting integration... endtime =', et, ' stepsize =', dt
|
|
|
397 t0 = time.time()
|
|
|
398 states = simulate(et, dt)
|
|
|
399 t1 = time.time()
|
|
|
400 print 'That was heavy, it took me ', t1-t0, ' seconds!'
|
|
|
401 app = QApplication(sys.argv)
|
|
|
402 w = W(states, dt)
|
|
|
403 w.show()
|
|
|
404 sys.exit(app.exec_())
|
|
|
405
|
