[PYTHON] Try doubling the PyODE slider
Yuki Kotonari
In the previous article `` PyODE Spring Damper'' The spring damper was constructed by the ODE slider (ode.SliderJoint).
However, I want to do compound springs. A single slider does not make much sense because it requires balanced connections of springs and dampers with different properties. (I'm still not sure if they should consist of sliders.)
So I tried to see if I could double the slider between the two objects.
What you did
・ I tried putting two sliders between two objects ・ CFM value adjustment of world -Added visualization of Pygame
Conclusion earlier:
-It was necessary to adjust the CFM value of ** world, but double slides can be configured. ** **
CFM adjustment required
** * This ERP / CFM is different from the slider ERP / CFM. ** **
When there is one slider
ERP_GLOBAL = 1.0
CFM_GLOBAL = 0.0
(Omitted)
world.setERP( ERP_GLOBAL ) #### ERP : Error Reduction Parameter
world.setCFM( CFM_GLOBAL ) #### CFM : Constraint Force Mixing
And so on, ERP = 1.0, `` `CFM = 0.0```, and it worked.
However, I think that 1.0 and 0.0 are originally unreasonable values. It probably moved because there was only one joint and there was nothing to interfere with each other.
When I added the sliders to 2 with the same settings and executed it
At the same time as the start, this message came out and stopped.
It can be said that stopping itself is as expected.
But I think this message is unfriendly. Even if I read it, I have no idea what the cause is.
Right now, I was thinking about adjusting ERP / CFM, so for the time being
ERP_GLOBAL = 1.0
CFM_GLOBAL = 1.0e-6 #0.0
I changed it to. Now it works.
With this configuration, it was okay to leave ERP at 1.0.
Execution result
2D visualization with Pygame.
It follows the method of PyODE "Tutorial 2".
(There are a lot of margins, but I don't think it will take time to make such adjustments.)
↑ The black circle is a fixed object, and the red-brown circle is connected above it via two Slider Joints.
** The slider lines are offset to the left and right, but they overlap on the ODE. ** **
(The posted code is not offset)
The position of the red-brown circle vs. the time plot is output as in the previous case. ↓
** Blue line: Logical solution, Orange line: ODE calculation, Velocity on the vertical axis label is forgotten to be erased **
Instead of doubling the sliders, the coefficients of the springs and dampers are each halved. Then, the result is the same as above.
Code (all)
It's been a lot of mess I will post the whole thing this time as well.
Added visualization of Pygame.
#### 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