# Add some pin joints to hold the circles in place. space.add(pymunk.PivotJoint(b1, space.static_body, (50, 100) + Vec2d(*box_offset))) space.add(pymunk.PivotJoint(b2, space.static_body, (150, 100) + Vec2d(*box_offset))) # Ratchet every 90 degrees c = pymunk.RatchetJoint(b1, b2, 0, math.pi / 2) txts[box_offset] = inspect.getdoc(c) space.add(c) box_offset = box_size * 2, box_size b1 = add_bar(space, (50, 100), box_offset) b2 = add_bar(space, (150, 100), box_offset) # Add some pin joints to hold the circles in place. space.add(pymunk.PivotJoint(b1, space.static_body, (50, 100) + Vec2d(*box_offset))) space.add(pymunk.PivotJoint(b2, space.static_body, (150, 100) + Vec2d(*box_offset))) # Force one to sping 2x as fast as the other c = pymunk.GearJoint(b1, b2, 0, 2) txts[box_offset] = inspect.getdoc(c) space.add(c) box_offset = box_size * 3, box_size b1 = add_bar(space, (50, 100), box_offset) b2 = add_bar(space, (150, 100), box_offset) # Add some pin joints to hold the circles in place. space.add(pymunk.PivotJoint(b1, space.static_body, (50, 100) + Vec2d(*box_offset))) space.add(pymunk.PivotJoint(b2, space.static_body, (150, 100) + Vec2d(*box_offset))) # Make them spin at 1/2 revolution per second in relation to each other. c = pymunk.SimpleMotor(b1, b2, math.pi) txts[box_offset] = inspect.getdoc(c) space.add(c) # TODO add one or two advanced constraints examples, such as a car or rope
def main():
N = int(DURATION / DT) + 1
space = pymunk.Space()
space.gravity = (0, 0)
box_body = pymunk.Body()
box_body.position = (1, 0.3)
box_r = 0.5
box_corners = [(-box_r, box_r), (-box_r, -box_r), (box_r, -box_r),
(box_r, box_r)]
box = pymunk.Poly(box_body, box_corners, radius=0.01)
# box = pymunk.Circle(box_body, 0.5)
box.mass = M
box.friction = 0.5
box.collision_type = 1
space.add(box.body, box)
# linear friction
pivot = pymunk.PivotJoint(space.static_body, box.body, box.body.position)
pivot.max_force = MU * M * G # μmg
pivot.max_bias = 0 # disable correction so that body doesn't bounce back
space.add(pivot)
# angular friction
gear = pymunk.GearJoint(space.static_body, box.body, 0, 1)
gear.max_force = 2
gear.max_bias = 0
space.add(gear)
control_body = pymunk.Body(body_type=pymunk.Body.KINEMATIC)
control_body.position = (0, 0)
control_shape = pymunk.Circle(control_body, 0.1, (0, 0))
control_shape.friction = 1
control_shape.collision_type = 1
space.add(control_shape.body, control_shape)
watcher = CollisionWatcher()
space.add_collision_handler(1, 1).post_solve = watcher.update
plt.ion()
fig = plt.figure()
ax = plt.gca()
ax.set_xlabel('x (m)')
ax.set_ylabel('y (m)')
ax.set_xlim([-5, 5])
ax.set_ylim([-3, 3])
ax.grid()
ax.set_aspect('equal')
options = pymunk.matplotlib_util.DrawOptions(ax)
options.flags = pymunk.SpaceDebugDrawOptions.DRAW_SHAPES
space.debug_draw(options)
fig.canvas.draw()
fig.canvas.flush_events()
control_body.velocity = (0.25, 0)
for i in range(N - 1):
t = i * DT
# step the sim
space.step(DT)
if i % PLOT_PERIOD == 0:
ax.cla()
ax.set_xlim([-5, 5])
ax.set_ylim([-3, 3])
ax.grid()
space.debug_draw(options)
fig.canvas.draw()
fig.canvas.flush_events()
def testPhase(self):
a,b = p.Body(10,10), p.Body(20,20)
j = p.GearJoint(a, b, 1, 0)
self.assertEqual(j.phase, 1)
j.phase = 2
self.assertEqual(j.phase, 2)
def testRatio(self):
a,b = p.Body(10,10), p.Body(20,20)
j = p.GearJoint(a, b, 0, 1)
self.assertEqual(j.ratio, 1)
j.ratio = 2
self.assertEqual(j.ratio, 2)
def __init__(self, name, context, pos, color, manager, width= 2.5, length=2.5, mass = 120, maxForce = 400, maxTorque = 20000, maxSpeed = 30, maxAngSpeed = 1): #force in lbs
self.name = name
self.context = context
self.pPickUp = dict.fromkeys(RET_NAMES) # probability of picking up retrievable
self.tPickUp = dict.fromkeys(RET_NAMES) # avg time spent picking up retrievable
self.maxPickUp = dict.fromkeys(RET_NAMES) # max # of rets in possession
self.stPickUp = dict.fromkeys(RET_NAMES) # std dev of pickup time?
self.scores = dict.fromkeys(SCORE_NAMES) # attainable scores in scorespots
self.pScores = dict.fromkeys(SCORE_NAMES) # probability of getting score at scorespots
self.tScores = dict.fromkeys(SCORE_NAMES) # avg time spent scoring at scorespot (seconds)
self.stScores = dict.fromkeys(SCORE_NAMES) # std dev of score time?
self.width = width * SCALE
self.length = length * SCALE
self.mass=mass
self.pos = pos * SCALE
self.maxSpeed = maxSpeed * SCALE
self.maxAngSpeed = maxAngSpeed * SCALE
self.angFriction = maxTorque * SCALE * 9
self.maxForce = maxForce * SCALE
self.force = Vec2d(0,0)
self.maxTorque = maxTorque * SCALE
self.torque = 0
self.color = color
self.scale = SCALE
# create body
points = [(-self.width/2, -self.length/2),(-self.width/2,self.length/2),(self.width/2,self.length/2),(self.width/2,-self.length/2)]
self.inertia = pymunk.moment_for_poly(self.mass,points)
self.body = pymunk.Body(self.mass, self.inertia)
self.body.position = self.pos
# setup control body
self.control = pymunk.Body(pymunk.inf, pymunk.inf, body_type = pymunk.cp.CP_BODY_TYPE_KINEMATIC)
self.control.position = self.body.position
self.controlPivot = pymunk.PivotJoint(self.body, self.control,(0,0),(0,0))
self.controlPivot.max_bias = 0
self.controlPivot.max_force = 100 * self.maxForce
self.controlGear = pymunk.GearJoint(self.body, self.control, 0, 1)
self.controlGear.max_bias = 0
self.controlGear.max_force = 250 * self.maxTorque
#create shape
self.shape = pymunk.Poly(self.body,points)
self.shape.elasticity = 0.95
self.shape.friction = 1000
self.shape.color = pygame.color.THECOLORS[self.color]
if color is "blue":
self.score = self.context.blueScore
self.enemyScore = self.context.redScore
self.multiplier = -1
else:
self.score = self.context.redScore
self.enemyScore = self.context.blueScore
self.multiplier = 1
self.VisField = SensorField("VisField",self.context,self.body, self.multiplier * 10, self.multiplier * 15, owner = self)# make variables for vis field dims
self.RetField = SensorField("RetField", self.context, self.body, self.multiplier * 2, self.multiplier * 10, owner = self)
self.shape.collision_type = collision_types[self.name]
self.context.objects[self.shape._get_shapeid()] = self
self.Randomize()
self.damping = 1
self.friction = maxForce * SCALE * 6
self.canPickup = False
self.canScore = False
self.immobileTime = 0
self.inputs = []
self.RNNInputs = []
self.logits = []
self.actions = []
self.teamScores =[]
self.objectList = []
self.rets = defaultdict(lambda:[])
self.prev = 0 # team's score 1 second ago
self.numObstacles = 0
def setup(self, *args, **kwargs) -> None:
super().setup(*args, **kwargs)
# Physics. This joint setup was taken form tank.py in the pymunk
# examples.
if self.shape_type == ShapeType.SQUARE:
side_len = math.sqrt(math.pi) * self.shape_size
shapes = self._make_square(side_len)
elif self.shape_type == ShapeType.CIRCLE:
shapes = self._make_circle()
elif self.shape_type == ShapeType.STAR:
star_npoints = 5
star_out_rad = 1.3 * self.shape_size
star_in_rad = 0.5 * star_out_rad
shapes, convex_parts = self._make_star(star_npoints, star_out_rad,
star_in_rad)
else:
# These are free-form shapes b/c no helpers exist in Pymunk.
try:
factor, num_sides = POLY_TO_FACTOR_SIDE_PARAMS[self.shape_type]
except KeyError:
raise NotImplementedError("haven't implemented",
self.shape_type)
side_len = factor * gtools.regular_poly_circ_rad_to_side_length(
num_sides, self.shape_size)
shapes, poly_verts = self._make_regular_polygon(
num_sides, side_len)
for shape in shapes:
shape.friction = 0.5
self.add_to_space(shape)
trans_joint = pm.PivotJoint(self.space.static_body, self.shape_body,
(0, 0), (0, 0))
trans_joint.max_bias = 0
trans_joint.max_force = self.phys_vars.shape_trans_joint_max_force
self.add_to_space(trans_joint)
rot_joint = pm.GearJoint(self.space.static_body, self.shape_body, 0.0,
1.0)
rot_joint.max_bias = 0
rot_joint.max_force = self.phys_vars.shape_rot_joint_max_force
self.add_to_space(rot_joint)
# Graphics.
geoms_outer = []
if self.shape_type == ShapeType.SQUARE:
geoms = [r.make_square(side_len, outline=True)]
elif self.shape_type == ShapeType.CIRCLE:
geoms = [r.make_circle(self.shape_size, 100, True)]
elif self.shape_type == ShapeType.STAR:
star_short_verts = gtools.compute_star_verts(
star_npoints,
star_out_rad - SHAPE_LINE_THICKNESS,
star_in_rad - SHAPE_LINE_THICKNESS,
)
short_convex_parts = autogeom.convex_decomposition(
star_short_verts + star_short_verts[:1], 0)
geoms = []
for part in short_convex_parts:
geoms.append(r.Poly(part, outline=False))
geoms_outer = []
for part in convex_parts:
geoms_outer.append(r.Poly(part, outline=False))
elif (self.shape_type == ShapeType.OCTAGON
or self.shape_type == ShapeType.HEXAGON
or self.shape_type == ShapeType.PENTAGON
or self.shape_type == ShapeType.TRIANGLE):
geoms = [r.Poly(poly_verts, outline=True)]
else:
raise NotImplementedError("haven't implemented", self.shape_type)
if self.shape_type == ShapeType.STAR:
for g in geoms_outer:
g.color = darken_rgb(self.color)
for g in geoms:
g.color = self.color
else:
for g in geoms:
g.color = self.color
g.outline_color = darken_rgb(self.color)
self.shape_xform = r.Transform()
shape_compound = r.Compound(geoms_outer + geoms)
shape_compound.add_transform(self.shape_xform)
self.viewer.add_geom(shape_compound)
def stick_arrow_to_target(arrow_body, target_body, position, space):
pivot_joint = pymunk.PivotJoint(arrow_body, target_body, position)
phase = target_body.angle - arrow_body.angle
gear_joint = pymunk.GearJoint(arrow_body, target_body, phase, 1)
space.add(pivot_joint)
space.add(gear_joint)
def run_once(self, keep=False, start=False, verbose=False, **kwargs):
pygame.init()
if not self.headless:
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
# box_width = 75
# box_height = 200
# box_width = 200
# box_height = 100
leg_length = 100
# leg_length = 125
leg_thickness = 2
leg_shape_filter = pymunk.ShapeFilter(group=LEG_GROUP)
# Create torso.
torso_mass = 500
torso_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(torso_mass, torso_points)
body1 = pymunk.Body(torso_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
body1.center_of_gravity = (0, 0) # centered
# body1.center_of_gravity = (0, box_height/2) # top-heavy
# body1.center_of_gravity = (box_width/2, 0) # right-heavy
shape1 = pymunk.Poly(body1, torso_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.
leg_mass = 1
leg_points = [
(leg_thickness/2, -leg_length/2),
(-leg_thickness/2, -leg_length/2),
# (-leg_thickness/2, leg_length/2-leg_thickness),
# (leg_thickness/2, leg_length/2-leg_thickness/2)
(-leg_thickness/2, leg_length/2),
(leg_thickness/2, leg_length/2)
]
leg_moment = pymunk.moment_for_poly(leg_mass, leg_points)
body2 = pymunk.Body(leg_mass, leg_moment)
if self.leg_position == CORNER:
# Position leg at far corner of torso.
body2.position = (self.display_size[0]/2-box_width/2+leg_thickness/2, self.ground_y+leg_length/2)
elif self.leg_position == OFFSET:
# Position leg near center of torso, but still offset to create slight instability.
body2.position = (self.display_size[0]/2-leg_thickness/2, self.ground_y+leg_length/2)
elif self.leg_position == CENTER:
body2.position = (self.display_size[0]/2, self.ground_y+leg_length/2)
else:
raise NotImplementedError
body2.start_position = Vec2d(body2.position)
body2.start_angle = body2.angle
shape2 = pymunk.Poly(body2, leg_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-leg_thickness))
if self.leg_position == CORNER:
pj = pymunk.PivotJoint(body1, body2, (self.display_size[0]/2-box_width/2+leg_thickness/2, self.ground_y+leg_length)) # far corner
elif self.leg_position == OFFSET:
pj = pymunk.PivotJoint(body1, body2, (self.display_size[0]/2-leg_thickness/2, self.ground_y+leg_length)) # near center
elif self.leg_position == CENTER:
pj = pymunk.PivotJoint(body1, body2, (self.display_size[0]/2, self.ground_y+leg_length)) # near center
else:
raise NotImplementedError
# pj = pymunk.PivotJoint(body1, body2, Vec2d(body2.position))
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.
if self.leg_position == CORNER:
pj = pymunk.PivotJoint(self.space.static_body, body2, (self.display_size[0]/2-box_width/2+leg_thickness/2, self.ground_y+leg_thickness)) # far
elif self.leg_position == OFFSET:
pj = pymunk.PivotJoint(self.space.static_body, body2, (self.display_size[0]/2-leg_thickness/2, self.ground_y+leg_thickness)) # off center
elif self.leg_position == CENTER:
pj = pymunk.PivotJoint(self.space.static_body, body2, (self.display_size[0]/2, self.ground_y+leg_thickness)) # center
else:
raise NotImplementedError
self.space.add(pj)
# Actuate the bars via a motor.
motor_joint = None
motor_joint = pymunk.SimpleMotor(body1, body2, 0)
# motor_joint.max_force = 1e10 # mimicks default infinity, too strong, breaks rotary limit joints
motor_joint.max_force = 1e9
# motor_joint.max_force = 1e7 # too weak, almost no movement
self.space.add(motor_joint)
# Simulate friction on the foot joint.
# https://chipmunk-physics.net/forum/viewtopic.php?f=1&t=2916
foot_gj = pymunk.GearJoint(self.space.static_body, body2, 0.0, 1.0)
foot_gj.max_force = 1e8
self.space.add(foot_gj)
# Simulate friction on the hip joint.
# https://chipmunk-physics.net/forum/viewtopic.php?f=1&t=2916
# hip_gj = pymunk.GearJoint(body1, body2, 0.0, 1.0)
# hip_gj.max_force = 1e10
# self.space.add(hip_gj)
# Add hard stops to leg pivot so the leg can't rotate through the torso.
angle_range = pi/4. # 45 deg
# angle_range = pi/2. # 90 deg
hip_limit_joint = pymunk.RotaryLimitJoint(body1, body2, -angle_range, angle_range) # -45deg:+45deg
self.space.add(hip_limit_joint)
# default angle = 270/2 = 135 deg
servo = None
if motor_joint:
servo = ServoController(motor_joint, max_rate=20.0)
servo.set_degrees(135)
# 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)
self.reset()
last_body1_pos = None
last_body1_vel = None
simulate = False
self.cnt = 0
failed = None
simulate = start
current_state = None
# The point at which the torso has toppled over.
failure_degrees = 45
while running:
self.cnt += 1
# 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))
# Angle of the torso, where 0 degrees is the torso perfectly parallel to the ground.
# A positive angle is clockwise rotation.
# This simulates the IMU's y euler angle measurement.
torso_angle = -body1.angle * 180/pi
# print('torso_angle:', torso_angle)
# Angle of the hip servo.
# A positive angle is clockwise rotation.
# This simulates the estimated hip servo potentiometer.
hip_angle = (body2.angle - body1.angle) * 180/pi # 0 degrees means leg is angled straight down
# Angle of the foot, where 0 degrees is the foot perfectly perpedicular to the ground.
# A positive angle is clockwise rotation.
foot_angle = -body2.angle * 180/pi
# print('foot_angle:', foot_angle)
# Re-frame angle so that 135 degrees indicates the straight down orientation.
hip_angle += 135
# print('hip_angle:', hip_angle)
current_state = [torso_angle/360., hip_angle/360., foot_angle/360.]
# assert not [_ for _ in current_state if abs(_) > 1]
# Auto-shift COG using foot angle as proportion of shift
# iters=163
# If we lean right, shift COG left, and vice versa.
# body1.center_of_gravity = (-box_width/2*foot_angle/90.*10, 0)
# cog_limit = box_width/2
# print('cog_limit:', cog_limit)
# print('foot_angle:', foot_angle)
# dist_from_center = sin(-body2.angle)*leg_length
# print('dist_from_center:', dist_from_center)
# max_dist_from_center = sin(pi/4)*leg_length
# print('max_dist_from_center:', max_dist_from_center)
# comp_rate = -dist_from_center/max_dist_from_center
# print('comp_rate:', comp_rate)
# gamma = 4
# comp_rate2 = exp(abs(comp_rate*gamma))
# if comp_rate < 0:
# comp_rate2 = -comp_rate2
# print('comp_rate2:', comp_rate2)
# comp_rate3 = max(min(comp_rate2, cog_limit), -cog_limit)
# print('comp_rate3:', comp_rate3)
# body1.center_of_gravity = (comp_rate3, 0)
if self.verbose:
print('foot.angular_velocity:', body2.angular_velocity) # positive=CCW, negative=CW
if isnan(float(body2.angular_velocity)):
print('Physics breakdown! Terminating simulation!', file=sys.stderr)
return
# Auto-shift COG using foot angular velocity direction.
# iters=403
servo.set_position(1500)
servo.attach()
# if body2.angular_velocity > 0:
# # falling left
# body1.center_of_gravity = (box_width/2, 0) # shift weight right
# else:
# # falling right
# body1.center_of_gravity = (-box_width/2, 0) # shift weight left
# Auto-shift COG using foot angular velocity magnitude.
# body1.center_of_gravity = (box_width/2*body2.angular_velocity/5*1, 0)# iters=158
# body1.center_of_gravity = (box_width/2*body2.angular_velocity/5*2, 0)# iters=161
# body1.center_of_gravity = (box_width/2*body2.angular_velocity/5*2.5, 0)# iters=157
# body1.center_of_gravity = (box_width/2*body2.angular_velocity/5*3, 0)# iters=160
# body1.center_of_gravity = (box_width/2*body2.angular_velocity/5*5, 0)# iters=153
# body1.center_of_gravity = (max(min(box_width/2*body2.angular_velocity*1, box_width/2), -box_width/2), 0)# iters=153
# body1.center_of_gravity = (max(min(box_width/2*body2.angular_velocity*2, box_width/2), -box_width/2), 0)# iters=135
# body1.center_of_gravity = (max(min(box_width/2*body2.angular_velocity*3, box_width/2), -box_width/2), 0)# iters=132
# body1.center_of_gravity = (max(min(box_width/2*body2.angular_velocity*100, box_width/2), -box_width/2), 0)# iters=290
# body1.center_of_gravity = (max(min(box_width/2*body2.angular_velocity*50, box_width/2), -box_width/2), 0)# iters=216
# body1.center_of_gravity = (max(min(box_width/2*body2.angular_velocity*25, box_width/2), -box_width/2), 0)# iters=278
# body1.center_of_gravity = (max(min(box_width/2*body2.angular_velocity*200, box_width/2), -box_width/2), 0)# iters=293
# body1.center_of_gravity = (max(min(box_width/2*body2.angular_velocity*1000, box_width/2), -box_width/2), 0)# iters=304
# body1.center_of_gravity = (max(min(box_width/2*body2.angular_velocity*10000, box_width/2), -box_width/2), 0)# iters=333
# body1.center_of_gravity = (max(min(box_width/2*body2.angular_velocity*100000, box_width/2), -box_width/2), 0)# iters=333
if self.verbose:
print('cog:', body1.center_of_gravity)
# Auto-shift hip angle to level.
#TODO
# Handle balancer agent input.
if self.balancer:
relative_position_change = self.balancer.get_action(state=current_state, actions=self.get_actions(self.balancer))
if self.verbose:
print('balancer action:', relative_position_change)
# Otherwise convert the relative +/- position into an absolute servo position.
hip_position = servo.degree_to_position(hip_angle)
new_hip_position = hip_position + relative_position_change
servo.set_position(new_hip_position)
# servo.set_position(1000)#manual
servo.attach()
# Handle shifter agent input.
if self.shifter:
shifter_state = [body2.angular_velocity/10.]
cog_shift = self.shifter.get_action(
state=list(shifter_state),
actions=[-box_width/2., -box_width/4., -box_width/16., -box_width/32., 0, +box_width/2., +box_width/4., +box_width/16., +box_width/32.])
body1.center_of_gravity = (max(min(cog_shift, box_width/2), -box_width/2), 0)
# Handle user input.
for event in pygame.event.get():
if event.type == QUIT or (event.type == KEYDOWN and event.key in (K_q, K_ESCAPE)):
running = False
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.
self.reset_bodies()
elif event.type == KEYDOWN and event.key == K_LEFT:
# Angle backwards by rotating torso about the hips counter-clockwise.
servo.set_degrees(servo.min_degrees)
servo.attach()
elif event.type == KEYDOWN and event.key == K_RIGHT:
# Angle forwarads by rotating torso about the hips clockwise.
servo.set_degrees(servo.max_degrees)
servo.attach()
elif event.type == KEYUP:
# Otherwise, make the servo go unpowered and idle.
servo.detach()
servo.update(current_degrees=hip_angle)
if not self.headless:
self.draw()
last_body1_pos = Vec2d(body1.position)
if current_body1_vel:
last_body1_vel = Vec2d(current_body1_vel)
### 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)
if not self.headless:
pygame.display.flip()
clock.tick(fps)
# Give balancer feedback.
# The balancer's objective is to keep the torso as level as possible, balanced on the foot.
failed = abs(foot_angle) > failure_degrees
if self.balancer:
# Feedback is the inverse distance of the torso's angle from center.
if failed:
# Terminal failure.
feedback = -1
else:
# Otherwise, grade performance based on how level the torso is.
feedback = valmap(abs(torso_angle), 0, 90, 1, -1)
# print('failed:', failed)
self.balancer.reinforce(feedback=feedback+self.cnt, state=current_state, end=failed)
if self.shifter:
if failed:
feedback = -1
else:
# feedback = 0
feedback = (90. - abs(foot_angle))/90.
self.shifter.reinforce(feedback=feedback, state=shifter_state, end=failed)
# Check failure criteria.
if not keep and (failed or self.cnt >= 1000):
break
print('Result: %s' % {True: 'failure', False:'aborted', None:'aborted'}[failed])
print('Iterations: %i' % self.cnt)
if self.balancer:
self.balancer.save()
if self.shifter:
self.shifter.save()
def init():
space = pymunk.Space()
space.iterations = 10
space.sleep_time_threshold = 0.5
static_body = space.static_body
# Create segments around the edge of the screen.
shape = pymunk.Segment(static_body, (1,1), (1,480), 1.0)
space.add(shape)
shape.elasticity = 1
shape.friction = 1
shape = pymunk.Segment(static_body, (640,1), (640,480), 1.0)
space.add(shape)
shape.elasticity = 1
shape.friction = 1
shape = pymunk.Segment(static_body, (1,1), (640,1), 1.0)
space.add(shape)
shape.elasticity = 1
shape.friction = 1
shape = pymunk.Segment(static_body, (1,480), (640,480), 1.0)
space.add(shape)
shape.elasticity = 1
shape.friction = 1
for _ in range(50):
body = add_box(space, 20, 1)
pivot = pymunk.PivotJoint(static_body, body, (0,0), (0,0))
space.add(pivot)
pivot.max_bias = 0 # disable joint correction
pivot.max_force = 1000 # emulate linear friction
gear = pymunk.GearJoint(static_body, body, 0.0, 1.0)
space.add(gear)
gear.max_bias = 0 # disable joint correction
gear.max_force = 5000 # emulate angular friction
# We joint the tank to the control body and control the tank indirectly by modifying the control body.
global tank_control_body
tank_control_body = pymunk.Body(body_type=pymunk.Body.KINEMATIC)
tank_control_body.position = 320, 240
space.add(tank_control_body)
global tank_body
tank_body = add_box(space, 30, 10)
tank_body.position = 320, 240
for s in tank_body.shapes:
s.color = (0,255,100,255)
pivot = pymunk.PivotJoint(tank_control_body, tank_body, (0,0), (0,0))
space.add(pivot)
pivot.max_bias = 0 # disable joint correction
pivot.max_force = 10000 # emulate linear friction
gear = pymunk.GearJoint(tank_control_body, tank_body, 0.0, 1.0)
space.add(gear)
gear.error_bias = 0 # attempt to fully correct the joint each step
gear.max_bias = 1.2 # but limit it's angular correction rate
gear.max_force = 50000 # emulate angular friction
return space
def __init__(self, space: pymunk.Space, theta: int = 0):
self.space = space
# Define initial angle
angle = Angle(theta)
# Define bodies
self.top = pymunk.Body(50, 10000, pymunk.Body.STATIC)
self.bottom = pymunk.Body(50, 10000, pymunk.Body.STATIC)
self.rod1 = pymunk.Body(5, 10000)
self.rod2 = pymunk.Body(5, 10000)
self.rod3 = pymunk.Body(5, 10000)
self.seat = pymunk.Body(10, 10000)
self.torso = pymunk.Body(10, 10000)
self.legs = pymunk.Body(10, 10000)
self.head = pymunk.Body(5, 10000)
# Create shapes
self.top_shape = pymunk.Poly.create_box(self.top, size=(1200, 50))
self.bottom_shape = pymunk.Poly.create_box(self.bottom,
size=(1200, 50))
self.rod1_shape = pymunk.Segment(self.rod1, (0, 0),
angle.rod1,
radius=3)
self.rod2_shape = pymunk.Segment(self.rod2, (0, 0),
angle.rod2,
radius=3)
self.rod3_shape = pymunk.Segment(self.rod3, (0, 0),
angle.rod3,
radius=3)
self.seat_shape = pymunk.Segment(self.seat,
angle.seat[0],
angle.seat[1],
radius=5)
self.torso_shape = pymunk.Poly.create_box(self.torso,
size=(10, torso_length))
self.legs_shape = pymunk.Poly.create_box(self.legs,
size=(10, legs_length))
self.head_shape = pymunk.Circle(self.head, radius=20)
# Set layer of shapes (disables collisions between bodies)
self.rod1_shape.filter = pymunk.ShapeFilter(
categories=0b100, mask=pymunk.ShapeFilter.ALL_MASKS ^ 0b100)
self.rod2_shape.filter = pymunk.ShapeFilter(
categories=0b100, mask=pymunk.ShapeFilter.ALL_MASKS ^ 0b100)
self.rod3_shape.filter = pymunk.ShapeFilter(
categories=0b100, mask=pymunk.ShapeFilter.ALL_MASKS ^ 0b100)
self.seat_shape.filter = pymunk.ShapeFilter(
categories=0b100, mask=pymunk.ShapeFilter.ALL_MASKS ^ 0b100)
self.torso_shape.filter = pymunk.ShapeFilter(
categories=0b100, mask=pymunk.ShapeFilter.ALL_MASKS ^ 0b100)
self.legs_shape.filter = pymunk.ShapeFilter(
categories=0b100, mask=pymunk.ShapeFilter.ALL_MASKS ^ 0b100)
self.head_shape.filter = pymunk.ShapeFilter(
categories=0b100, mask=pymunk.ShapeFilter.ALL_MASKS ^ 0b100)
# Set positions of bodies
self.top.position = (600, 720)
self.bottom.position = (600, 0)
self.rod1.position = (600, 695)
self.rod2.position = angle.point1
self.rod3.position = angle.point2
self.seat.position = angle.point4
self.torso.position = (angle.point4[0] + angle.seat[1][0] +
(torso_length / 2) * math.sin(angle._theta),
angle.point4[1] + angle.seat[1][1] +
(torso_length / 2) * math.cos(angle._theta))
self.torso._set_angle(-angle._theta)
self.legs.position = (angle.point4[0] + angle.seat[0][0] -
(legs_length / 2) * math.sin(angle._theta),
angle.point4[1] + angle.seat[0][1] -
(legs_length / 2) * math.cos(angle._theta))
self.legs._set_angle(-angle._theta)
self.head.position = (angle.point4[0] + angle.seat[1][0] +
(torso_length - 10) * math.sin(angle._theta),
angle.point4[1] + angle.seat[1][1] +
(torso_length - 10) * math.cos(angle._theta))
# Create pivot joints for bodies
self.pivot1 = pymunk.PivotJoint(self.top, self.rod1, (600, 695))
self.pivot2 = pymunk.PivotJoint(self.rod1, self.rod2, angle.point1)
self.pivot3 = pymunk.PivotJoint(self.rod2, self.rod3, angle.point2)
self.pivot4 = pymunk.PivotJoint(self.rod3, self.seat, angle.point4)
self.pivot5 = pymunk.PinJoint(self.rod3,
self.seat,
anchor_a=angle.rod3,
anchor_b=angle.seat[0])
self.pivot6 = pymunk.PinJoint(self.rod3,
self.seat,
anchor_a=angle.rod3,
anchor_b=angle.seat[1])
self.pivot7 = pymunk.PivotJoint(self.torso, self.seat,
(angle.point4[0] + angle.seat[1][0],
angle.point4[1] + angle.seat[1][1]))
self.pivot8 = pymunk.PivotJoint(self.seat, self.legs,
(angle.point4[0] + angle.seat[0][0],
angle.point4[1] + angle.seat[0][1]))
self.pivot9 = pymunk.PivotJoint(self.head, self.torso,
self.head.position)
self.gear1 = pymunk.GearJoint(self.torso, self.seat, angle._theta, 1.0)
self.gear2 = pymunk.GearJoint(self.seat, self.legs, -angle._theta, 1.0)
# Prevent pivots from exerting infinite force
self.pivot1.max_force = 1000000
self.pivot2.max_force = 1000000
self.pivot3.max_force = 1000000
self.pivot4.max_force = 1000000
# Add damping (not yet working)
# damping1 = pymunk.DampedSpring(self.top, self.rod1, anchor_a=(-600,-25),
# anchor_b=(0,-rod1_length / 2),
# rest_length=100, stiffness=5, damping=3)
# damping2 = pymunk.DampedSpring(self.top, self.rod1, anchor_a=(600,-25),
# anchor_b=(0,-rod1_length / 2),
# rest_length=100, stiffness=5, damping=3)
# Disable collisions between rods
self.pivot1.collide_bodies = False
self.pivot2.collide_bodies = False
self.pivot3.collide_bodies = False
self.pivot4.collide_bodies = False
self.pivot7.collide_bodies = False
self.pivot8.collide_bodies = False
self.pivot9.collide_bodies = False
# Add bodies and pivots to space
self.space.add(self.top, self.top_shape, self.bottom,
self.bottom_shape, self.rod1, self.rod1_shape,
self.rod2, self.rod2_shape, self.rod3, self.rod3_shape,
self.seat, self.seat_shape, self.torso,
self.torso_shape, self.legs, self.legs_shape, self.head,
self.head_shape, self.pivot1, self.pivot2, self.pivot3,
self.pivot4, self.pivot5, self.pivot6, self.pivot7,
self.pivot8, self.pivot9, self.gear1, self.gear2)
body.position = xylist[i]
body.velocity = (0, 0)
shape = pymunk.Poly(body, vertices)
shape.collision_type = COLLTYPE_UNIT
shape.elasticity = elasticityConstant
shape.friction = frictionConstant
shape.color = color
shape.group = 0
space.add(body, shape)
# Add joints
botlist = []
for bot in space.shapes:
if isinstance(bot, pymunk.Poly) and bot.body != None:
botlist.append(bot.body)
space.add(pymunk.PivotJoint(botlist[0], botlist[1], jointlist[0]))
space.add(pymunk.GearJoint(botlist[0], botlist[1], 0, 1))
space.add(pymunk.PivotJoint(botlist[0], botlist[2], jointlist[1]))
space.add(pymunk.GearJoint(botlist[0], botlist[2], 0, 1))
space.add(pymunk.PivotJoint(botlist[1], botlist[3], jointlist[2]))
space.add(pymunk.GearJoint(botlist[1], botlist[3], 0, 1))
space.add(pymunk.PivotJoint(botlist[2], botlist[3], jointlist[3]))
space.add(pymunk.GearJoint(botlist[2], botlist[3], 0, 1))
# End of Option 2
# Instantiate collision handler
ch = space.add_collision_handler(COLLTYPE_UNIT, COLLTYPE_UNIT)
ch.data["surface"] = screen
ch.pre_solve = groupCheck
ch.post_solve = addJoints
# Counter for random motion
def AddToSpace(self):
if self.shape.space == None:
#constrain to bot's body
self.pivotConstraint = pymunk.PivotJoint(self.bot, self.body, (0,0), (0,0))
self.gearJoint = pymunk.GearJoint(self.bot, self.body,0 , 1)
self.context.space.add(self.body,self.shape, self.pivotConstraint, self.gearJoint)
def __init__(self, b, b2, phase, ratio):
joint = pymunk.GearJoint(b, b2, phase, ratio)
space.add(joint)