[PYTHON] Amortisseur à ressort avec PyODE
J'ai essayé de mettre en œuvre un système d'amortisseur à ressort avec PyODE. Alors J'ai vérifié si cela fonctionne selon les propriétés physiques.
1. Vue d'ensemble
Un site auquel j'ai fait référence sans autorisation → https://w.atwiki.jp/bambooflow/pages/242.html
Il n'y a pas de ressorts ou d'amortisseurs dans ODE. Par conséquent, la constante du ressort et le taux d'amortissement sont remplacés par les valeurs de ERP et CFM, qui sont les conditions de contrainte mutuelle pour les objets. (Cela semble être écrit dans le manuel ODE.) Dans cet article, nous allons connecter deux objets avec des articulations de curseur et définir ERP et CFM. ↓ Ceci est le code partiel pertinent.
Méthode de réglage de l'amortisseur à ressort ODE (à l'aide du curseur)
h = DT = 0.01 ###Soit h la longueur du pas de temps
kp = 20.0 #### Spring-rate [N/m]
kd = 8.944 #### Damping-rate
erp = h*kp / (h*kp + kd)
cfm = 1.0 / (h*kp + kd)
j = j01 = ode.SliderJoint( world )
j.attach( body0, body1 )
j.setAxis( (0,1,0) ) ####Un curseur qui se déplace uniquement dans la direction de l'axe Y.
j.setParam(ode.ParamLoStop, 0.0) ####C'est la norme sur laquelle fonctionnent les ressorts et les amortisseurs.
j.setParam(ode.ParamHiStop, 0.0) ####La norme sur laquelle fonctionnent les ressorts et les amortisseurs.
j.setParam(ode.ParamStopERP, erp)
j.setParam(ode.ParamStopCFM, cfm)
2 Code (description complète)
Article précédent → "Exemple de jugement de contact PyODE et comportement lié" (https://qiita.com/emuai/items/652d36fb19b6f41dcd38) Il a été réparé sur la base du code de.
Aperçu du code
・ 2 objets (aucun plancher requis)
-Placez le premier objet body0 '' à une hauteur de 0 (*** Strictement 0 était un dysfonctionnement **, il était donc invisible et décalé à une hauteur de 1 mm). .. -Corps fixe0 à l'environnement. -Placez le deuxième objet body1``` dans le ciel` 1m``` au-dessus de body0```.
-Connectez body0 '' et body1 '' avec un curseur
・ Réglez la valeur ERP / CFM correspondant au ressort et à l'amortisseur du curseur.
・ Une fois le calcul terminé, une solution logique qui correspond aux conditions de calcul est générée et comparée au résultat pour le traçage.
↓ Code
slider_as_Spring-Damper
## pyODE example-1: with MPL-plot
## Appended ``Spring and dashpod'' to observe ocillation.
MILLI = 0.001
import ode
# Create a world object
world = ode.World()
DT = 0.001# 0.001 #0.04
G = 9.81
world.setGravity( (0,-G,0) )
R = 0.0001 #10.0 *MILLI
mass = 1.0
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 )
ERP_GLOBAL = 1.0 #0.8 #1.0
CFM_GLOBAL = 0.0
COLLISION = not True
BOUNCE = 0.5
JOINT = True
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()
j = j01 = ode.SliderJoint( world )
j.attach( body0, body1 )
j.setAxis( (0,1,0) )
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
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): #### Unnecessary for Spring-Damper
""" 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) )
tn=0
END_TIME = 5.0
while total_time <= END_TIME:
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:
# Detect collisions and create contact joints
space.collide( (world,contactgroup), near_callback )
world.step(dt)
total_time+=dt
if COLLISION:
# Remove all contact joints
contactgroup.empty()
tn += 1
end_tn = tn
######## 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)
Le but principal est de comparer avec la solution logique. Je l'ai commenté, mais vous pouvez maintenant lister les graphiques avec zêta = 0.0, 0.5, 1.0, 1.5.
3. Résultats
J'ai tracé la hauteur de l'objet en mouvement à 1 m au-dessus du ciel (Y = 1 ''). Étant donné que la masse est de 1 kg et que le coefficient du ressort est de 20 [kg / m] sous gravité, il doit vibrer autour de Y = 0,5 m. Lorsque j'ai essayé de secouer le taux d'atténuation KD '', il s'écartait parfois de la solution logique en fonction de la longueur du pas de temps `` DT```.
Ici, je vais vous montrer deux résultats de calcul.
↓ Premièrement, c'est le résultat de la longueur du pas de temps DT = 0,01```
DT = 0.01
À DT = 0,01, il ne correspond pas à la solution logique (trait épais). Le calcul est trop grossier.
Donc DT est réduit d'un facteur 10.
DT = 0.001
Il a les mêmes propriétés physiques
Ainsi, `` DT = 0,001 '' était normal.
4 Résumé
Il est à noter qu'il ne bouge pas selon les propriétés physiques en fonction du temps. ・ À propos, il semble que le pas de temps soit plus rugueux à condition que la viscosité soit beaucoup plus forte que l'exemple ci-dessus et que la vibration n'apparaisse pas et converge.
Comportement obscur de l'ODE
Où j'ai piégé cette fois:
-Les objets fixes ("` body0 "dans le code) ne semblent pas rester immobiles comme définis sauf s'ils sont placés au-dessus de`` 0.0 (?)
Qu'Est-ce que c'est? Dans tous les cas, si vous devez faire quelque chose comme "Tracer et vérifier les objets fixes", vous risquez de rester coincé dans un pot.