def __init__(self, space): super().__init__() shape = pymunk.Poly(self, ((-20, 7.5), (-5, 10), (20, 5), (20, -5), (-5, -10), (-20, -7.5))) shape.collision_type = 1 shape.density = 0.01 # タイヤ側の動作しか計算していないので、車両側は軽くして影響を減らしました……。 shape.elasticity = 0.2 self.tire_lf = Tire(space, self, 500) self.tire_rf = Tire(space, self, 500) self.tire_lr = Tire(space, self, 300) # ドリフトするように、リア・タイヤのグリップを落としました。 self.tire_rr = Tire(space, self, 300) self.tire_lf.position = ( 12.5, 12.5) # noqa: E201, E241 self.tire_rf.position = ( 12.5, -12.5) # noqa: E201 self.tire_lr.position = (-12.5, 15.0) # noqa: E241 self.tire_rr.position = (-12.5, -15.0) pyvot_tire_lf = pymunk.PivotJoint(self, self.tire_lf, self.tire_lf.position, (0, 0)) pyvot_tire_rf = pymunk.PivotJoint(self, self.tire_rf, self.tire_rf.position, (0, 0)) pyvot_tire_lr = pymunk.PivotJoint(self, self.tire_lr, self.tire_lr.position, (0, 0)) pyvot_tire_rr = pymunk.PivotJoint(self, self.tire_rr, self.tire_rr.position, (0, 0)) rotation_tire_lf = pymunk.RotaryLimitJoint(self, self.tire_lf, -pi / 6, pi / 6) rotation_tire_rf = pymunk.RotaryLimitJoint(self.tire_lf, self.tire_rf, 0, 0) rotation_tire_lr = pymunk.RotaryLimitJoint(self, self.tire_lr, 0, 0) rotation_tire_rr = pymunk.RotaryLimitJoint(self, self.tire_rr, 0, 0) space.add(self, shape, pyvot_tire_lf, pyvot_tire_rf, pyvot_tire_lr, pyvot_tire_rr, rotation_tire_lf, rotation_tire_rf, rotation_tire_lr, rotation_tire_rr)
def add_robot(space, seat): #define robot upper body and upper leg ubt = Vec2d(3.5, 0) robot_u_points = [ Vec2d(-4, 31.64) + ubt, Vec2d(-4, 0) + ubt, Vec2d(4, 0) + ubt, Vec2d(4, 31.64) + ubt ] robot_body = pymunk.Body() robot_body.position = seat.position + (0, 3) robot_u = pymunk.Poly(robot_body, robot_u_points) robot_u.mass = 3.311 robot_u.color = THECOLORS["red"] robot_u.filter = pymunk.ShapeFilter(mask=pymunk.ShapeFilter.ALL_MASKS ^ 1) robot_u_leg = pymunk.Poly(seat, [(1, 3), (-10.5, 3), (-10.5, 8), (1, 8)]) robot_u_leg.color = THECOLORS["red"] robot_u_leg.mass = 0.603 #robot_u_leg_extra = pymunk.Circle(seat,2,(5,0)) #robot_u_leg_extra.mass = 2 #define robot lower leg robot_leg = pymunk.Body() robot_leg.position = seat.position robot_l_leg = pymunk.Poly(robot_leg, [(-10.5, 5), (-10.5, -11.8), (-4.0, -11.8), (-4.0, 5)]) robot_l_leg.mass = 1.214 robot_l_leg.color = THECOLORS["red"] space.add(robot_body, robot_u, robot_u_leg, robot_leg, robot_l_leg) #motor and pivot for hip #uses measured values of angles rather than given in program seat_motor = pymunk.SimpleMotor(seat, robot_body, 0) seat_motor.max_force = 1e6 seat_pivot = pymunk.PivotJoint(seat, robot_body, robot_body.position + ubt) seat_pivot._set_collide_bodies(False) seat_pivot_lim = pymunk.RotaryLimitJoint(robot_body, seat, 0, 0.575959) space.add(seat_motor, seat_pivot, seat_pivot_lim) #motor and pivot for knee max_knee_ang = 5 max_knee_ang = np.deg2rad(max_knee_ang) knee_motor = pymunk.SimpleMotor(seat, robot_leg, 0) knee_motor.max_force = 1e5 knee_pivot = pymunk.PivotJoint(seat, robot_leg, seat.position + (-8, 7)) knee_pivot._set_collide_bodies(False) knee_pivot_lim = pymunk.RotaryLimitJoint(seat, robot_leg, max_knee_ang - np.deg2rad(69), max_knee_ang) space.add(knee_motor, knee_pivot, knee_pivot_lim) return seat_motor, knee_motor, robot_body, robot_leg, robot_u_leg
def __init__(self, space, base_body, body_pos, length, angle, mass=1, thickness=5): b = body_pos dif = (math.sin(angle) * length, math.cos(angle) * length) v = b + dif base_dif = body_pos - base_body.position link_body = pymunk.Body(mass, mass * length * length / 3) link_body.position = body_pos self.link = pymunk.Segment(link_body, (0,0), dif, thickness) self.link.filter = pymunk.ShapeFilter(categories=0b1, mask=0b0) link_body.moment = pymunk.moment_for_segment(mass, self.link.a, self.link.b, thickness) link_body.center_of_gravity = (self.link.a + self.link.b)/2.0 base_motor = pymunk.SimpleMotor(base_body, link_body, 0) base_joint = pymunk.PinJoint(base_body, link_body, base_dif, (0,0)) base_joint.error_bias = 0.0001 base_clamp = pymunk.RotaryLimitJoint(base_body, link_body, (-3*math.pi/4), (3*math.pi/4)) space.add(self.link, link_body, base_motor, base_joint, base_clamp) print("Body pos: " + str(link_body.position)) print("COG pos: " + str(link_body.center_of_gravity)) print("a" + str(self.link.a)) print("b" + str(self.link.b)) self.motor = base_motor self.body = link_body self.start_tip = v
def __init__(self, a, b, position, range=(-pi, pi), max_rate=2.0): self.pivot = pymunk.PivotJoint(a, b, position) self.motor = pymunk.SimpleMotor(a, b, 0) self.motor.max_force = 1e7 self.limit = pymunk.RotaryLimitJoint(a, b, range[0], range[1]) self.limit.max_force = 1e1 self.limit.max_bias = 0.5 self.max_rate = max_rate
def create_physics(self): constraint = pymunk.RotaryLimitJoint( self.a.body, self.b.body, min=self.min, max=self.max, ) self._constraint = constraint self.engine.space.add(constraint)
def __init__(self, space, pos, ul, ll, w, \ lua, lla, rua, rla): '''Creates a passive dynamic walker. The parameters are as follow: space -- the pymunk space pos -- the initial position ul -- the length of the upper leg ll -- the length of the lower leg w -- the width of the robot lua -- the angle of the left hip lla -- the angle of the left ankle rua -- the angle of the right hip rla -- the angle of the right angle ''' #print "Creating Walker: ", pos, ul, ll, w, lua, lla, rua, rla # Constructor method self.space = space self.density = 1.0 self.friction = 1.0 self.elasticity = 0.4 self.group = 1 # Create left leg self.lul = self._create_leg(pos, ul, w, lua) ll_pos = Vec2d(0, -ul).rotated(lua) + pos self.lll = self._create_leg(ll_pos, ll, w, lua + lla) self.l_knee = pymunk.PivotJoint(self.lul.body, \ self.lll.body, ll_pos) # Create right leg self.rul = self._create_leg(pos, ul, w, rua) rl_pos = Vec2d(0, -ul).rotated(rua) + pos self.rll = self._create_leg(rl_pos, ll, w, rua + rla) self.r_knee = pymunk.PivotJoint(self.rul.body, \ self.rll.body, rl_pos) # Hip self.hip = pymunk.PivotJoint(self.lul.body, self.rul.body, pos) # Limit rotation on the knees between zero and pi self.l_limit = pymunk.RotaryLimitJoint(self.lul.body, \ self.lll.body, (-pi/360.0)-lla, pi-lla) self.r_limit = pymunk.RotaryLimitJoint(self.rul.body, \ self.rll.body, (-pi/360.0)-rla, pi-rla) # add the constraints to the space self.space.add(self.l_knee, self.r_knee, self.hip, \ self.l_limit, self.r_limit)
def testPickle(self): a, b = p.Body(10, 10), p.Body(20, 20) j = p.RotaryLimitJoint(a, b, 1, 2) s = pickle.dumps(j) j2 = pickle.loads(s) self.assertEqual(j.min, j2.min) self.assertEqual(j.max, j2.max) self.assertEqual(j.a.mass, j2.a.mass) self.assertEqual(j.b.mass, j2.b.mass)
def _attach_to_anchor(self): self.joint = pymunk.PivotJoint(self.anchor.pm_body, self.pm_body, self._rel_coord_anchor, self._rel_coord_part) self.joint.collide_bodies = False self.limit = pymunk.RotaryLimitJoint(self.anchor.pm_body, self.pm_body, self.angle_offset - self.rotation_range / 2, self.angle_offset + self.rotation_range / 2) self.motor = pymunk.SimpleMotor(self.anchor.pm_body, self.pm_body, 0) self.pm_elements += [self.joint, self.motor, self.limit]
def __init__(self, app, pos, angle): self.app = app rads = angle / -57.3 hw = self.size[1] * .5 self.body = pymunk.Body() self.body.position = pos[::2] self.body.angle = rads static_body = self.app.physics.space.static_body self.collider = pymunk.Poly.create_box(self.body, self.size) self.collider.y = pos[1] self.collider.height = self.height self.collider.mass = .5 self.collider.collision_type = physics.STATIC mask = pymunk.ShapeFilter.ALL_MASKS ^ physics.STATIC_FILTER ^ physics.DOOR_FILTER self.collider.filter = pymunk.ShapeFilter(categories=physics.DOOR_FILTER, mask=mask) hinge_dx = math.sin(-rads) * -hw hinge_dy = math.cos(-rads) * -hw self.hinge_joint = pymunk.PivotJoint(static_body, self.body, (pos[0] + hinge_dx, pos[2] + hinge_dy)) self.rotary_spring = pymunk.DampedRotarySpring(static_body, self.body, -rads, 5, 5) min_angle = rads - self.swing_angle max_angle = rads + self.swing_angle self.rotary_limit = pymunk.RotaryLimitJoint(static_body, self.body, min_angle, max_angle) self.app.physics.space.add(self.body, self.collider, self.hinge_joint, self.rotary_spring, self.rotary_limit) self.canvas = Canvas() with self.canvas: PushMatrix() self.pos = Translate(*pos) self.rot = Rotate(0, 0, 1, 0) self.app.graphic_data.draw_mesh('models/door', self.canvas, texture='door.png') PopMatrix()
def __init__(self, floor, leg_mass=5., leg_width=2., leg_length=40., leg_friction=.4, init_leg_angle=30): self.leg_length = leg_length self.leg_mass = leg_mass self.leg_width = leg_width self.leg_friction = leg_friction self.legs_min_angle = 0. self.legs_max_angle = .9 * math.pi self.leg1, self.joint_pos_l1 = self.create_leg( self.leg_mass, self.leg_length, self.leg_width, math.radians(init_leg_angle), leg_friction, pygame.color.THECOLORS['red']) self.leg2, self.joint_pos_l2 = self.create_leg( self.leg_mass, self.leg_length, self.leg_width, math.radians(-init_leg_angle), leg_friction, pygame.color.THECOLORS['green']) leg_position_x = floor.body.position.x / 8. leg_position_y = floor.body.position.y self.place_leg(self.leg1, leg_position_x, leg_position_y, math.radians(init_leg_angle)) self.place_leg(self.leg2, leg_position_x, leg_position_y, math.radians(-init_leg_angle)) self.rotation_center_legs_joint = pymunk.PivotJoint( self.leg1.body, self.leg2.body, self.joint_pos_l2, self.joint_pos_l1) self.rotation_center_legs_joint.collide_bodies = False self.leg1_floor_motor = pymunk.SimpleMotor(self.leg1.body, floor.body, 0.) self.leg2_floor_motor = pymunk.SimpleMotor(self.leg2.body, floor.body, 0.) self.legs_rotary_limit_joint = pymunk.RotaryLimitJoint( self.leg1.body, self.leg2.body, self.legs_min_angle, self.legs_max_angle)
def addToSpace(self, space): # samples for door # each sample contains information about surface point, normal and maximum radius of contacting body self.door_samples = [] for t in np.linspace(0.1, 0.9, 5, endpoint=True): self.door_samples.append( (Vec2d(self.x_mult * self.length/2.0 * t, -self.width/2.0), Vec2d(0,-1), float("inf")) ) radius_tab = [ 100, 50, 15, 5, 5 ] idx = 0 for t in np.linspace(0.1, 0.9, 5, endpoint=True): self.door_samples.append( (Vec2d(self.x_mult * self.length/2.0 * t, self.width/2.0), Vec2d(0,1), radius_tab[idx]) ) idx += 1 self.door_samples.append( (Vec2d(self.x_mult * (self.length/2.0*0.8 - 4*2.0), 4*4.0), Vec2d(0,1), float("inf")) ) self.door_samples.append( (Vec2d(self.x_mult * (self.length/2.0*0.8 - 4*2.0), 4*4.0), Vec2d(0,-1), 5) ) mass = 10.0 moment = pymunk.moment_for_poly(mass, self.fp) self.body = pymunk.Body(mass, moment) self.body.position = self.hinge_pos #t5 self.body.angle = self.rotation + self.init_angle self.door_shape = pymunk.Poly(self.body, self.fp, radius=1) self.door_shape.group = 1 # handle self.handle_shape1 = pymunk.Segment(self.body, Vec2d(self.x_mult * (self.length/2.0*0.8), self.width*0.5), Vec2d(self.x_mult * (self.length/2.0*0.8), self.width*4.0), self.width) self.handle_shape1.group = 1 self.handle_shape2 = pymunk.Segment(self.body, Vec2d(self.x_mult * (self.length/2.0*0.8 - self.width*4.0), self.width*4.0), Vec2d(self.x_mult * (self.length/2.0*0.8), self.width*4.0), self.width) self.handle_shape2.group = 1 space.add(self.body, self.door_shape, self.handle_shape1, self.handle_shape2) j = pymunk.PivotJoint(self.body, space.static_body, self.body.position) s = pymunk.DampedRotarySpring(self.body, space.static_body, 0, 0, 90000) space.add(j, s) j2 = pymunk.RotaryLimitJoint(self.body, space.static_body, -(self.rotation+self.limits[1]), -(self.rotation+self.limits[0])) space.add(j2)
def testRotaryLimitJoint(self): a, b = p.Body(10, 10), p.Body(20, 20) j = p.RotaryLimitJoint(a, b, 0.1, 0.2) self.assertEqual(j.max, 0.2) self.assertEqual(j.min, 0.1)
def __init__(self, space, screen): self.space = space self.screen = screen width, height = screen.get_size() pos = (3 * width / 4, height / 4) shapeFilter = pm.ShapeFilter(collisionGroups["PLAYER2"], collisionGroups["PLAYER1"]) self.coreBody = pm.Body(10, 1000) self.coreBody.position = pos self.rightArmAlive = True self.leftArmAlive = True self.rightLegAlive = True self.leftLegAlive = True self.torso = Body(space, screen, 2) self.torso.shape.filter = shapeFilter self.torsoRotationLimit = pm.RotaryLimitJoint(self.space.static_body, self.torso.shape.body, -math.pi / 10, math.pi / 10) self.torsoLocationTie = pm.PinJoint(self.coreBody, self.torso.shape.body) self.torsoLocationTie.distance = 0 self.torso.shape.body.position = pos # Set up arms self.rElbow = DefensiveBlock(self.space, self.screen) self.rElbow.shape.body.position = self.torso.shape.body.position + ( -50, 35) self.rElbow.shape.filter = shapeFilter self.rUpperArm = pm.PinJoint(self.torso.shape.body, self.rElbow.shape.body, (0, 35), (0, 0)) self.rUpperArm.distance = 40 self.rFist = OffensiveBlock(self.space, self.screen) self.rFist.shape.filter = shapeFilter self.rFist.shape.body.position = self.rElbow.shape.body.position + ( -25, 0) self.rFistRotationLimit = pm.RotaryLimitJoint(self.space.static_body, self.rFist.shape.body, -math.pi / 5, math.pi / 5) self.rLowerArm = pm.PinJoint(self.rElbow.shape.body, self.rFist.shape.body) self.rLowerArm.distance = 50 self.lElbow = DefensiveBlock(self.space, self.screen) self.lElbow.shape.body.position = self.torso.shape.body.position + (50, 35) self.lElbow.shape.filter = shapeFilter self.lUpperArm = pm.PinJoint(self.torso.shape.body, self.lElbow.shape.body, (0, 35), (0, 0)) self.lUpperArm.distance = 40 self.lFist = OffensiveBlock(self.space, self.screen) self.lFist.shape.filter = shapeFilter self.lFist.shape.body.position = self.lElbow.shape.body.position + (25, 0) self.lFistRotationLimit = pm.RotaryLimitJoint(self.space.static_body, self.lFist.shape.body, -math.pi / 5, math.pi / 5) self.lLowerArm = pm.PinJoint(self.lElbow.shape.body, self.lFist.shape.body) self.lLowerArm.distance = 50 # Set up legs self.rKnee = DefensiveBlock(self.space, self.screen) self.rKnee.shape.body.position = self.torso.shape.body.position + ( -25, -100) self.rKnee.shape.filter = shapeFilter self.rUpperLeg = pm.PinJoint(self.torso.shape.body, self.rKnee.shape.body, (0, -50), (0, 0)) self.rUpperLeg.distance = 40 self.rFoot = OffensiveBlock(self.space, self.screen) self.rFoot.shape.friction = 1.5 self.rFoot.shape.filter = shapeFilter self.rFoot.shape.body.position = self.rKnee.shape.body.position + (-25, -10) self.rFootRotationLimit = pm.RotaryLimitJoint(self.space.static_body, self.rFoot.shape.body, -math.pi / 5, math.pi / 5) self.rLowerLeg = pm.PinJoint(self.rKnee.shape.body, self.rFoot.shape.body, (0, 0), (0, 25)) self.rLowerLeg.distance = 50 self.lKnee = DefensiveBlock(self.space, self.screen) self.lKnee.shape.body.position = self.torso.shape.body.position + ( 25, -100) self.lKnee.shape.filter = shapeFilter self.lUpperLeg = pm.PinJoint(self.torso.shape.body, self.lKnee.shape.body, (0, -50), (0, 0)) self.lUpperLeg.distance = 40 self.rLowerLeg = pm.PinJoint(self.rKnee.shape.body, self.rFoot.shape.body, (0, 0), (0, 25)) self.rLowerLeg.distance = 50 self.lKnee = DefensiveBlock(self.space, self.screen) self.lKnee.shape.body.position = self.torso.shape.body.position + ( 25, -100) self.lKnee.shape.filter = shapeFilter self.lUpperLeg = pm.PinJoint(self.torso.shape.body, self.lKnee.shape.body, (0, -50), (0, 0)) self.lUpperLeg.distance = 40 self.lFoot = OffensiveBlock(self.space, self.screen) self.lFoot.shape.friction = 1.5 self.lFoot.shape.filter = shapeFilter self.lFoot.shape.body.position = self.lKnee.shape.body.position + (25, -10) self.lFootRotationLimit = pm.RotaryLimitJoint(self.space.static_body, self.lFoot.shape.body, -math.pi / 5, math.pi / 5) self.lLowerLeg = pm.PinJoint(self.lKnee.shape.body, self.lFoot.shape.body, (0, 0), (0, 25)) self.lLowerLeg.distance = 50 self.rLegDownwardForce = pm.DampedSpring(self.coreBody, self.rFoot.shape.body, (0, 0), (0, 25), 300, 1250, 1) self.lLegDownwardForce = pm.DampedSpring(self.coreBody, self.lFoot.shape.body, (0, 0), (0, 25), 300, 1250, 1) self.space.add(self.coreBody, self.torsoRotationLimit, self.torsoLocationTie, self.rUpperLeg, self.rLegDownwardForce, self.lUpperLeg, self.lLegDownwardForce, self.rFootRotationLimit, self.lFootRotationLimit, self.rFistRotationLimit, self.lFistRotationLimit, self.lLowerLeg, self.rLowerLeg, self.rUpperArm, self.rLowerArm, self.lUpperArm, self.lLowerArm)
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__(self): left_thigh = pymunk.Body(body_type=pymunk.Body.DYNAMIC) left_thigh.position = (100, 175) left_thigh_segment = pymunk.Segment(left_thigh, (0, 25), (0, -25), 2) left_thigh_segment.density = 1 left_thigh_segment.friction = 0.46 left_thigh_segment.filter = pymunk.ShapeFilter(group=0b1) left_knee_position = (100, 150) left_leg = pymunk.Body(body_type=pymunk.Body.DYNAMIC) left_leg.position = (100, 125) left_leg_segment = pymunk.Segment(left_leg, (0, 25), (0, -25), 2) left_leg_segment.density = 1 left_leg_segment.friction = 0.8 left_leg_segment.filter = pymunk.ShapeFilter(group=0b1) left_knee_joint = pymunk.PivotJoint(left_thigh, left_leg, left_knee_position) left_knee_limit = pymunk.RotaryLimitJoint(left_thigh, left_leg, -math.pi / 2, 0) left_ankle_position = (100, 100) left_foot = pymunk.Body(body_type=pymunk.Body.DYNAMIC) left_foot.position = (110, 100) left_foot_segment = pymunk.Segment(left_foot, (-10, 0), (10, 0), 2) left_foot_segment.density = 1 left_foot_segment.friction = 0.8 left_foot_segment.filter = pymunk.ShapeFilter(group=0b1) left_ankle_joint = pymunk.PivotJoint(left_leg, left_foot, left_ankle_position) left_ankle_limit = pymunk.RotaryLimitJoint(left_leg, left_foot, -math.pi / 6, math.pi / 6) # right_leg right_thigh = pymunk.Body(body_type=pymunk.Body.DYNAMIC) right_thigh.position = (100, 175) right_thigh_segment = pymunk.Segment(right_thigh, (0, 25), (0, -25), 2) right_thigh_segment.density = 1 right_thigh_segment.friction = 0.46 right_thigh_segment.filter = pymunk.ShapeFilter(group=0b1) right_knee_position = (100, 150) right_leg = pymunk.Body(body_type=pymunk.Body.DYNAMIC) right_leg.position = (100, 125) right_leg_segment = pymunk.Segment(right_leg, (0, 25), (0, -25), 2) right_leg_segment.density = 1 right_leg_segment.friction = 0.8 right_leg_segment.filter = pymunk.ShapeFilter(group=0b1) right_knee_joint = pymunk.PivotJoint(right_thigh, right_leg, right_knee_position) right_knee_limit = pymunk.RotaryLimitJoint(right_thigh, right_leg, -math.pi / 2, 0) right_ankle_position = (100, 100) right_foot = pymunk.Body(body_type=pymunk.Body.DYNAMIC) right_foot.position = (110, 100) right_foot_segment = pymunk.Segment(right_foot, (-10, 0), (10, 0), 2) right_foot_segment.density = 1 right_foot_segment.friction = 0.8 right_foot_segment.filter = pymunk.ShapeFilter(group=0b1) right_ankle_joint = pymunk.PivotJoint(right_leg, right_foot, right_ankle_position) right_ankle_limit = pymunk.RotaryLimitJoint(right_leg, right_foot, -math.pi / 6, math.pi / 6) hip_position = (100, 200) torso = pymunk.Body(body_type=pymunk.Body.DYNAMIC) torso.position = (100, 240) torso_segment = pymunk.Segment(torso, (0, 40), (0, -40), 2) torso_segment.density = 1 torso_segment.friction = 0.46 torso_segment.filter = pymunk.ShapeFilter(group=0b1) left_hip_joint = pymunk.PivotJoint(torso, left_thigh, hip_position) left_hip_limit = pymunk.RotaryLimitJoint(torso, left_thigh, -math.pi / 2, math.pi / 2) right_hip_joint = pymunk.PivotJoint(torso, right_thigh, hip_position) right_hip_limit = pymunk.RotaryLimitJoint(torso, right_thigh, -math.pi / 2, math.pi / 2) space.add(left_thigh, left_thigh_segment, left_leg, left_leg_segment, left_foot, left_foot_segment) space.add(right_thigh, right_thigh_segment, right_leg, right_leg_segment, right_foot, right_foot_segment) space.add(torso, torso_segment, left_hip_joint, right_hip_joint, left_knee_joint, right_knee_joint, left_ankle_joint, right_ankle_joint) space.add(left_hip_limit, right_hip_limit, left_knee_limit, right_knee_limit, left_ankle_limit, right_ankle_limit) self.l_thigh = left_thigh self.l_thigh_seg = left_thigh_segment self.l_leg = left_leg self.l_leg_seg = left_leg_segment self.l_foot = left_foot self.l_foot_seg = left_foot_segment self.r_thigh = right_thigh self.r_thigh_seg = right_thigh_segment self.r_leg = right_leg self.r_leg_seg = right_leg_segment self.r_foot = right_foot self.r_foot_seg = right_foot_segment self.chest = torso self.chest_seg = torso_segment self.alive = True
def load_elements(self): self.add_element(self.position, (22, 20), 60.0, pg.Rect(58, 32, 30, 25)) self.add_element(self.position + pm.Vec2d(0, -14), (12, 12), 4.0, pg.Rect(58, 106, 12, 12)) self.add_element(self.position + pm.Vec2d(-11, -8), (11, 7), 2.0, pg.Rect(85, 20, 11, 7)) self.add_element(self.position + pm.Vec2d(-19, -8), (14, 8), 2.0, pg.Rect(71, 19, 14, 8)) self.add_element(self.position + pm.Vec2d(-6, 14), (11, 12), 8.0, pg.Rect(181, 64, 11, 12)) self.add_element(self.position + pm.Vec2d(-5, 25), (11, 12), 4.0, pg.Rect(181, 76, 11, 12)) self.add_element(self.position + pm.Vec2d(11, -8), (11, 7), 2.0, pg.Rect(160, 20, 11, 7)) self.add_element(self.position + pm.Vec2d(19, -8), (14, 8), 2.0, pg.Rect(171, 19, 14, 8)) self.add_element(self.position + pm.Vec2d(6, 14), (11, 12), 8.0, pg.Rect(64, 64, 11, 12)) self.add_element(self.position + pm.Vec2d(5, 25), (11, 12), 4.0, pg.Rect(64, 76, 11, 12)) self.add_joint( pm.PivotJoint(self._elements[0][0], self._elements[1][0], self.position + pm.Vec2d(0, -11))) self.add_joint( pm.PivotJoint(self._elements[0][0], self._elements[1][0], self._elements[0][0].position + pm.Vec2d(0, -15))) self.add_joint( pm.PivotJoint(self._elements[0][0], self._elements[2][0], self.position + pm.Vec2d(-10, -7))) self.add_joint( pm.RotaryLimitJoint(self._elements[0][0], self._elements[2][0], math.radians(-90), math.radians(90))) self.add_joint( pm.PivotJoint(self._elements[2][0], self._elements[3][0], self.position + pm.Vec2d(-16, -11))) self.add_joint( pm.RotaryLimitJoint(self._elements[2][0], self._elements[3][0], math.radians(0), math.radians(110))) self.add_joint( pm.PivotJoint(self._elements[0][0], self._elements[4][0], self.position + pm.Vec2d(-5, 10))) self.add_joint( pm.RotaryLimitJoint(self._elements[0][0], self._elements[4][0], math.radians(0), math.radians(90))) self.add_joint( pm.PivotJoint(self._elements[4][0], self._elements[5][0], self.position + pm.Vec2d(-4, 18))) self.add_joint( pm.RotaryLimitJoint(self._elements[4][0], self._elements[5][0], math.radians(-110), math.radians(0))) self.add_joint( pm.PivotJoint(self._elements[0][0], self._elements[6][0], self.position + pm.Vec2d(10, -7))) self.add_joint( pm.RotaryLimitJoint(self._elements[0][0], self._elements[6][0], math.radians(-90), math.radians(90))) self.add_joint( pm.PivotJoint(self._elements[6][0], self._elements[7][0], self.position + pm.Vec2d(16, -8))) self.add_joint( pm.RotaryLimitJoint(self._elements[6][0], self._elements[7][0], math.radians(-110), math.radians(0))) self.add_joint( pm.PivotJoint(self._elements[0][0], self._elements[8][0], self.position + pm.Vec2d(5, 10))) self.add_joint( pm.RotaryLimitJoint(self._elements[0][0], self._elements[8][0], math.radians(-90), math.radians(0))) self.add_joint( pm.PivotJoint(self._elements[8][0], self._elements[9][0], self.position + pm.Vec2d(4, 18))) self.add_joint( pm.RotaryLimitJoint(self._elements[8][0], self._elements[9][0], math.radians(0), math.radians(110)))
def __init__(self, _id, _parent, _name): super(SceneLevel, self).__init__(_id, _parent) self.name = _name level_data_path = os.path.join(SceneLevel.LEVEL_ROOT, self.name) config_path = os.path.join(level_data_path, SceneLevel.CONFIG_FILE) config_file_handler = open(config_path, "r") config = json.load(config_file_handler) config_file_handler.close() self.fps = config["FPS"] # pymunk space self.space = pymunk.Space() self.space.gravity = config["GRAVITY"] self.space.damping = config["DAMPING"] # static data (layout of terrain, etc.) map_path = os.path.join(level_data_path, SceneLevel.MAP_BODY_FILE) map_file_handler = open(map_path, "r") self.map_data = json.load(map_file_handler) map_file_handler.close() map_img_path = os.path.join(level_data_path, SceneLevel.MAP_IMG_FILE) self.map_img = pygame.image.load(map_img_path) body = None for rev_shape in self.map_data: if len(rev_shape) <= 2: continue print(rev_shape) pos = get_center(rev_shape) shape = [(x[1] - pos[1], x[0] - pos[0]) for x in rev_shape] pos = tuple(reversed(pos)) body = pymunk.Body(body_type=pymunk.Body.STATIC) body.position = pos shape = pymunk.Poly(body, shape, radius=1) shape.friction = config["FRICTION"] / 2 self.space.add(body, shape) chara_img_path = os.path.join("pic", "CHARA.png") self.chara_img = pygame.image.load(chara_img_path) chara_shape = config["CHARA_BODY"] chara_shape_loc = get_center(chara_shape) chara_shape = [(x[1] - chara_shape_loc[1], x[0] - chara_shape_loc[0]) for x in chara_shape] chara_shape_loc = tuple(reversed(chara_shape_loc)) self.chara_body = pymunk.Body(config["CHARA_MASS"], config["CHARA_MOVEMENT"]) self.chara_body.position = chara_shape_loc chara_shape = pymunk.Poly(self.chara_body, chara_shape, radius=1) chara_shape.friction = config["FRICTION"] / 2 self.space.add(chara_shape, self.chara_body) self.chara_offset = tuple(reversed(config["CHARA_OFFSET"])) self.chara_ori_right = 1 def limit_velocity(body, gravity, damping, dt): max_velocity = 200 pymunk.Body.update_velocity(body, gravity, damping, dt) l = body.velocity.length if l > max_velocity: body.velocity = body.velocity * (max_velocity / l) self.chara_body.velocity_func = limit_velocity c = pymunk.RotaryLimitJoint(body, self.chara_body, 0, 0) self.space.add(c) self.pressing_key = None
def testMax(self): a, b = p.Body(10, 10), p.Body(20, 20) j = p.RotaryLimitJoint(a, b, 0, 1) self.assertEqual(j.max, 1) j.max = 2 self.assertEqual(j.max, 2)
def setup(self, *args, **kwargs): super().setup(*args, **kwargs) # physics setup, starting with main body # signature: moment_for_circle(mass, inner_rad, outer_rad, offset) inertia = pm.moment_for_circle(self.mass, 0, self.radius, (0, 0)) self.robot_body = body = pm.Body(self.mass, inertia) body.position = self.init_pos body.angle = self.init_angle self.add_to_space(body) # For control. The rough joint setup was taken form tank.py in the # pymunk examples. self.control_body = control_body = pm.Body(body_type=pm.Body.KINEMATIC) control_body.position = self.init_pos control_body.angle = self.init_angle self.add_to_space(control_body) pos_control_joint = pm.PivotJoint(control_body, body, (0, 0), (0, 0)) pos_control_joint.max_bias = 0 pos_control_joint.max_force = self.phys_vars.robot_pos_joint_max_force self.add_to_space(pos_control_joint) rot_control_joint = pm.GearJoint(control_body, body, 0.0, 1.0) rot_control_joint.error_bias = 0.0 rot_control_joint.max_bias = 2.5 rot_control_joint.max_force = self.phys_vars.robot_rot_joint_max_force self.add_to_space(rot_control_joint) # googly eye control bodies & joints self.pupil_bodies = [] for eye_side in [-1, 1]: eye_mass = self.mass / 10 eye_radius = self.radius eye_inertia = pm.moment_for_circle(eye_mass, 0, eye_radius, (0, 0)) eye_body = pm.Body(eye_mass, eye_inertia) eye_body.angle = self.init_angle eye_joint = pm.DampedRotarySpring(body, eye_body, 0, 0.1, 3e-3) eye_joint.max_bias = 3.0 eye_joint.max_force = 0.001 self.pupil_bodies.append(eye_body) self.add_to_space(eye_body, eye_joint) # finger bodies/controls (annoying) finger_thickness = 0.25 * self.radius finger_upper_length = 1.1 * self.radius finger_lower_length = 0.7 * self.radius self.finger_bodies = [] self.finger_motors = [] finger_vertices = [] finger_inner_vertices = [] self.target_finger_angle = 0.0 for finger_side in [-1, 1]: # basic finger body finger_verts = make_finger_vertices( upper_arm_len=finger_upper_length, forearm_len=finger_lower_length, thickness=finger_thickness, side_sign=finger_side) finger_vertices.append(finger_verts) finger_inner_verts = make_finger_vertices( upper_arm_len=finger_upper_length - ROBOT_LINE_THICKNESS * 2, forearm_len=finger_lower_length - ROBOT_LINE_THICKNESS * 2, thickness=finger_thickness - ROBOT_LINE_THICKNESS * 2, side_sign=finger_side) finger_inner_verts = [[(x, y + ROBOT_LINE_THICKNESS) for x, y in box] for box in finger_inner_verts] finger_inner_vertices.append(finger_inner_verts) # these are movement limits; they are useful below, but also # necessary to make initial positioning work if finger_side < 0: lower_rot_lim = -self.finger_rot_limit_inner upper_rot_lim = self.finger_rot_limit_outer if finger_side > 0: lower_rot_lim = -self.finger_rot_limit_outer upper_rot_lim = self.finger_rot_limit_inner finger_mass = self.mass / 8 finger_inertia = pm.moment_for_poly(finger_mass, sum(finger_verts, [])) finger_body = pm.Body(finger_mass, finger_inertia) if finger_side < 0: delta_finger_angle = upper_rot_lim finger_body.angle = self.init_angle + delta_finger_angle else: delta_finger_angle = lower_rot_lim finger_body.angle = self.init_angle + delta_finger_angle # position of finger relative to body finger_rel_pos = (finger_side * self.radius * 0.45, self.radius * 0.1) finger_rel_pos_rot = gtools.rotate_vec(finger_rel_pos, self.init_angle) finger_body.position = gtools.add_vecs(body.position, finger_rel_pos_rot) self.add_to_space(finger_body) self.finger_bodies.append(finger_body) # pin joint to keep it in place (it will rotate around this point) finger_pin = pm.PinJoint( body, finger_body, finger_rel_pos, (0, 0), ) finger_pin.error_bias = 0.0 self.add_to_space(finger_pin) # rotary limit joint to stop it from getting too far out of line finger_limit = pm.RotaryLimitJoint(body, finger_body, lower_rot_lim, upper_rot_lim) finger_limit.error_bias = 0.0 self.add_to_space(finger_limit) # motor to move the fingers around (very limited in power so as not # to conflict with rotary limit joint) finger_motor = pm.SimpleMotor(body, finger_body, 0.0) finger_motor.rate = 0.0 finger_motor.max_bias = 0.0 finger_motor.max_force = self.phys_vars.robot_finger_max_force self.add_to_space(finger_motor) self.finger_motors.append(finger_motor) # For collision. Main body circle. Signature: Circle(body, radius, # offset). robot_group = 1 body_shape = pm.Circle(body, self.radius, (0, 0)) body_shape.filter = pm.ShapeFilter(group=robot_group) body_shape.friction = 0.5 self.add_to_space(body_shape) # the fingers finger_shapes = [] for finger_body, finger_verts, finger_side in zip( self.finger_bodies, finger_vertices, [-1, 1]): finger_subshapes = [] for finger_subverts in finger_verts: finger_subshape = pm.Poly(finger_body, finger_subverts) finger_subshape.filter = pm.ShapeFilter(group=robot_group) # grippy fingers finger_subshape.friction = 5.0 finger_subshapes.append(finger_subshape) self.add_to_space(*finger_subshapes) finger_shapes.append(finger_subshapes) # graphics setup # draw a circular body circ_body_in = r.make_circle(radius=self.radius - ROBOT_LINE_THICKNESS, res=100) circ_body_out = r.make_circle(radius=self.radius, res=100) robot_colour = COLOURS_RGB['grey'] dark_robot_colour = darken_rgb(robot_colour) light_robot_colour = lighten_rgb(robot_colour, 4) circ_body_in.set_color(*robot_colour) circ_body_out.set_color(*dark_robot_colour) # draw the two fingers self.finger_xforms = [] finger_outer_geoms = [] finger_inner_geoms = [] for finger_outer_subshapes, finger_inner_verts, finger_side in zip( finger_shapes, finger_inner_vertices, [-1, 1]): finger_xform = r.Transform() self.finger_xforms.append(finger_xform) for finger_subshape in finger_outer_subshapes: vertices = [(v.x, v.y) for v in finger_subshape.get_vertices()] finger_outer_geom = r.make_polygon(vertices) finger_outer_geom.set_color(*robot_colour) finger_outer_geom.add_attr(finger_xform) finger_outer_geoms.append(finger_outer_geom) for vertices in finger_inner_verts: finger_inner_geom = r.make_polygon(vertices) finger_inner_geom.set_color(*light_robot_colour) finger_inner_geom.add_attr(finger_xform) finger_inner_geoms.append(finger_inner_geom) for geom in finger_outer_geoms: self.viewer.add_geom(geom) for geom in finger_inner_geoms: self.viewer.add_geom(geom) # draw some cute eyes eye_shapes = [] self.pupil_transforms = [] for x_sign in [-1, 1]: eye = r.make_circle(radius=0.2 * self.radius, res=20) eye.set_color(1.0, 1.0, 1.0) eye_base_transform = r.Transform().set_translation( x_sign * 0.4 * self.radius, 0.3 * self.radius) eye.add_attr(eye_base_transform) pupil = r.make_circle(radius=0.12 * self.radius, res=10) pupil.set_color(0.1, 0.1, 0.1) # The pupils point forward slightly pupil_transform = r.Transform() pupil.add_attr(r.Transform().set_translation( 0, self.radius * 0.07)) pupil.add_attr(pupil_transform) pupil.add_attr(eye_base_transform) self.pupil_transforms.append(pupil_transform) eye_shapes.extend([eye, pupil]) # join them together self.robot_xform = r.Transform() robot_compound = r.Compound([circ_body_out, circ_body_in, *eye_shapes]) robot_compound.add_attr(self.robot_xform) self.viewer.add_geom(robot_compound)
def testMin(self): a, b = p.Body(10, 10), p.Body(20, 20) j = p.RotaryLimitJoint(a, b, 1, 0) self.assertEqual(j.min, 1) j.min = 2 self.assertEqual(j.min, 2)
def addToSpace(self, space): line_width = 4.0 p1 = Vec2d(-self.width / 2.0, self.depth / 2.0) p2 = Vec2d(-self.width / 2.0, -self.depth / 2.0) p3 = Vec2d(self.width / 2.0, -self.depth / 2.0) p4 = Vec2d(self.width / 2.0, self.depth / 2.0) p5 = Vec2d(-self.width / 2.0, self.depth / 2.0 + line_width * 2) # left door joint t1 = p1.rotated( self.rotation ) + self.position #transform(p1, self.position, self.rotation) t2 = p2.rotated( self.rotation ) + self.position #transform(p2, self.position, self.rotation) t3 = p3.rotated( self.rotation ) + self.position #transform(p3, self.position, self.rotation) t4 = p4.rotated( self.rotation ) + self.position #transform(p4, self.position, self.rotation) t5 = p5.rotated( self.rotation ) + self.position #transform(p5, self.position, self.rotation) static = [ pymunk.Segment(space.static_body, t1, t2, line_width), pymunk.Segment(space.static_body, t2, t3, line_width), pymunk.Segment(space.static_body, t3, t4, line_width) ] for s in static: s.friction = 1. s.group = 1 space.add(static) # samples for door # each sample contains information about surface point, normal and maximum radius of contacting body self.door_samples = [] for t in np.linspace(0.1, 0.9, 5, endpoint=True): self.door_samples.append( (Vec2d(self.width / 2.0 * t, -line_width / 2.0), Vec2d(0, -1), float("inf"))) radius_tab = [100, 50, 15, 5, 5] idx = 0 for t in np.linspace(0.1, 0.9, 5, endpoint=True): self.door_samples.append( (Vec2d(self.width / 2.0 * t, line_width / 2.0), Vec2d(0, 1), radius_tab[idx])) idx += 1 self.door_samples.append((Vec2d(self.width / 2.0 * 0.8 - 4 * 2.0, 4 * 4.0), Vec2d(0, 1), float("inf"))) self.door_samples.append((Vec2d(self.width / 2.0 * 0.8 - 4 * 2.0, 4 * 4.0), Vec2d(0, -1), 5)) # polygon for left door p6 = Vec2d(0, -line_width / 2.0) p7 = Vec2d(0, line_width / 2.0) p8 = Vec2d(self.width / 2.0 - line_width, line_width / 2.0) p9 = Vec2d(self.width / 2.0 - line_width, -line_width / 2.0) # ph1 = Vec2d(self.width/2.0*0.9, line_width/2.0) # ph2 = Vec2d(self.width/2.0*0.9, line_width*4.0) # ph3 = Vec2d(self.width/2.0*0.9-line_width*4.0, line_width*4.0) # ph4 = Vec2d(self.width/2.0*0.9-line_width*4.0, line_width*3.0) # ph5 = Vec2d(self.width/2.0*0.9-line_width*1.0, line_width*3.0) # ph6 = Vec2d(self.width/2.0*0.9-line_width*1.0, line_width/2.0) self.fp = [p9, p8, p7, p6] mass = 100.0 moment = pymunk.moment_for_poly(mass, self.fp) # left door self.l_door_body = pymunk.Body(mass, moment) self.l_door_body.position = t5 self.l_door_body.angle = self.rotation l_door_shape = pymunk.Poly(self.l_door_body, self.fp, radius=1) l_door_shape.group = 1 # handle left_h1_shape = pymunk.Segment( self.l_door_body, Vec2d(self.width / 2.0 * 0.8, line_width * 0.5), Vec2d(self.width / 2.0 * 0.8, line_width * 4.0), line_width) left_h1_shape.group = 1 left_h2_shape = pymunk.Segment( self.l_door_body, Vec2d(self.width / 2.0 * 0.8 - line_width * 4.0, line_width * 4.0), Vec2d(self.width / 2.0 * 0.8, line_width * 4.0), line_width) left_h2_shape.group = 1 space.add(self.l_door_body, l_door_shape, left_h1_shape, left_h2_shape) j = pymunk.PivotJoint(self.l_door_body, space.static_body, self.l_door_body.position) s = pymunk.DampedRotarySpring(self.l_door_body, space.static_body, 0, 0, 90000) space.add(j, s) j2 = pymunk.RotaryLimitJoint(self.l_door_body, space.static_body, -self.rotation - math.pi / 2.0, -self.rotation) space.add(j2)
def _setup_extra_bodies(self): super()._setup_extra_bodies() # Finger bodies/controls (annoying). self.finger_bodies = [] self.finger_motors = [] self.finger_vertices = [] self.finger_inner_vertices = [] self.target_finger_angle = 0.0 for finger_side in [-1, 1]: # Basic finger body. finger_verts = make_finger_vertices( upper_arm_len=self.finger_upper_length, forearm_len=self.finger_lower_length, thickness=self.finger_thickness, side_sign=finger_side, ) self.finger_vertices.append(finger_verts) finger_inner_verts = make_finger_vertices( upper_arm_len=self.finger_upper_length - ROBOT_LINE_THICKNESS * 2, forearm_len=self.finger_lower_length - ROBOT_LINE_THICKNESS * 2, thickness=self.finger_thickness - ROBOT_LINE_THICKNESS * 2, side_sign=finger_side, ) finger_inner_verts = [[(x, y + ROBOT_LINE_THICKNESS) for x, y in box] for box in finger_inner_verts] self.finger_inner_vertices.append(finger_inner_verts) # These are movement limits; they are useful below, but also # necessary to make initial positioning work. if finger_side < 0: lower_rot_lim = -self.finger_rot_limit_inner upper_rot_lim = self.finger_rot_limit_outer if finger_side > 0: lower_rot_lim = -self.finger_rot_limit_outer upper_rot_lim = self.finger_rot_limit_inner finger_mass = self.mass / 8 finger_inertia = pm.moment_for_poly(finger_mass, sum(finger_verts, [])) finger_body = pm.Body(finger_mass, finger_inertia) if finger_side < 0: delta_finger_angle = upper_rot_lim finger_body.angle = self.init_angle + delta_finger_angle else: delta_finger_angle = lower_rot_lim finger_body.angle = self.init_angle + delta_finger_angle # Position of finger relative to body. finger_rel_pos = ( finger_side * self.radius * 0.45, self.radius * 0.1, ) finger_rel_pos_rot = gtools.rotate_vec(finger_rel_pos, self.init_angle) finger_body.position = gtools.add_vecs(self.body.position, finger_rel_pos_rot) self.add_to_space(finger_body) self.finger_bodies.append(finger_body) # Pivot joint to keep it in place (it will rotate around this # point). finger_piv = pm.PivotJoint(self.body, finger_body, finger_body.position) finger_piv.error_bias = 0.0 self.add_to_space(finger_piv) # Rotary limit joint to stop it from getting too far out of line. finger_limit = pm.RotaryLimitJoint(self.body, finger_body, lower_rot_lim, upper_rot_lim) finger_limit.error_bias = 0.0 self.add_to_space(finger_limit) # Motor to move the fingers around (very limited in power so as not # to conflict with rotary limit joint). finger_motor = pm.SimpleMotor(self.body, finger_body, 0.0) finger_motor.rate = 0.0 finger_motor.max_bias = 0.0 finger_motor.max_force = self.phys_vars.robot_finger_max_force self.add_to_space(finger_motor) self.finger_motors.append(finger_motor)
def fill_space(space, custom_color=(255, 255, 0, 255)): captions = [] ### Static captions.append(((50, 680), "Static Shapes")) #Static Segments segments = [ pymunk.Segment(space.static_body, (10, 400), (10, 600), 0), pymunk.Segment(space.static_body, (20, 400), (20, 600), 1), pymunk.Segment(space.static_body, (30, 400), (30, 600), 3), pymunk.Segment(space.static_body, (50, 400), (50, 600), 5) ] space.add(segments) b = pymunk.Body(body_type=pymunk.Body.STATIC) b.position = (40, 630) b.angle = 3.14 / 7 s = pymunk.Segment(b, (-30, 0), (30, 0), 2) space.add(s) # Static Circles b = pymunk.Body(body_type=pymunk.Body.STATIC) b.position = (120, 630) s = pymunk.Circle(b, 10) space.add(s) b = pymunk.Body(body_type=pymunk.Body.STATIC) b.position = (120, 630) s = pymunk.Circle(b, 10, (-30, 0)) space.add(s) b = pymunk.Body(body_type=pymunk.Body.STATIC) b.position = (120, 560) b.angle = 3.14 / 4 s = pymunk.Circle(b, 40) space.add(s) # Static Polys b = pymunk.Body(body_type=pymunk.Body.STATIC) b.position = (120, 460) b.angle = 3.14 / 4 s = pymunk.Poly(b, [(0, -25), (30, 25), (-30, 25)]) space.add(s) b = pymunk.Body(body_type=pymunk.Body.STATIC) b.position = (120, 500) t = pymunk.Transform(ty=-100) s = pymunk.Poly(b, [(0, -25), (30, 25), (-30, 25)], t, radius=3) space.add(s) b = pymunk.Body(body_type=pymunk.Body.STATIC) b.position = (50, 430) t = pymunk.Transform(ty=-100) s = pymunk.Poly(b, [(0, -50), (50, 0), (30, 50), (-30, 50), (-50, 0)], t) space.add(s) ### Kinematic captions.append(((220, 680), "Kinematic Shapes")) # Kinematic Segments b = pymunk.Body(body_type=pymunk.Body.KINEMATIC) segments = [ pymunk.Segment(b, (180, 400), (180, 600), 0), pymunk.Segment(b, (190, 400), (190, 600), 1), pymunk.Segment(b, (200, 400), (200, 600), 3), pymunk.Segment(b, (220, 400), (220, 600), 5) ] space.add(segments) b = pymunk.Body(body_type=pymunk.Body.KINEMATIC) b.position = (210, 630) b.angle = 3.14 / 7 s = pymunk.Segment(b, (-30, 0), (30, 0), 2) space.add(s) # Kinematic Circles b = pymunk.Body(body_type=pymunk.Body.KINEMATIC) b.position = (290, 630) s = pymunk.Circle(b, 10) space.add(s) b = pymunk.Body(body_type=pymunk.Body.KINEMATIC) b.position = (290, 630) s = pymunk.Circle(b, 10, (-30, 0)) space.add(s) b = pymunk.Body(body_type=pymunk.Body.KINEMATIC) b.position = (290, 560) b.angle = 3.14 / 4 s = pymunk.Circle(b, 40) space.add(s) # Kinematic Polys b = pymunk.Body(body_type=pymunk.Body.KINEMATIC) b.position = (290, 460) b.angle = 3.14 / 4 s = pymunk.Poly(b, [(0, -25), (30, 25), (-30, 25)]) space.add(s) b = pymunk.Body(body_type=pymunk.Body.KINEMATIC) b.position = (290, 500) t = pymunk.Transform(ty=-100) s = pymunk.Poly(b, [(0, -25), (30, 25), (-30, 25)], t, radius=3) space.add(s) b = pymunk.Body(body_type=pymunk.Body.KINEMATIC) b.position = (230, 430) t = pymunk.Transform(ty=-100) s = pymunk.Poly(b, [(0, -50), (50, 0), (30, 50), (-30, 50), (-50, 0)], t) space.add(s) ### Dynamic captions.append(((390, 680), "Dynamic Shapes")) #Dynamic Segments b = pymunk.Body(1, 1) segments = [ pymunk.Segment(b, (350, 400), (350, 600), 0), pymunk.Segment(b, (360, 400), (360, 600), 1), pymunk.Segment(b, (370, 400), (370, 600), 3), pymunk.Segment(b, (390, 400), (390, 600), 5), ] space.add(segments) b = pymunk.Body(1, 1) b.position = (380, 630) b.angle = 3.14 / 7 s = pymunk.Segment(b, (-30, 0), (30, 0), 2) space.add(s) # Dynamic Circles b = pymunk.Body(1, 1) b.position = (460, 630) s = pymunk.Circle(b, 10) space.add(s) b = pymunk.Body(1, 1) b.position = (460, 630) s = pymunk.Circle(b, 10, (-30, 0)) space.add(s) b = pymunk.Body(1, 1) b.position = (460, 560) b.angle = 3.14 / 4 s = pymunk.Circle(b, 40) space.add(s) # Dynamic Polys b = pymunk.Body(1, 1) b.position = (460, 460) b.angle = 3.14 / 4 s = pymunk.Poly(b, [(0, -25), (30, 25), (-30, 25)]) space.add(s) b = pymunk.Body(1, 1) b.position = (460, 500) s = pymunk.Poly(b, [(0, -25), (30, 25), (-30, 25)], pymunk.Transform(ty=-100), radius=3) space.add(s) b = pymunk.Body(1, 1) b.position = (400, 430) s = pymunk.Poly(b, [(0, -50), (50, 0), (30, 50), (-30, 50), (-50, 0)], pymunk.Transform(ty=-100)) space.add(s) ###Constraints # PinJoints captions.append(((560, 660), "Pin Joints")) a = pymunk.Body(1, 1) a.position = (550, 600) sa = pymunk.Circle(a, 20) b = pymunk.Body(1, 1) b.position = (650, 620) sb = pymunk.Circle(b, 20) j = pymunk.PinJoint(a, b) space.add(sa, sb, a, b, j) a = pymunk.Body(1, 1) a.position = (550, 550) sa = pymunk.Circle(a, 20) b = pymunk.Body(1, 1) b.position = (650, 570) sb = pymunk.Circle(b, 20) j = pymunk.PinJoint(a, b, anchor_a=(0, 20), anchor_b=(0, -20)) space.add(sa, sb, a, b, j) # SlideJoints captions.append(((560, 490), "Slide Joint")) a = pymunk.Body(1, 1) a.position = (550, 430) sa = pymunk.Circle(a, 20) b = pymunk.Body(1, 1) b.position = (650, 450) sb = pymunk.Circle(b, 20) j = pymunk.SlideJoint(a, b, anchor_a=(0, 20), anchor_b=(0, -20), min=10, max=30) space.add(sa, sb, a, b, j) # PivotJoints captions.append(((560, 390), "Pivot Joint")) a = pymunk.Body(1, 1) a.position = (550, 330) sa = pymunk.Circle(a, 20) b = pymunk.Body(1, 1) b.position = (650, 350) sb = pymunk.Circle(b, 20) j = pymunk.PivotJoint(a, b, (600, 320)) space.add(sa, sb, a, b, j) # GrooveJoints captions.append(((760, 660), "Groove Joint")) a = pymunk.Body(1, 1) a.position = (750, 600) sa = pymunk.Circle(a, 20) b = pymunk.Body(1, 1) b.position = (850, 620) sb = pymunk.Circle(b, 20) j = pymunk.GrooveJoint(a, b, (790, 610), (790, 620), (840, 620)) space.add(sa, sb, a, b, j) # DampedSpring captions.append(((760, 550), "Damped Spring")) a = pymunk.Body(1, 1) a.position = (750, 480) sa = pymunk.Circle(a, 20) b = pymunk.Body(1, 1) b.position = (850, 500) sb = pymunk.Circle(b, 20) j = pymunk.DampedSpring(a, b, (0, 0), (0, 10), 100, 1, 1) space.add(sa, sb, a, b, j) # DampedRotarySpring captions.append(((740, 430), "Damped Rotary Spring")) a = pymunk.Body(1, 1) a.position = (750, 350) sa = pymunk.Circle(a, 20) b = pymunk.Body(1, 1) b.position = (850, 380) sb = pymunk.Circle(b, 20) j = pymunk.DampedRotarySpring(a, b, 10, 1, 1) space.add(sa, sb, a, b, j) # RotaryLimitJoint captions.append(((740, 300), "Rotary Limit Joint")) a = pymunk.Body(1, 1) a.position = (750, 220) sa = pymunk.Circle(a, 20) b = pymunk.Body(1, 1) b.position = (850, 250) sb = pymunk.Circle(b, 20) j = pymunk.RotaryLimitJoint(a, b, 1, 2) b.angle = 3 space.add(sa, sb, a, b, j) # RatchetJoint captions.append(((740, 170), "Ratchet Joint")) a = pymunk.Body(1, 1) a.position = (750, 100) sa = pymunk.Circle(a, 20) b = pymunk.Body(1, 1) b.position = (850, 120) sb = pymunk.Circle(b, 20) j = pymunk.RatchetJoint(a, b, 1, 0.1) b.angle = 3 space.add(sa, sb, a, b, j) # GearJoint and SimpleMotor omitted since they are similar to the already # added joints # TODO: more stuff here :) ### Other # Objects in custom color captions.append(((150, 150), "Custom Color (static & dynamic)")) b = pymunk.Body(body_type=pymunk.Body.STATIC) b.position = (200, 200) s = pymunk.Circle(b, 40) s.color = custom_color space.add(s) b = pymunk.Body(1, 1) b.position = (300, 200) s = pymunk.Circle(b, 40) s.color = custom_color space.add(s) # Collision captions.append(((450, 150), "Collisions")) b = pymunk.Body(body_type=pymunk.Body.STATIC) b.position = (470, 200) s = pymunk.Circle(b, 40) space.add(s) b = pymunk.Body(1, 1) b.position = (500, 250) s = pymunk.Circle(b, 40) space.add(s) # Sleeping captions.append(((50, 150), "Sleeping")) b = pymunk.Body(1, 1) b.position = (75, 200) space.sleep_time_threshold = 0.01 s = pymunk.Circle(b, 40) space.add(s, b) b.sleep() space.step(0.000001) return captions
space.add( pymunk.PivotJoint(b2, space.static_body, (150, 80) + Vec2d(box_offset))) c = pymunk.DampedRotarySpring(b1, b2, 0, 3000, 60) txts[box_offset] = inspect.getdoc(c) space.add(c) box_offset = 0, box_size b1 = add_lever(space, (50, 100), box_offset) b2 = add_lever(space, (150, 100), box_offset) # Add some 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))) # Hold their rotation within 90 degrees of each other. c = pymunk.RotaryLimitJoint(b1, b2, math.pi / 2, math.pi / 2) txts[box_offset] = inspect.getdoc(c) space.add(c) box_offset = box_size, box_size b1 = add_lever(space, (50, 100), box_offset) b2 = add_lever(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))) # Ratchet every 90 degrees c = pymunk.RatchetJoint(b1, b2, 0, math.pi / 2) txts[box_offset] = inspect.getdoc(c) space.add(c)
def __init__(self, b, b2, min, max, collide=True): joint = pymunk.RotaryLimitJoint(b, b2, min, max) joint.collide_bodies = collide space.add(joint)
def __init__(self, space: pymunk.Space, theta: int = 0): ''' Generate swing and robot and add to space. ''' 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(10, 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.point2 self.torso.position = (angle.point2[0] + angle.seat[1][0] + (torso_length / 2) * math.sin(angle._theta), angle.point2[1] + angle.seat[1][1] + (torso_length / 2) * math.cos(angle._theta)) self.torso._set_angle(-angle._theta) self.legs.position = (angle.point2[0] + angle.seat[0][0] - (legs_length / 2) * math.sin(angle._theta), angle.point2[1] + angle.seat[0][1] - (legs_length / 2) * math.cos(angle._theta)) self.legs._set_angle(-angle._theta) self.head.position = (angle.point2[0] + angle.seat[1][0] + (torso_length - 10) * math.sin(angle._theta), angle.point2[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.seat, 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.rod1, self.seat, anchor_a=angle.rod1, anchor_b=angle.seat[0]) self.pivot6 = pymunk.PinJoint(self.rod1, self.seat, anchor_a=angle.rod1, anchor_b=angle.seat[1]) self.pivot7 = pymunk.PivotJoint(self.torso, self.seat, (angle.point2[0] + angle.seat[1][0], angle.point2[1] + angle.seat[1][1])) self.pivot8 = pymunk.PivotJoint(self.seat, self.legs, (angle.point2[0] + angle.seat[0][0], angle.point2[1] + angle.seat[0][1])) self.pivot9 = pymunk.PivotJoint(self.head, self.torso, self.head.position) # self.arms = pymunk.DampedSpring(self.torso, self.rod3, anchor_a=(0,0), anchor_b=(0,0), rest_length=25, stiffness=100, damping=0.5) self.gear1 = pymunk.RotaryLimitJoint(self.torso, self.seat, -0.1, 0.5) self.gear2 = pymunk.RotaryLimitJoint(self.seat, self.legs, -1.2, 0.9) # 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.seat, self.seat_shape, self.torso, self.torso_shape, self.legs, self.legs_shape, self.head, self.head_shape, self.pivot1, self.pivot2, self.pivot5, self.pivot6, self.pivot7, self.pivot8, self.pivot9, self.gear1, self.gear2)