Logo Questions Linux Laravel Mysql Ubuntu Git Menu
 

Balance box on wheels with springs (lock rotation relative to box) pymunk

I want to be able to lock the angle of the wheels relative to the car's chassis. In between the wheels, there are springs, that should allow the car to suspend, but right now, the angle is not locked. I am using pymunk's function "RotaryLimitJoint"

A behavior like this is the goal (gif)

Wanted behavior

Right now it looks like this:

Current state

My code:

car_pos = Vec2d(100,500)
mass = 30
radius = 10
moment = pymunk.moment_for_circle(mass, 20, radius)
wheel1_b = pymunk.Body(mass, moment)
wheel1_s = pymunk.Circle(wheel1_b, radius)
wheel1_s.friction = 1.5
wheel1_s.color = wheel_color
space.add(wheel1_b, wheel1_s)

mass = 30
radius = 10
moment = pymunk.moment_for_circle(mass, 20, radius)
wheel2_b = pymunk.Body(mass, moment)
wheel2_s = pymunk.Circle(wheel2_b, radius)
wheel2_s.friction = 1.5
wheel2_s.color = wheel_color
space.add(wheel2_b, wheel2_s)

mass = 100
size = (80,25)
moment = pymunk.moment_for_box(mass, size)
chassi_b = pymunk.Body(mass, moment)
chassi_s = pymunk.Poly.create_box(chassi_b, size)
chassi_s.color = chassi_color
space.add(chassi_b, chassi_s)


#Positions
chassi_b.position = car_pos + (0,-15)
wheel1_b.position = car_pos + (-25,0)
wheel2_b.position = car_pos + (25,0)

#Joints

spring1 = pymunk.DampedSpring(chassi_b, wheel1_b, (-25,0), (0,0), 20, 100000, 1)
spring1.collide_bodies = False
spring2 = pymunk.DampedSpring(chassi_b, wheel2_b, (25,0), (0,0), 20, 100000, 1)
spring2.collide_bodies = False

wheelAngle1 = pymunk.RotaryLimitJoint(wheel1_b, chassi_b, 0, 0)
wheelAngle1.collide_bodies = False
wheelAngle2 = pymunk.RotaryLimitJoint(chassi_b, wheel2_b, 0, 0)
wheelAngle2.collide_bodies = False



space.add(
    spring1,
    spring2,
    wheelAngle1,
    wheelAngle2
)
speed = 20

space.add(
    pymunk.SimpleMotor(wheel1_b, chassi_b, speed),
    pymunk.SimpleMotor(wheel2_b, chassi_b, speed)
)
like image 619
Andreas G.D Petersen Avatar asked May 05 '20 09:05

Andreas G.D Petersen


2 Answers

First off credits to @viblo.

What makes the following code work, is that the GrooveJoint (see docs) is created perpendicular to the car's chassis. The GrooveJoint is defining a line, where a body is free to slide on. Defining the GrooveJoint it is attached to the car's chassis and to the wheel (for both the front and back wheel).

It looks like this now:

Working gif

I converted the code (from @viblo) to python and here it is:

def car(space, speed, add_car):
    car_pos = Vec2d(100,500)

    #bodies
    wheel_color = 0,0,0
    chassi_color = 255,0,0
    wheelCon_color = 0,255,255

    #Wheel 1
    mass = 25
    radius = 10
    moment = pymunk.moment_for_circle(mass, 20, radius)
    wheel1_b = pymunk.Body(mass, moment)
    wheel1_s = pymunk.Circle(wheel1_b, radius)
    wheel1_s.friction = 1.5
    wheel1_s.color = wheel_color

    #Wheel 2
    mass = 25
    radius = 10
    moment = pymunk.moment_for_circle(mass, 20, radius)
    wheel2_b = pymunk.Body(mass, moment)
    wheel2_s = pymunk.Circle(wheel2_b, radius)
    wheel2_s.friction = 1.5
    wheel2_s.color = wheel_color

    #Chassi
    mass = 30
    size = (80,25)
    moment = pymunk.moment_for_box(mass, size)
    chassi_b = pymunk.Body(mass, moment)
    chassi_s = pymunk.Poly.create_box(chassi_b, size)
    chassi_s.color = chassi_color

    #Positions
    chassi_b.position = car_pos + (0,-15)
    wheel1_b.position = car_pos + (-25,0)
    wheel2_b.position = car_pos + (25,0)

    #Joints
    spring1 = pymunk.constraint.DampedSpring(chassi_b, wheel1_b, (-25,0), (0,0), 15, 5000, 250)
    spring1.collide_bodies = False
    spring2 = pymunk.constraint.DampedSpring(chassi_b, wheel2_b, (25,0), (0,0), 15, 5000, 250)
    spring2.collide_bodies = False

    groove1 = pymunk.constraint.GrooveJoint(chassi_b, wheel1_b, (-25,0), (-25,25), (0, 0))
    groove1.collide_bodies = False
    groove2 = pymunk.constraint.GrooveJoint(chassi_b, wheel2_b, (25,0), (25,25), (0,0))
    groove2.collide_bodies = False

    if add_car:
        motor1 = pymunk.SimpleMotor(wheel1_b, chassi_b, speed)
        motor2 = pymunk.SimpleMotor(wheel2_b, chassi_b, speed)
        space.add(
            spring1,
            spring2,
            groove1,
            groove2,
            motor1,
            motor2,
            chassi_b, 
            chassi_s,
            wheel2_b,
            wheel2_s,
            wheel1_b,
            wheel1_s
        )
like image 175
Andreas G.D Petersen Avatar answered Oct 21 '22 18:10

Andreas G.D Petersen


The RotaryLimitJoint is not what you need. It will constrain the angle between the wheel and chassis, but the wheel needs to rotate so it wont work.

What you can try with instead is a GrooveJoint.

This is how the code looks like in c code, it should be fairly easy to convert it to python:

Full source: https://github.com/slembcke/Chipmunk2D/blob/master/demo/Joints.c#L263

The relevant part:

boxOffset = cpv(0, 0);
cpBody *wheel1 = addWheel(space, posA, boxOffset);
cpBody *wheel2 = addWheel(space, posB, boxOffset);
cpBody *chassis = addChassis(space, cpv(80, 100), boxOffset);

cpSpaceAddConstraint(space, cpGrooveJointNew(chassis, wheel1, cpv(-30, -10), cpv(-30, -40), cpvzero));
cpSpaceAddConstraint(space, cpGrooveJointNew(chassis, wheel2, cpv( 30, -10), cpv( 30, -40), cpvzero));

cpSpaceAddConstraint(space, cpDampedSpringNew(chassis, wheel1, cpv(-30, 0), cpvzero, 50.0f, 20.0f, 10.0f));
cpSpaceAddConstraint(space, cpDampedSpringNew(chassis, wheel2, cpv( 30, 0), cpvzero, 50.0f, 20.0f, 10.0f));

If you cant figure it out I can try to convert it to python, but unfortunately Im not on a computer with Python & Pymunk setup for that now.

like image 2
viblo Avatar answered Oct 21 '22 18:10

viblo