[PYTHON] Versuchen Sie, den PyODE-Schieberegler zu verdoppeln
Yuki Kotonari
Im vorherigen Artikel `` PyODE Spring Damper '' Der Federdämpfer wurde mit dem ODE-Schieber (ode.SliderJoint) konstruiert.
Ich möchte jedoch Verbundfedern machen. Ein einzelner Schieber macht wenig Sinn, da er ausgewogene Verbindungen von Federn und Dämpfern mit unterschiedlichen Eigenschaften erfordert. (Ich bin mir immer noch nicht sicher, ob sie aus Schiebereglern bestehen sollen.)
Also habe ich versucht zu sehen, ob ich den Schieberegler zwischen den beiden Objekten verdoppeln kann.
Was du getan hast
・ Ich habe versucht, zwei Schieberegler zwischen zwei Objekte zu setzen ・ CFM-Wertanpassung der Welt
- Visualisierung von Pygame hinzugefügt
Fazit früher:
-Es war notwendig, den CFM-Wert von ** world anzupassen, aber Doppelfolien können konfiguriert werden. ** **.
CFM-Anpassung erforderlich
** * Dieses ERP / CFM unterscheidet sich vom Schieberegler ERP / CFM. ** **.
Wenn es einen Schieberegler gibt
ERP_GLOBAL = 1.0
CFM_GLOBAL = 0.0
(Weggelassen)
world.setERP( ERP_GLOBAL ) #### ERP : Error Reduction Parameter
world.setCFM( CFM_GLOBAL ) #### CFM : Constraint Force Mixing
Und so weiter, `ERP = 1.0```,` CFM = 0.0```, und es hat funktioniert.
Ich denke jedoch, dass 1.0 und 0.0 ursprünglich unangemessene Werte sind. Es bewegte sich wahrscheinlich, weil es nur ein Gelenk gab und es nichts gab, was sich gegenseitig störte.
Als ich die Schieberegler mit den gleichen Einstellungen zu 2 hinzufügte und ausführte
Gleichzeitig mit dem Start wurde eine solche Meldung angezeigt und gestoppt.
Es kann gesagt werden, dass das Stoppen wie erwartet ist.
Aber ich denke, diese Nachricht ist unfreundlich. Selbst wenn ich es lese, habe ich keine Ahnung, was die Ursache ist.
Im Moment habe ich überlegt, ERP / CFM vorerst anzupassen
ERP_GLOBAL = 1.0
CFM_GLOBAL = 1.0e-6 #0.0
Ich habe es geändert in. Jetzt funktioniert es.
In dieser Konfiguration war es in Ordnung, ERP bei 1.0 zu belassen.
Ausführungsergebnis
2D-Visualisierung mit Pygame.
Es folgt der Methode von PyODE "Tutorial 2".
(Es gibt viele Ränder, aber ich glaube nicht, dass es einige Zeit dauern wird, solche Anpassungen vorzunehmen.)
↑ Der schwarze Kreis ist ein festes Objekt und der rotbraune Kreis ist über zwei Gleitgelenke darüber verbunden.
** Die Gleitlinien werden mit linken und rechten Offsets gezeichnet, überlappen sich jedoch auf der ODE. ** **.
(Der gebuchte Code wird nicht verrechnet)
Die Position des rotbraunen Kreises gegenüber dem Zeitdiagramm wird wie im vorherigen Fall ausgegeben. ↓
** Blaue Linie: logische Lösung, orange Linie: ODE-Berechnung, vergessen Sie, die Geschwindigkeit auf der vertikalen Achse zu löschen **
Anstatt die Schieber zu verdoppeln, werden die Feder- und Dämpfungskoeffizienten jeweils halbiert. Dann ist das Ergebnis das gleiche wie oben.
Code (alle)
Es war viel Chaos Ich werde das Ganze auch dieses Mal posten.
Pygame-Visualisierung wurde hinzugefügt.
#### tutorial_1_3.py
## pyODE example-1: with MPL-plot
## Appended: ``Spring and dashpod'' to observe ocillation.
## Appended: Pygame 2d-visualization
import pygame
from pygame.locals import *
#import ode
#DT = 0.05 #1
KEEP_FPS = not True #False
FPS = 10. #30.
RENDERING_INTERVAL = 100 #10
GIF = True
W = 640
H = 640
CX = 320
CY = 320
S = 200.0
def coord(xyz):
(x,y,z) = xyz
"Convert world coordinates to pixel coordinates."
return int( CX +S*x ), int( CY -S*y)
from PIL import Image, ImageDraw
def storeImage( srfc, images ):
if NI_MAX <= len(images):
return
s = srfc
buf = s.get_buffer()
im = Image.frombytes( "RGBA", s.get_size(), buf.raw )
B,G,R,A = im.split()
img = Image.merge("RGBA",(R,G,B,A))
images.append(img)
def gif(images):
print(' @ gif(), ')
image0 = images[0]
image_end = images[-1]
for i in range( 5 ):
images.insert(0,image0)
for i in range( 5 ):
images.append(image_end)
savepath = 'tutorial_1.gif'
images[0].save( savepath, save_all=True, append_images=images[1:], optimize=not False, duration=100, loop=0 )
print(' Exported : [%s]'%savepath)
NI_MAX = 10000
images=[]
# Initialize pygame
pygame.init()
# Open a display
srf = pygame.display.set_mode( (W,H) )
MILLI = 0.001
DT = 0.001# 0.001 #0.04
G = 9.81
import ode
# Create a world object
world = ode.World()
world.setGravity( (0,-G,0) )
R = 0.0001 #10.0 *MILLI
mass = 1.0
ERP_GLOBAL = 1.0 #0.8 #1.0 #0.8 #1.0
CFM_GLOBAL = 1.0e-6 #0.0
COLLISION = not True
BOUNCE = 0.5
JOINT = True
KP = 20.0 #### Spring-rate [N/m]
KD = 8.944 * 0.01 #0.25 #### Damping-rate
def get_body( pos, vel=(0.,0.,0.) ):
# Create a body inside the world
body = ode.Body(world)
M = ode.Mass()
#rho = 2700.0 ## AL??
m = mass
r = R
M.setSphereTotal( m, r )
M.mass = 1.0
body.setMass(M)
body.setPosition( pos )
#body.addForce( (0,200,0) )
body.setLinearVel( vel )
return body
body0 = get_body( (0.,0.001,0.) )
body0.setGravityMode( False )
body1 = get_body( (0.,1.,0.) )
#body1.setGravityMode( False )
bodys = [body0,body1]
RGBs = {body0:(0,0,0), body1:(127,63,63) , None:(63,0,0)}
if COLLISION or JOINT:
# Create a space object
space = ode.Space()
if COLLISION:
# Create a plane geom which prevent the objects from falling forever
floor = ode.GeomPlane( space, (0,1,0), 0.1 )
geom0 = ode.GeomSphere( space, radius=R )
geom0.setBody( body0 )
if JOINT:
geom1 = ode.GeomSphere( space, radius=R )
geom1.setBody( body1 )
j = fix0 = ode.FixedJoint(world)
j.attach( body0, ode.environment )
j.setFixed()
joints = [fix0]
j = j01 = ode.SliderJoint( world )
j.attach( body0, body1 )
j.setAxis( (0,1,0) )
joints.append( j01 )
h = step_size = DT# 0.04
kp = KP #20.0 #### Spring-rate [N/m]
kd = KD #8.944 * 0.01 #0.25 #0.0 #4.45 #8.9 #### Damping-rate
Cc = 2.0 * (mass*kp)**0.5 #### 8.944
zeta = kd / Cc
omega0 = (kp / mass )**0.5
kp_ = 0.5 *KP #20.0 #### Spring-rate [N/m]
kd_ = 0.5 *KD #8.944 * 0.01 #0.25 #0.0 #4.45 #8.9 #### Damping-rate
erp = h*kp_ / (h*kp_ + kd_)
cfm = 1.0 / (h*kp_ + kd_)
j.setParam(ode.ParamLoStop, 0.0)
j.setParam(ode.ParamHiStop, 0.0)
j.setParam(ode.ParamStopERP, erp)
j.setParam(ode.ParamStopCFM, cfm)
j = j01_2 = ode.SliderJoint( world )
j.attach( body0, body1 )
j.setAxis( (0,1,0) )
joints.append( j01_2 )
#kp = KP #20.0 #### Spring-rate [N/m]
#kd = KD #8.944 * 0.01 #0.25 #0.0 #4.45 #8.9 #### Damping-rate
#erp = h*kp / (h*kp + kd)
#cfm = 1.0 / (h*kp + kd)
j.setParam(ode.ParamLoStop, 0.0)
j.setParam(ode.ParamHiStop, 0.0)
j.setParam(ode.ParamStopERP, erp)
j.setParam(ode.ParamStopCFM, cfm)
world.setERP( ERP_GLOBAL ) #### ERP : Error Reduction Parameter
world.setCFM( CFM_GLOBAL ) #### CFM : Constraint Force Mixing
# A joint group for the contact joints that are generated whenever
# two bodies collide
contactgroup = ode.JointGroup()
# Collision callback
def near_callback(args, geom1, geom2):
""" Callback function for the collide() method.
This function checks if the given geoms do collide and
creates contact joints if they do.
"""
# Check if the objects do collide
contacts = ode.collide(geom1, geom2)
# Create contact joints
(world, contactgroup) = args
for c in contacts:
c.setBounce( BOUNCE )
c.setMu(5000)
j = ode.ContactJoint( world, contactgroup, c )
j.attach( geom1.getBody(), geom2.getBody() )
## Proceed the simulation...
total_time = 0.0
dt = DT #0.04 #0.04
import numpy as np
nt = 10000
txyzuvw = np.zeros( (7,nt+1) )
#dt = DT #1.0/fps
fps = FPS #30.0
loopFlag = True
clk = pygame.time.Clock()
END_TIME = 5.0
PRNT = False
tn=-1
while loopFlag and total_time <= END_TIME:
tn += 1
events = pygame.event.get()
for e in events:
if e.type==QUIT:
loopFlag=False
if e.type==KEYDOWN:
loopFlag=False
# Clear the screen
srf.fill((255,255,255))
for joint in joints:
#print('type(joint) = ',type(joint) )
#if joint is j0:
# continue
rgb = (127,127,127)
b_move = None
b0 = joint.getBody(0)
b1 = joint.getBody(1)
if PRNT:
print('b0 = ', b0 )
print('b1 = ', b1 )
if type(joint) == ode.FixedJoint or type(joint) == ode.SliderJoint:
#continue
if None in [b0,b1]:
continue
p0 = coord( b0.getPosition() )
p1 = coord( b1.getPosition() )
elif type(joint) == ode.HingeJoint:
if b0:
if b0.getPosition() != joint.getAnchor():
b_move = b0
if None is b0:
p0 = coord( joint.getAnchor() )
if None is p0:
p0 = coord( body0.getPosition() )
else:
p0 = coord( b0.getPosition() )
if b1:
if b1.getPosition() != joint.getAnchor():
b_move = b0
if None is b1:
p1 = coord( joint.getAnchor() )
if None is p1:
p1 = coord( body0.getPosition() )
else:
p1 = coord( b1.getPosition() )
lw = 5
rgb = RGBs[b_move]
if PRNT:
print('p0=',p0)
print('p1=',p1)
print( flush=True)
if( None is not p0 and None is not p1):
pygame.draw.line( srf, rgb, p0,p1, lw)
for body in bodys:
xyz = body.getPosition()
rgb = RGBs[body] #(127,127,127)
pygame.draw.circle(srf, rgb, coord( xyz ), 20, 0)
pygame.display.flip()
if tn % RENDERING_INTERVAL == 0:
storeImage(srf,images)
body = body1
x,y,z = body.getPosition()
u,v,w = body.getLinearVel()
print( "%1.2fsec: pos=(%6.3f, %6.3f, %6.3f) vel=(%6.3f, %6.3f, %6.3f)" % (total_time, x, y, z, u,v,w) )
if tn <= nt:
txyzuvw[0][tn]=total_time
txyzuvw[1][tn]=x
txyzuvw[2][tn]=y
txyzuvw[3][tn]=z
txyzuvw[4][tn]=u
txyzuvw[5][tn]=v
txyzuvw[6][tn]=w
if COLLISION or JOINT:
# Detect collisions and create contact joints
space.collide( (world,contactgroup), near_callback )
world.step(dt)
# Try to keep the specified framerate
if KEEP_FPS:
clk.tick(fps)
total_time+=dt
if COLLISION:
# Remove all contact joints
contactgroup.empty()
#tn += 1
end_tn = tn
if GIF:
gif( images )
######## MPL-Plot
import matplotlib.pyplot as plt
PLOT_THEORY = True
if PLOT_THEORY:
import math
ys_zeta00 = np.zeros( end_tn )
ys_zeta05 = np.zeros( end_tn )
ys_zeta10 = np.zeros( end_tn )
ys_zeta15 = np.zeros( end_tn )
ys_zeta = np.zeros( end_tn )
A = (mass * G / kp)
y0 = 1.0-A
for tn in range( end_tn ):
t = txyzuvw[0][tn]
ot = omega0 *t
s = sigma = 0.0
#z1 = abs( z*z -1.0 )**0.5
y_zeta_00 = y0 +A *math.cos( ot )
z = 0.5
z1 = abs( z*z -1.0 )**0.5
z2= (s + z) / z1
A_ = A *( 1.0 + ( z2 )**2.0 )**0.5
alpha = math.atan( - z2 )
y_zeta_05 = y0 +A_ *math.exp( -z *ot) * math.cos( ot*z1 + alpha)
y_zeta_10 = y0 +A *math.exp( -ot ) *( (s+1.0) *ot +1 )
z = 1.5
z1 = abs( z*z -1.0 )**0.5
y_zeta_15 = y0 +A * math.exp( - z * ot ) * ( math.cosh( ot*z1 ) +( s+z) / z1 *math.sinh( ot *z1 ) )
'''
ys_zeta00[tn] = y_zeta_00
ys_zeta05[tn] = y_zeta_05
ys_zeta10[tn] = y_zeta_10
ys_zeta15[tn] = y_zeta_15
'''
z = zeta
z1 = abs( z*z -1.0 )**0.5
z2= (s + z) / z1
if z < 1.0:
A_ = A *( 1.0 + ( z2 )**2.0 )**0.5
alpha = math.atan( - z2 )
y_zeta = y0 +A_ *math.exp( -z *ot) * math.cos( ot*z1 + alpha)
if z == 1.0:
y_zeta = y_zeta10
elif z > 1.0:
y_zeta = y0 +A *math.exp( - z * ot ) *( math.cosh( ot*z1 ) +( s + z ) / z1 *math.sinh( ot *z1 ) )
ys_zeta[tn] = y_zeta
'''
plt.plot( txyzuvw[0][0:end_tn], ys_zeta00[0:end_tn], label=r'$\zeta=0$')
plt.plot( txyzuvw[0][0:end_tn], ys_zeta05[0:end_tn], label=r'$\zeta=0.5$')
plt.plot( txyzuvw[0][0:end_tn], ys_zeta10[0:end_tn], label=r'$\zeta=1$')
plt.plot( txyzuvw[0][0:end_tn], ys_zeta15[0:end_tn], label=r'$\zeta=1.5$')
'''
plt.plot( txyzuvw[0][0:end_tn], ys_zeta[0:end_tn], label=r'theory $\zeta=%g$'%(zeta), lw=5.0 )
plt.plot( txyzuvw[0][0:end_tn], txyzuvw[2][0:end_tn], label='Vertical position')
#plt.plot( txyzuvw[0][0:end_tn], txyzuvw[5][0:end_tn], label='Vertical velocity')
plt.xlabel('time [s]')
#plt.ylabel('Vertical position [m]')
plt.ylabel('Position [m] | Velocity [m/s]')
plt.legend()
plt.title( r'$k_{\mathrm{p} }=%g, k_{\mathrm{d}}=%g, C_{\mathrm{c}}=%g, \zeta=%g, \omega_{0}=%g$'%(kp,kd, Cc, zeta, omega0))
xmin = np.min( txyzuvw[0] )
xmax = np.max( txyzuvw[0] )
plt.hlines( [0], xmin, xmax, "blue", linestyles='dashed') # hlines
plt.tight_layout()
#savepath = './y_ERP%g_CFM%g_BR%g.png'%(ERP_GLOBAL, CFM_GLOBAL, BOUNCE)
savepath = './y_DT%g_kp%g_kd%g_zeta%g.png'%(DT, kp,kd, zeta )
plt.savefig( savepath )
print('An image-file exported : [%s]'%savepath )
#plt.show()
plt.pause(1.0)
Recommended Posts