[ODE] strange oscillations while steering a roboter arm

[Sebastian Gieselmann]

Here is the Code:


import pygame
from pygame.locals import *
import ode


def coord(x,y):
    "Convert world coordinates to pixel coordinates."
    return 320+170*x, 400-170*y

def xcoord(x):
    return 320+170*x

def ycoord(y):
    return 400-170*y

# Initialize pygame
pygame.init()

# Open a display
srf = pygame.display.set_mode((640,480))

# Create a world object
world = ode.World()
world.setGravity((0,-9.81,0))
world.setERP(1.8)
#world.setCFM(0.002)


# Create bodies
arm = ode.Body(world)
Marm = ode.Mass()
Marm.setSphere(3000, 0.2)
arm.setMass(Marm)
arm.setPosition(( 0, 1, 0))

# Joints
jArm = ode.HingeJoint(world)
jArm.attach(arm, ode.environment)
jArm.setAnchor( (0, 2, 0) )
jArm.setAxis((0,0,1))

# Simulation loop...

fps = 50
dt = 1.0/fps
loopFlag = True
clk = pygame.time.Clock()
force = 0.0
#jArm.addTorque(1000)

while loopFlag:
    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))

    # Draw the two bodies
    x2,y2,z2 = arm.getPosition()
    pygame.draw.circle(srf, (255,0,0), coord(x2,y2), 20, 0)
    pygame.draw.line(srf, (255,0,0), coord(0,2), coord(x2,y2), 2)

    pygame.display.flip()

    # Next simulation step          
    force = force + 2.4
    jArm.addTorque(force)

    world.step(dt)
    
    # Try to keep the specified framerate    
    clk.tick(fps)