Skip to main content
 首页 » 编程设计

python之如何修复 Pymunk 中不平衡的下落运动学

2025年01月19日9sharpest

为什么以下 Pymunk 示例中的物理错误?

from __future__ import print_function 
import sys 
from math import pi 
 
import pygame 
from pygame.locals import USEREVENT, QUIT, KEYDOWN, KEYUP, K_s, K_r, K_q, K_ESCAPE, K_UP, K_DOWN, K_LEFT, K_RIGHT 
from pygame.color import THECOLORS 
 
import pymunk 
from pymunk import Vec2d 
import pymunk.pygame_util 
 
LEG_GROUP = 1 
 
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 
        leg_length = 125 
        leg_thickness = 2 
 
        leg_shape_filter = pymunk.ShapeFilter(group=LEG_GROUP) 
 
        # Create torso. 
        mass = 200 
        points = [(-box_width/2, -box_height/2), (-box_width/2, box_height/2), (box_width/2, box_height/2), (box_width/2, -box_height/2)] 
        moment = pymunk.moment_for_poly(mass, points) 
        body1 = pymunk.Body(mass, moment) 
        body1.position = (self.display_size[0]/2, self.ground_y+box_height/2+leg_length) 
        body1.start_position = Vec2d(body1.position) 
        body1.start_angle = body1.angle 
        shape1 = pymunk.Poly(body1, points) 
        shape1.filter = leg_shape_filter 
        shape1.friction = 0.8 
        shape1.elasticity = 0.0 
        self.space.add(body1, shape1) 
 
        # Create leg extending from the right to the origin. 
        mass = 10 
        points = [ 
            (leg_thickness/2, -leg_length/2), 
            (-leg_thickness/2, -leg_length/2), 
            (-leg_thickness/2, leg_length/2), 
            (leg_thickness/2, leg_length/2) 
        ] 
        moment = pymunk.moment_for_poly(mass, points) 
        body2 = pymunk.Body(mass, moment) 
        body2.position = (self.display_size[0]/2-box_width/2+leg_thickness/2, self.ground_y+leg_length/2) 
        body2.start_position = Vec2d(body2.position) 
        body2.start_angle = body2.angle 
        shape2 = pymunk.Poly(body2, points) 
        shape2.filter = leg_shape_filter 
        shape2.friction = 0.8 
        shape2.elasticity = 0.0 
        self.space.add(body2, shape2) 
 
        # Link bars together at end. 
        pj = pymunk.PivotJoint(body1, body2, (self.display_size[0]/2-box_width/2, self.ground_y+leg_length)) 
        self.space.add(pj) 
 
        # Attach the foot to the ground in a fixed position. 
        # We raise it above by the thickness of the leg to simulate a ball-foot. Otherwise, the default box foot creates discontinuities. 
        pj = pymunk.PivotJoint(self.space.static_body, body2, (self.display_size[0]/2-box_width/2, self.ground_y+leg_thickness)) 
        self.space.add(pj) 
 
        # Actuate the bars via a motor. 
        motor_joint = pymunk.SimpleMotor(body1, body2, 0) 
        motor_joint.max_force = 1e10 # mimicks default infinity 
        # motor_joint.max_force = 1e9 
        # motor_joint.max_force = 1e7 # too weak, almost no movement 
        self.space.add(motor_joint) 
 
        # Add hard stops to leg pivot so the leg can't rotate through the torso. 
        hip_limit_joint = pymunk.RotaryLimitJoint(body1, body2, -pi/4., pi/4.) # -45deg:+45deg 
        self.space.add(hip_limit_joint) 
 
        last_body1_pos = None 
        last_body1_vel = None 
        simulate = False 
        while running: 
            # print('angles:', body1.angle, body2.angle) 
            # print('torso force:', body1.force) 
            print('body1.position: %.02f %.02f' % (body1.position.x, body1.position.y)) 
 
            current_body1_vel = None 
            if last_body1_pos: 
                current_body1_vel = body1.position - last_body1_pos 
                print('current_body1_vel: %.02f %.02f' % (current_body1_vel.x, current_body1_vel.y)) 
 
            current_body1_accel = None 
            if last_body1_vel: 
                current_body1_accel = current_body1_vel - last_body1_vel 
                print('current_body1_accel: %.02f %.02f' % (current_body1_accel.x, current_body1_accel.y)) 
 
            servo_angle = (body1.angle - body2.angle) * 180/pi # 0 degrees means leg is angled straight down 
            servo_cw_enabled = servo_angle > -45 
            servo_ccw_enabled = servo_angle < 45 
 
            for event in pygame.event.get(): 
                if event.type == QUIT or (event.type == KEYDOWN and event.key in (K_q, K_ESCAPE)): 
                    sys.exit(0) 
                elif event.type == KEYDOWN and event.key == K_s: 
                    # Start/stop simulation. 
                    simulate = not simulate 
 
            last_body1_pos = Vec2d(body1.position) 
            if current_body1_vel: 
                last_body1_vel = Vec2d(current_body1_vel) 
 
            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() 

这会呈现一个放置在细腿上的盒子。腿通过枢轴接头连接到盒子,并通过另一个枢轴接头连接到地面。但是,腿连接到箱子上时偏向左侧,因此重心不平衡。在现实世界中,这种设置会导致箱子向右倾倒。但是,当您运行此代码(并按“s”启动)时,它会显示盒子向左倾倒。这是为什么?

我已经尝试调整质量(盒子的质量高,腿的质量低),盒子的重心,并调整关节的连接点,但似乎没有任何改变结果。我做错了什么?

我想用它来模拟真实世界的现象,但在我能用它来重现真实世界的现象之前,我被困住了。

请您参考如下方法:

好像是因为腿的形状和底部的地面形状发生了碰撞。

使它们不发生碰撞的最简单方法是将它们稍微分开一点。例如,让腿短一点,这样它就不会接触地面。

另一种解决方案是像您在其他问题中所做的那样,忽略腿与地面之间的碰撞。为此,您可以设置形状过滤器,但由于您可能希望防止盒子与腿碰撞,同时计算盒子与地面之间的碰撞,我认为您需要使用形状的类别/掩码此处记录的过滤器:http://www.pymunk.org/en/latest/pymunk.html#pymunk.ShapeFilter