Модель серво в симуляторе физики
13.01.2021, 16:38. Показов 1790. Ответов 1
В чем можно сделать модель сервопривода чтобы управлять углом отрезка с учетом 2д физики? Это нужно для управления лапами 2д шагалки. Пока читаю про pymunk. Нужно будет в коде задавать угол между двумя отрезками а симулятор физики поворачивает одно звено относительно другого до достижения угла. По идее тут нельзя задавать угол поворотом звена напрямую, нужно делать это создавая вращательный момент или силу до момента достижения заданного угла. Еще не понятен момент тут по идее будут колебания положения т.к. это симулятор физики и нужно будет встраивать PID регулятор? Или это как-то упрощено? Еще нужно получать некоторое значение работы или энергии затраченной на поворот. Не вижу примера кода сервопривода нигде.
Вот найден странный пример некий костыль, угол между 2 отрезками.
https://coderoad.ru/49594143/С... ние-Pymunk
| Python | 1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
| 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() |
|
Вот еще тут выдает кучу ошибок, пример по идее более продвинутый кирпич с лапками.
https://stackoverflow.com/ques... ts-at-will
| Python | 1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
| import sys
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
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):
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)
def to_pygame(p):
return int(p.x), int(-p.y+height) #Small hack to convert pymunk to pygame coordinates
def from_pygame(p):
return to_pygame(p)
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+100)
chWd = 70; chHt = 50
chassisMass = 10
legWd_a = 50; legHt_a = 5
legWd_b = 100; legHt_b = 5
legMass = 1
relativeAnguVel = 0
#---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
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
#---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
#---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
#---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
#---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.SimpleMotor(leftLeg_1b_body, leftLeg_1a_body, relativeAnguVel)
#---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.SimpleMotor(leftLeg_1a_body, chassis_b, relativeAnguVel)
#---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.SimpleMotor(rightLeg_1b_body, rightLeg_1a_body, relativeAnguVel)
#---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.SimpleMotor(rightLeg_1a_body, chassis_b, relativeAnguVel)
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 = False
rotationRate = 2
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_ba1Left.rate = rotationRate
elif event.type == KEYDOWN and event.key == K_DOWN:
motor_ba1Left.rate = -rotationRate
elif event.type == KEYDOWN and event.key == K_LEFT:
motor_ac1Left.rate = rotationRate
elif event.type == KEYDOWN and event.key == K_RIGHT:
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() |
|
Почему это тут так сложно и костыльно?
Может нужно взять другой физ движок буллет или там box2d?
Или это просто реализовать тут в пуманк?
0
|
(
