How do you implement a "servo" joint in Pymunk?
I'm trying to create a simple model where a box is balanced on a single thin "leg" below it. I've been able create a box and join it to the ground using a PinJoint, but there doesn't seem to be any way to control the angle where the join attaches to the box. I want to be able to specify the angle of attachment. None of the other joints seem to support this. They all seem to be passive joints, with the exception of the SimpleMotor joint, but even that is only a constant spinning joint that you can't control.
I've managed to cobble something together, by using a PinJoint to attach two thin boxes together at their ends, as well as a SimpleMotor, to make them rotate relative to each other in response to the user pressing the "up" and "down" arrow keys. Below is the code:
import sys
import pygame
from pygame.locals import USEREVENT, QUIT, KEYDOWN, KEYUP, K_s, K_r, K_q, K_ESCAPE, K_UP, K_DOWN
from pygame.color import THECOLORS
import pymunk
from pymunk import Vec2d
import pymunk.pygame_util
class Simulator(object):
def __init__(self):
self.display_flags = 0
self.display_size = (600, 600)
self.space = pymunk.Space()
self.space.gravity = (0.0, -1900.0)
self.space.damping = 0.999 # to prevent it from blowing up.
# Pymunk physics coordinates start from the lower right-hand corner of the screen.
self.ground_y = 100
ground = pymunk.Segment(self.space.static_body, (5, self.ground_y), (595, self.ground_y), 1.0)
ground.friction = 1.0
self.space.add(ground)
self.screen = None
self.draw_options = None
def reset_bodies(self):
for body in self.space.bodies:
if not hasattr(body, 'start_position'):
continue
body.position = Vec2d(body.start_position)
body.force = 0, 0
body.torque = 0
body.velocity = 0, 0
body.angular_velocity = 0
body.angle = body.start_angle
def draw(self):
### Clear the screen
self.screen.fill(THECOLORS["white"])
### Draw space
self.space.debug_draw(self.draw_options)
### All done, lets flip the display
pygame.display.flip()
def main(self):
pygame.init()
self.screen = pygame.display.set_mode(self.display_size, self.display_flags)
width, height = self.screen.get_size()
self.draw_options = pymunk.pygame_util.DrawOptions(self.screen)
def to_pygame(p):
"""Small hack to convert pymunk to pygame coordinates"""
return int(p.x), int(-p.y+height)
def from_pygame(p):
return to_pygame(p)
clock = pygame.time.Clock()
running = True
font = pygame.font.Font(None, 16)
# Create the torso box.
box_width = 50
box_height = 100
leg_length = 100
mass = 1
points = [(-100, -1), (0, -1), (0, 1), (-100, 1)]
moment = pymunk.moment_for_poly(mass, points)
body1 = pymunk.Body(mass, moment)
# body1.position = (0, 0)
body1.position = (self.display_size[0]/2, self.ground_y+100)
body1.start_position = Vec2d(body1.position)
body1.start_angle = body1.angle
shape1 = pymunk.Poly(body1, points)
shape1.friction = 0.8
self.space.add(body1, shape1)
# Create bar 2 extending from the right to the origin.
mass = 1
points = [(100, -1), (0, -1), (0, 1), (100, 1)]
moment = pymunk.moment_for_poly(mass, points)
body2 = pymunk.Body(mass, moment)
# body2.position = (0, 0)
body2.position = (self.display_size[0]/2, self.ground_y+100)
body2.start_position = Vec2d(body2.position)
body2.start_angle = body2.angle
shape2 = pymunk.Poly(body2, points)
shape2.friction = 0.8
self.space.add(body2, shape2)
# Link bars together at end.
pj = pymunk.PinJoint(body1, body2, (0, 0), (0, 0))
self.space.add(pj)
motor_joint = pymunk.SimpleMotor(body1, body2, 0)
self.space.add(motor_joint)
pygame.time.set_timer(USEREVENT+1, 70000) # apply force
pygame.time.set_timer(USEREVENT+2, 120000) # reset
pygame.event.post(pygame.event.Event(USEREVENT+1))
pygame.mouse.set_visible(False)
simulate = False
while running:
for event in pygame.event.get():
if event.type == QUIT or (event.type == KEYDOWN and event.key in (K_q, K_ESCAPE)):
#running = False
sys.exit(0)
elif event.type == KEYDOWN and event.key == K_s:
# Start/stop simulation.
simulate = not simulate
elif event.type == KEYDOWN and event.key == K_r:
# Reset.
# simulate = False
self.reset_bodies()
elif event.type == KEYDOWN and event.key == K_UP:
motor_joint.rate = 1
elif event.type == KEYDOWN and event.key == K_DOWN:
motor_joint.rate = -1
elif event.type == KEYUP:
motor_joint.rate = 0
self.draw()
### Update physics
fps = 50
iterations = 25
dt = 1.0/float(fps)/float(iterations)
if simulate:
for x in range(iterations): # 10 iterations to get a more stable simulation
self.space.step(dt)
pygame.display.flip()
clock.tick(fps)
if __name__ == '__main__':
sim = Simulator()
sim.main()
However, the behavior is somewhat strange. When you press up/down, that dynamically sets the rate on the SimpleMotor joint, causing the two boxes to pivot at their common "servo joint", like:
except over time the two bars will flip onto one end, and otherwise defy gravity and look like:
Why is this? I'm still fairly new to the Pymunk/Chipmunk physics simulator, so I'm not sure I'm using these joints correctly.
A couple of things that can cause problems:
Ignore collisions between the two shapes. Since the motor and pin joint force them together, but collision resolution pushes them apart strange things might happen. You can do this by setting the two shapes to the same group:
shape_filter = pymunk.ShapeFilter(group=1) shape1.filter = shape_filter shape2.filter = shape_filterThe center of gravity for the two shapes are at their ends, not in the center. Try to move it to center (
[(-50, -1), (50, -1), (50, 1), (-50, 1)]).
(In this case I think 1 is enough to fix the problem, but I added 2 in case you notice other strange things)
来源：https://stackoverflow.com/questions/49594143/pymunk-servo-joint