pymunk
pymunk copied to clipboard
viblo
Can not set max_force od pymunk.constraints.DampedRotarySpring
I try to limit the torque that produce from damped rotary spring by setting the property "max_force" but clearly it doesn't work. Seem like the torque only take stiffness and damping into the account and doesn't limited by max_force's property.
This is my code, and you can see my robot still stood-up no mater how low max_force is.
import sys
import numpy as np
import pygame
from pygame.locals import USEREVENT, QUIT, KEYDOWN, KEYUP, K_s, K_r, K_q, K_ESCAPE, K_UP, K_DOWN, K_RIGHT, K_LEFT, K_x
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 = (1000, 800)
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, (0, self.ground_y), (1000, self.ground_y), 5.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):
self.screen.fill(THECOLORS["white"]) ### Clear the screen
self.space.debug_draw(self.draw_options) ### Draw space
pygame.display.flip() ### All done, lets flip the display
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)
pymunk.pygame_util.positive_y_is_up = True
clock = pygame.time.Clock()
running = True
font = pygame.font.Font(None, 16)
# Create the spider
chassisXY = Vec2d(self.display_size[0] / 2, self.ground_y + 200)
chWd = 70
chHt = 50
chassisMass = 10
legWd_a = 75
legHt_a = 5
legWd_b = 75
legHt_b = 5
legMass = 1
relativeAnguVel = 0
motor_max_force = np.inf
motor_max_rate = 5
motor_p_gain = 100
# ---chassis
chassis_b = pymunk.Body(chassisMass, pymunk.moment_for_box(chassisMass, (chWd, chHt)))
chassis_b.position = chassisXY
chassis_shape = pymunk.Poly.create_box(chassis_b, (chWd, chHt))
chassis_shape.color = 200, 200, 200, 100
chassis_shape.friction = 0.5
print("chassis position")
print(chassis_b.position)
# ---first left leg a
leftLeg_1a_body = pymunk.Body(legMass, pymunk.moment_for_box(legMass, (legWd_a, legHt_a)))
leftLeg_1a_body.position = chassisXY - ((chWd / 2) + (legWd_a / 2), 0)
leftLeg_1a_shape = pymunk.Poly.create_box(leftLeg_1a_body, (legWd_a, legHt_a))
leftLeg_1a_shape.color = 255, 0, 0, 100
leftLeg_1a_shape.friction = 1
# ---first left leg b
leftLeg_1b_body = pymunk.Body(legMass, pymunk.moment_for_box(legMass, (legWd_b, legHt_b)))
leftLeg_1b_body.position = leftLeg_1a_body.position - ((legWd_a / 2) + (legWd_b / 2), 0)
leftLeg_1b_shape = pymunk.Poly.create_box(leftLeg_1b_body, (legWd_b, legHt_b))
leftLeg_1b_shape.color = 0, 255, 0, 100
leftLeg_1b_shape.friction = 1
# ---first right leg a
rightLeg_1a_body = pymunk.Body(legMass, pymunk.moment_for_box(legMass, (legWd_a, legHt_a)))
rightLeg_1a_body.position = chassisXY + ((chWd / 2) + (legWd_a / 2), 0)
rightLeg_1a_shape = pymunk.Poly.create_box(rightLeg_1a_body, (legWd_a, legHt_a))
rightLeg_1a_shape.color = 255, 0, 0, 100
rightLeg_1a_shape.friction = 1
# ---first right leg b
rightLeg_1b_body = pymunk.Body(legMass, pymunk.moment_for_box(legMass, (legWd_b, legHt_b)))
rightLeg_1b_body.position = rightLeg_1a_body.position + ((legWd_a / 2) + (legWd_b / 2), 0)
rightLeg_1b_shape = pymunk.Poly.create_box(rightLeg_1b_body, (legWd_b, legHt_b))
rightLeg_1b_shape.color = 0, 255, 0, 100
rightLeg_1b_shape.friction = 1
leg_stiffness = 10000000
leg_damping = 200000
# ---link left leg b with left leg a
pj_ba1left = pymunk.PinJoint(leftLeg_1b_body, leftLeg_1a_body, (legWd_b / 2, 0),
(-legWd_a / 2, 0)) # anchor point coordinates are wrt the body; not the space
motor_ba1Left = pymunk.DampedRotarySpring(leftLeg_1b_body, leftLeg_1a_body, rest_angle=np.pi/2,stiffness=leg_stiffness,damping=leg_damping)
# ---link left leg a with chassis
pj_ac1left = pymunk.PinJoint(leftLeg_1a_body, chassis_b, (legWd_a / 2, 0), (-chWd / 2, 0))
motor_ac1Left = pymunk.DampedRotarySpring(leftLeg_1a_body, chassis_b, rest_angle=np.pi/6,stiffness=leg_stiffness,damping=leg_damping)
# ---link right leg b with right leg a
pj_ba1Right = pymunk.PinJoint(rightLeg_1b_body, rightLeg_1a_body, (-legWd_b / 2, 0),
(legWd_a / 2, 0)) # anchor point coordinates are wrt the body; not the space
motor_ba1Right = pymunk.DampedRotarySpring(rightLeg_1b_body, rightLeg_1a_body, rest_angle=-np.pi/2,stiffness=leg_stiffness,damping=leg_damping)
# ---link right leg a with chassis
pj_ac1Right = pymunk.PinJoint(rightLeg_1a_body, chassis_b, (-legWd_a / 2, 0), (chWd / 2, 0))
motor_ac1Right = pymunk.DampedRotarySpring(rightLeg_1a_body, chassis_b, rest_angle=-np.pi/6,stiffness=leg_stiffness,damping=leg_damping)
self.space.add(chassis_b, chassis_shape)
self.space.add(leftLeg_1a_body, leftLeg_1a_shape, rightLeg_1a_body, rightLeg_1a_shape)
self.space.add(leftLeg_1b_body, leftLeg_1b_shape, rightLeg_1b_body, rightLeg_1b_shape)
self.space.add(pj_ba1left, motor_ba1Left, pj_ac1left, motor_ac1Left)
self.space.add(pj_ba1Right, motor_ba1Right, pj_ac1Right, motor_ac1Right)
# ---prevent collisions with ShapeFilter
shape_filter = pymunk.ShapeFilter(group=1)
chassis_shape.filter = shape_filter
leftLeg_1a_shape.filter = shape_filter
rightLeg_1a_shape.filter = shape_filter
leftLeg_1b_shape.filter = shape_filter
rightLeg_1b_shape.filter = shape_filter
simulate = True
rotationRate = 2
motor_ba1Left.max_force = 1
motor_ba1Right.max_force = 1
motor_ac1Left.max_force = 1
motor_ac1Right.max_force = 1
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
print('reset')
self.reset_bodies()
elif event.type == KEYDOWN and event.key == K_RIGHT:
motor_ba1Left.rate = rotationRate
elif event.type == KEYDOWN and event.key == K_LEFT:
motor_ba1Left.rate = -rotationRate
elif event.type == KEYDOWN and event.key == K_DOWN:
motor_ac1Left.rate = rotationRate
elif event.type == KEYDOWN and event.key == K_UP:
motor_ac1Left.rate = -rotationRate
elif event.type == KEYUP:
motor_ba1Left.rate = 0
motor_ac1Left.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()
Ah, yes you are right. Max force does not work on the DampedSpring and DampedRotarySpring.
Im not sure why.. when I look at the code it looks quite straight forward to have it. On the other hand, in Chipmunk (the underlying c-library which does the actual simulation), there's the possibility to override the whole torque calculation function.. so in that way more or less anything could be done.. (the default is (relative_angle - rest_angle)*stiffness). However, I never included it in the Pymunk API so its not easy for you to use.
I think a couple of things should be done:
- Add the possibility to customize the force functions also from Pymunk.
- One of a and b: a. Warn in the documentation that maxForce doesnt work (maybe also prevent it from being used on these two constraints). b. Make max_force work on the two constraints.
(I created an issue on the Chipmunk issue tracker to see if there's some special reasoning behind this..)
I should add Im always a bit hesitant to make changes to Chipmunk (making max_force work) since then the fork used by Pymunk diverges more and more from the upstream, making it more and more work to port over any changes from upstream etc. Im also not that skilled in writing c-code or the math behind the simulation.
Alright, I understand. I would conclude the point and close the issue as it not relavent to the wrap-up python library.
- It become clear that this is limitaton due to chipmunk base library.
- We should show this limitation on the pymunk documentation
- We will keep track on this problem by looking for update from chipmunk team.
- We should provide work around solution for people that required this feature.
This is my work around solution base on the same idea
target_pos = target_pos_list[motor_idx]
force = motor.stiffness*(target_pos - self.get_motor_angle(motor))
if abs(force) > self.motor_max_force:
target_pos = self.motor_max_force/motor.stiffness - self.get_motor_angle(motor)
motor.rest_angle = target_pos
I think its best to keep this opened, otherwise I will forget to update docs / fix it :) I will close once Ive implemented something.
The last couple of days I have had a chance to look at this again. There's one question that came up: Should the velocity loss from damping be included in the limit on max force? At least for a damped spring the total impulse (the "main" force + force from damping) can be bigger than just the main force. (The damping force is included in the spring.impulse property)
Hi there. For me, it's okay whatsoever. Considered what I was trying to do all along. Seem like it original lib, spring, and damper are passive components that should not be able to limit the producing force-torque by real-world analogy. Personally, as I use a spring-damper as a PD controller we normally clamp the result control input afterward therefore it equivalent to limit the total force produced from both spring and damper. BTW, I checked the issue you opened in the Chipmunk repo. seem like they not response and inactive. what are we gonna do next?
Yeah, I'm leaning towards including both, mainly to make it consistent with the constraint.impulse property. For someone without much physics I think it make sense at least, from an api point of view. And from an ideal spring point I guess both are equally bad.
I will make a fix for this shortly, that will also allow overriding the spring force function which is currently not exposed from pymunk api. (And fix a bug I noticed in the rotary spring while researching this, it seems the damping correction is added to total impulse with wrong sign (simulation is ok, just the value of spring.impulse is not correct)