def create_hill(space, sprite_list, start_x, y, count):
""" Create a hill """
# Left edge
sprite = PymunkSprite("./images/tiles/grassHill_right.png", start_x - constants.SPRITE_SIZE, y, scale=constants.SPRITE_SCALING, body_type=pymunk.Body.STATIC)
t = pymunk.Transform(tx=-60, ty=-96)
sprite.shape = pymunk.Poly(sprite.body, [(80, 120), (80, 60), (20, 60)], transform=t)
sprite.shape.friction = constants.DEFAULT_FRICTION / 4
sprite_list.append(sprite)
space.add(sprite.body, sprite.shape)
# Add dirt under
dirt_y = 0
sprite = PymunkSprite("./images/tiles/grassCorner_right.png", start_x - constants.SPRITE_SIZE, dirt_y, scale=constants.SPRITE_SCALING, body_type=pymunk.Body.STATIC)
sprite_list.append(sprite)
# Middle
for x in range(start_x, start_x + count * constants.SPRITE_SIZE + 1, constants.SPRITE_SIZE):
sprite = PymunkSprite("./images/tiles/grassMid.png", x, y, scale=constants.SPRITE_SCALING, body_type=pymunk.Body.STATIC)
sprite_list.append(sprite)
space.add(sprite.body, sprite.shape)
# Add dirt under
dirt_y = 0
sprite = PymunkSprite("./images/tiles/grassCenter.png", x, dirt_y, scale=constants.SPRITE_SCALING, body_type=pymunk.Body.STATIC)
sprite_list.append(sprite)
# Right edge
sprite = PymunkSprite("./images/tiles/grassHill_left.png", start_x + constants.SPRITE_SIZE * count + constants.SPRITE_SIZE, y, scale=constants.SPRITE_SCALING, body_type=pymunk.Body.STATIC)
sprite.shape.friction = constants.DEFAULT_FRICTION / 4
t = pymunk.Transform(tx=-60, ty=-96)
sprite.shape = pymunk.Poly(sprite.body, [(20, 120), (20, 60), (80, 60)], transform=t)
sprite_list.append(sprite)
space.add(sprite.body, sprite.shape)
# Add dirt under
dirt_y = 0
sprite = PymunkSprite("./images/tiles/grassCorner_left.png", start_x + constants.SPRITE_SIZE * count + constants.SPRITE_SIZE, dirt_y, scale=constants.SPRITE_SCALING, body_type=pymunk.Body.STATIC)
sprite_list.append(sprite)
def car(self, space):
pos = Vec2d(100, 100)
wheel_color = 0.2, 0.86, 0.47
shovel_color = 0.86, 0.47, 0.2
mass = 100
radius = 25
moment = pymunk.moment_for_circle(mass, 20, radius)
wheel1_b = pymunk.Body(mass, moment)
wheel1_s = pymunk.Circle(wheel1_b, radius)
wheel1_s.friction = 1.5
wheel1_s.color = wheel_color
space.add(wheel1_b, wheel1_s)
mass = 100
radius = 25
moment = pymunk.moment_for_circle(mass, 20, radius)
wheel2_b = pymunk.Body(mass, moment)
wheel2_s = pymunk.Circle(wheel2_b, radius)
wheel2_s.friction = 1.5
wheel2_s.color = wheel_color
space.add(wheel2_b, wheel2_s)
mass = 100
size = (50, 30)
moment = pymunk.moment_for_box(mass, size)
chassi_b = pymunk.Body(mass, moment)
chassi_s = pymunk.Poly.create_box(chassi_b, size)
space.add(chassi_b, chassi_s)
vs = [(0, 0), (0, -45), (25, -45)]
shovel_s = pymunk.Poly(chassi_b, vs, transform=pymunk.Transform(tx=85))
shovel_s.friction = 0.5
shovel_s.color = shovel_color
space.add(shovel_s)
wheel1_b.position = pos - (55, 0)
wheel2_b.position = pos + (55, 0)
chassi_b.position = pos + (0, 25)
space.add(
pymunk.PinJoint(wheel1_b, chassi_b, (0, 0), (-25, -15)),
pymunk.PinJoint(wheel1_b, chassi_b, (0, 0), (-25, 15)),
pymunk.PinJoint(wheel2_b, chassi_b, (0, 0), (25, -15)),
pymunk.PinJoint(wheel2_b, chassi_b, (0, 0), (25, 15)),
)
speed = -4
space.add(
pymunk.SimpleMotor(wheel1_b, chassi_b, speed),
pymunk.SimpleMotor(wheel2_b, chassi_b, speed),
)
with self.canvas:
Color(*wheel_color)
wheel1_s.ky = self.ellipse_from_circle(wheel1_s)
Color(*wheel_color)
wheel2_s.ky = self.ellipse_from_circle(wheel2_s)
Color(*shovel_color)
chassi_s.ky = Quad(points=self.points_from_poly(chassi_s))
shovel_s.ky = Triangle(points=self.points_from_poly(shovel_s))
def __init__(self, app, pos, angle):
self.app = app
t = pymunk.Transform(a=self.size[0],
b=0,
c=0,
d=self.size[1],
tx=pos[0],
ty=pos[2])
self.collider = pymunk.Poly(self.app.physics.space.static_body,
vertices=[(i, j) for i in (-1, 1)
for j in (-1, 1)],
transform=t)
self.collider.y = pos[1]
self.collider.height = self.height
self.collider.collision_type = physics.STATIC
self.app.physics.space.add(self.collider)
self.canvas = Canvas()
with self.canvas:
PushMatrix()
self.pos = Translate(*pos)
self.rot = Rotate(angle, 0, 1, 0)
self.app.graphic_data.draw_mesh('models/staircase',
self.canvas,
texture=None)
PopMatrix()
def generateBodyAndShape(self, physObj, body=None, rel_pos=(0, 0)):
if body is None:
moment = pymunk.moment_for_poly(physObj.mass, self.verts)
body = pymunk.Body(
physObj.mass, moment, body_type=pymunk.Body.STATIC if physObj.static else pymunk.Body.DYNAMIC
)
shape = pymunk.Poly(
body,
self.verts,
transform=pymunk.Transform(
a=np.cos(physObj.rotation),
b=np.sin(physObj.rotation),
c=-np.sin(physObj.rotation),
d=np.cos(physObj.rotation),
tx=rel_pos[0],
ty=rel_pos[1],
),
)
shape.friction = physObj.friction_coefficient
shape.elasticity = physObj.restitution_coefficient
shape.collision_type = 1
shape.sensor = physObj.sensor
from ev3sim.objects.base import STATIC_CATEGORY, DYNAMIC_CATEGORY
shape.filter = pymunk.ShapeFilter(categories=STATIC_CATEGORY if physObj.static else DYNAMIC_CATEGORY)
return body, shape
def __init__(self,
x,
y,
segments=[[-1, 0], [0, 1], [1, 0]],
velocity=(0, 0),
frame_size=(0, 0),
transform=pymunk.Transform(1, 0, 0, 1, 0, 0)):
print("Creating cylinder: ", x, y, len(segments), frame_size,
(transform.tx, transform.ty))
self.transform = transform
self.frame_size = frame_size # is the size of the matrix/image the cylindar (mat file)
# was stored. Use to convert from opencv frame to pymunk
# center of cilynder
self.x = x
self.y = y
self.velocity = velocity
# defined by vertices in relative pose (center)
self.segments = segments
# bodies an shapes for pymunk
self.bodies_segments = []
self.shapes_segments = []
self.deb_maxp = [0, 0]
self.deb_maxpm = [0, 0]
self.segment_mass = 0.1
self.segment_thickness = 20
self.create_segments()
def testCircleNoBody(self):
s = p.Space()
c = p.Circle(None,5)
bb = c.update(p.Transform(1, 2, 3, 4, 5, 6))
self.assertEqual(c.bb, bb)
self.assertEqual(c.bb, p.BB(0, 1, 10, 11))
def __init__(self, id, position, angle, space, window_height):
self.height = 30 * 3
self.window_height = window_height
self.w_1 = 10
self.w_2 = 10
self.id = id
self.size = (10, 60)
self.mass = 20
self.body = pymunk.Body(self.mass,
pymunk.inf,
body_type=pymunk.Body.KINEMATIC)
self.shape = pymunk.Poly(
self.body,
((0, 0), (self.w_1, 0), (self.w_1, self.height), (0, self.height)),
transform=pymunk.Transform(tx=0, ty=-self.height / 2))
self.shape.body.position = position
self.shape.body.angle = angle
self.shape.elasticity = 1.0
self.shape.friction = 100.0
self.shape.thing = self
space.add(self.body, self.shape)
self.velocity = [
Vec2d(0, 0),
Vec2d(1, 0),
Vec2d(-1, 0),
Vec2d(0, -1),
Vec2d(0, 1)
]
def set_scale(self, step):
"""Set the scale of the symbol to the scaling function at the given time step.
@param step: The current time step of the environment
"""
self.scale = self.scale_fn(step)
self.shape.unsafe_set_vertices(self._base_vertices,
transform=pm.Transform(self.scale, 0, 0, self.scale, 0, 0))
def create_transform(self):
sprite = self.sprite
if not sprite:
return None
a = sprite.width / sprite.texture.width
d = sprite.height / sprite.texture.height
#t = pymunk.Transform(a=a, d=d, tx=-center.x, ty=-center.y)
t = pymunk.Transform(a=a, d=d)
return t
def setup_pymunk_shape(self, px_mm_scale, body):
width = self.width_mm * px_mm_scale
height = self.height_mm * px_mm_scale
vs = [(0, 0), (width, 0), (width, height), (0, height)]
t = pymunk.Transform(tx=self.x_offset * px_mm_scale - width / 2,
ty=self.y_offset * px_mm_scale - height / 2)
self.shape = pymunk.Poly(body, vs, transform=t)
self.shape.friction = DEFAULT_FRICTION
self.shape.mass = DEFAULT_MASS
def testInit(self) -> None:
t = p.Transform(1, 2, 3, 4, 5, 6)
self.assertEqual(t.a, 1)
self.assertEqual(t.b, 2)
self.assertEqual(t.c, 3)
self.assertEqual(t.d, 4)
self.assertEqual(t.tx, 5)
self.assertEqual(t.ty, 6)
self.assertEqual(str(t), "Transform(a=1, b=2, c=3, d=4, tx=5, ty=6)")
t = p.Transform(b=4, ty=2)
self.assertEqual(t.a, 1)
self.assertEqual(t.b, 4)
self.assertEqual(t.c, 0)
self.assertEqual(t.d, 1)
self.assertEqual(t.tx, 0)
self.assertEqual(t.ty, 2)
def testVertices(self):
vs = [(-10,10), (0,0),(20,0),(10,10)]
c = p.Poly(None, vs, None, 0)
self.assertEqual(c.get_vertices(), vs)
c = p.Poly(None, vs, p.Transform(1,2,3,4,5,6), 0)
vs2 = [(5.0, 6.0), (25.0, 26.0), (45.0, 66.0), (25.0, 46.0)]
self.assertEqual(c.get_vertices(), vs2)
def car(space):
import pymunk
from pymunk.vec2d import Vec2d
space.gravity = 0, -9.8
pos = Vec2d(300, 200)
wheel_color = 52, 219, 119
shovel_color = 219, 119, 52
mass = 100
radius = 25
moment = pymunk.moment_for_circle(mass, 20, radius)
wheel1_b = pymunk.Body(mass, moment)
wheel1_s = pymunk.Circle(wheel1_b, radius)
wheel1_s.friction = 1.5
wheel1_s.color = wheel_color
space.add(wheel1_b, wheel1_s)
mass = 100
radius = 25
moment = pymunk.moment_for_circle(mass, 20, radius)
wheel2_b = pymunk.Body(mass, moment)
wheel2_s = pymunk.Circle(wheel2_b, radius)
wheel2_s.friction = 1.5
wheel2_s.color = wheel_color
space.add(wheel2_b, wheel2_s)
mass = 100
size = (50, 30)
moment = pymunk.moment_for_box(mass, size)
chassi_b = pymunk.Body(mass, moment)
chassi_s = pymunk.Poly.create_box(chassi_b, size)
space.add(chassi_b, chassi_s)
vs = [(0, 0), (25, -45), (0, -45)]
shovel_s = pymunk.Poly(chassi_b, vs, transform=pymunk.Transform(tx=85))
shovel_s.friction = 0.5
shovel_s.color = shovel_color
space.add(shovel_s)
wheel1_b.position = pos - (55, 0)
wheel2_b.position = pos + (55, 0)
chassi_b.position = pos + (0, 25)
space.add(pymunk.PinJoint(wheel1_b, chassi_b, (0, 0), (-25, -15)),
pymunk.PinJoint(wheel1_b, chassi_b, (0, 0), (-25, 15)),
pymunk.PinJoint(wheel2_b, chassi_b, (0, 0), (25, -15)),
pymunk.PinJoint(wheel2_b, chassi_b, (0, 0), (25, 15)))
speed = 4
space.add(pymunk.SimpleMotor(wheel1_b, chassi_b, speed),
pymunk.SimpleMotor(wheel2_b, chassi_b, speed))
floor = pymunk.Segment(space.static_body, (-100, 100), (1000, 100), 5)
floor.friction = 1.0
space.add(floor)
def add_to_space(self, position: Tuple[float, float], rotation: float):
vehicle = pymunk.Body() # 1
vehicle.position = position # 2
vehicle.angle = rotation
vehicle.velocity_func = self.drag_callback
body = pymunk.Circle(vehicle, self.body_radius) # 3
body.mass = self.body_mass # 4
body.friction = 1 # Asumme no surface friction
left_motor = pymunk.Poly(
vehicle,
[
(self.side_motor_length / 2, self.side_motor_width / 2),
(-self.side_motor_length / 2, self.side_motor_width / 2),
(-self.side_motor_length / 2, -self.side_motor_width / 2),
(self.side_motor_length / 2, -self.side_motor_width / 2),
],
transform=pymunk.Transform(ty=self.body_radius +
self.side_motor_length / 2),
radius=0.001,
)
left_motor.mass = self.side_motor_mass
left_motor.friction = 1
right_motor = pymunk.Poly(
vehicle,
[
(self.side_motor_length / 2, self.side_motor_width / 2),
(-self.side_motor_length / 2, self.side_motor_width / 2),
(-self.side_motor_length / 2, -self.side_motor_width / 2),
(self.side_motor_length / 2, -self.side_motor_width / 2),
],
transform=pymunk.Transform(ty=-self.body_radius -
self.side_motor_length / 2),
radius=0.001,
)
right_motor.mass = self.side_motor_mass
right_motor.friction = 1
self.space.add(vehicle, body, left_motor, right_motor)
return vehicle
def rect(data):
transform = pymunk.Transform(tx=-data["width"] / 2, ty=-data["height"] / 2)
collider = pymunk.Poly(None,
vertices=[(data["x"], data["y"]),
(data["x"] + data["width"], data["y"]),
(data["x"] + data["width"],
data["y"] + data["width"]),
(data["x"], data["y"] + data["width"])],
transform=transform,
radius=data["radius"])
collider.collision_type = data["collision_type"]
return collider
def setup_pymunk_shape(self, px_mm_scale, body):
"""
Create the shape of a body part as a square.
"""
width = self.width_mm * px_mm_scale
height = self.height_mm * px_mm_scale
vertices = [(0, 0), (width, 0), (width, height), (0, height)]
transform = pymunk.Transform(tx=self.x_offset * px_mm_scale - width/2,
ty=self.y_offset * px_mm_scale - height/2)
self.shape = pymunk.Poly(body, vertices, transform=transform)
self.shape.friction = DEFAULT_FRICTION
self.shape.mass = DEFAULT_MASS
def car(space):
pos = Vec2d(100, 200)
wheel_color = 52, 219, 119, 255
shovel_color = 219, 119, 52, 255
mass = 100
radius = 25
moment = pymunk.moment_for_circle(mass, 20, radius)
wheel1_b = pymunk.Body(mass, moment)
wheel1_s = pymunk.Circle(wheel1_b, radius)
wheel1_s.friction = 1.5
wheel1_s.color = wheel_color
space.add(wheel1_b, wheel1_s)
mass = 100
radius = 25
moment = pymunk.moment_for_circle(mass, 20, radius)
wheel2_b = pymunk.Body(mass, moment)
wheel2_s = pymunk.Circle(wheel2_b, radius)
wheel2_s.friction = 1.5
wheel2_s.color = wheel_color
space.add(wheel2_b, wheel2_s)
mass = 100
size = (50, 30)
moment = pymunk.moment_for_box(mass, size)
chassi_b = pymunk.Body(mass, moment)
chassi_s = pymunk.Poly.create_box(chassi_b, size)
space.add(chassi_b, chassi_s)
vs = [(0, 0), (25, 45), (0, 45)]
shovel_s = pymunk.Poly(chassi_b, vs, transform=pymunk.Transform(tx=85))
shovel_s.friction = 0.5
shovel_s.color = shovel_color
space.add(shovel_s)
wheel1_b.position = pos - (55, 0)
wheel2_b.position = pos + (55, 0)
chassi_b.position = pos + (0, -25)
space.add(
pymunk.PinJoint(wheel1_b, chassi_b, (0, 0), (-25, -15)),
pymunk.PinJoint(wheel1_b, chassi_b, (0, 0), (-25, 15)),
pymunk.PinJoint(wheel2_b, chassi_b, (0, 0), (25, -15)),
pymunk.PinJoint(wheel2_b, chassi_b, (0, 0), (25, 15)),
)
speed = 4
space.add(
pymunk.SimpleMotor(wheel1_b, chassi_b, speed),
pymunk.SimpleMotor(wheel2_b, chassi_b, speed),
)
def create_logo_lines(self, logo_img):
logo_bb = pymunk.BB(0, 0, logo_img.width, logo_img.height)
def sample_func(point):
try:
color = logo_img.read_pixel(point.x, point.y)
return color[3] * 255
except:
return 0
line_set = pymunk.autogeometry.PolylineSet()
def segment_func(v0, v1):
line_set.collect_segment(v0, v1)
pymunk.autogeometry.march_soft(logo_bb, logo_img.width,
logo_img.height, 99, segment_func,
sample_func)
r = 10
letter_group = 0
lines = []
for line in line_set:
line = pymunk.autogeometry.simplify_curves(line, .7)
max_x = 0
min_x = 1000
max_y = 0
min_y = 1000
for l in line:
max_x = max(max_x, l.x)
min_x = min(min_x, l.x)
max_y = max(max_y, l.y)
min_y = min(min_y, l.y)
w, h = max_x - min_x, max_y - min_y
# we skip the line which has less than 35 height, since its the "hole" in
# the p in pymunk, and we dont need it.
if h < 35:
continue
center = Vec2d(min_x + w / 2., min_y + h / 2.)
t = pymunk.Transform(a=1.0, d=1.0, tx=-center.x, ty=-center.y)
r += 30
if r > 255:
r = 0
line = [Vec2d(l.x, 300 - l.y) for l in line]
lines.append(line)
return lines
def create_wheel(self, wheel_side):
if wheel_side not in ['rear', 'front']:
raise Exception('Wheel position must be front or rear')
wheel_objects = []
wheel_mass = getattr(self, wheel_side + '_wheel_mass')
wheel_radius = getattr(self, wheel_side + '_wheel_radius')
wheel_position = getattr(self, wheel_side + '_wheel_position')
wheel_friction = getattr(self, wheel_side + '_wheel_friction')
wheel_elasticity = getattr(self, wheel_side + '_wheel_elasticity')
wheel_joint = getattr(self, wheel_side + '_wheel_joint')
wheel_damp_position = getattr(self, wheel_side + '_wheel_damp_position')
wheel_damp_length = getattr(self, wheel_side + '_wheel_damp_length')
wheel_damp_stiffness = getattr(self, wheel_side + '_wheel_damp_stiffness')
wheel_damp_damping = getattr(self, wheel_side + '_wheel_damp_damping')
wheel_body = pymunk.Body(wheel_mass, pymunk.moment_for_circle(wheel_mass, 0, wheel_radius))
wheel_body.position = (wheel_position[0] * self.x_modification, wheel_position[1])
wheel_shape = pymunk.Circle(wheel_body, wheel_radius)
wheel_shape.filter = pymunk.ShapeFilter(group=self.car_group)
wheel_shape.color = 255, 34, 150
wheel_shape.friction = wheel_friction
wheel_shape.elasticity = wheel_elasticity
wheel_objects.append(wheel_shape)
wheel_joint = pymunk.PinJoint(wheel_body, self.car_body, anchor_b=(wheel_joint[0] * self.x_modification, wheel_joint[1]))
wheel_objects.append(wheel_joint)
wheel_damp = pymunk.DampedSpring(wheel_body, self.car_body, anchor_a=(0, 0),
anchor_b=(wheel_damp_position[0] * self.x_modification, wheel_damp_position[1]),
rest_length=wheel_damp_length,
stiffness=wheel_damp_stiffness,
damping=wheel_damp_damping)
wheel_objects.append(wheel_damp)
wheel_stop = pymunk.Poly(self.car_body, [(0, 0), (0, 1), (wheel_radius * 2 * self.x_modification, 1), (wheel_radius * 2 * self.x_modification, 0)],
transform=pymunk.Transform(tx=wheel_damp_position[0] * self.x_modification - wheel_radius * self.x_modification, ty=wheel_damp_position[1]))
wheel_objects.append(wheel_stop)
wheel_stop.color = 0, 255, 0
wheel_motor = None
if (wheel_side == 'rear' and self.drive in [self.AWD, self.FR]) or (wheel_side == 'front' and self.drive in [self.AWD, self.FF]):
wheel_motor = pymunk.SimpleMotor(wheel_body, self.car_body, 0)
# wheel_objects.append(motor)
return wheel_body, wheel_motor, wheel_objects
def can_place_entity(self, entity, expansion: float = 0.0) -> bool:
"""
Checks if an entity could be placed without colliding with other entities.
Currently designed to only work with polygonal entities belonging to CollisionCategory.dynamic.
"""
assert entity.category == EntityCategory.dynamic, 'only dynamic entities are supported'
space = self.space
for body in entity.bodies:
for shape in entity.shapes:
assert isinstance(
shape,
pymunk.Poly), 'Only polygon based entities are supported.'
# vertices = [body.local_to_world(vert) for vert in shape.get_vertices()]
# print('check ', entity.position, [body.local_to_world(vert) for vert in shape.get_vertices()])
# poly = pymunk.Poly(None, vertices, radius=shape.radius * 10)
poly = pymunk.Poly(None,
shape.get_vertices(),
radius=shape.radius + expansion)
c = math.cos(body.angle)
s = math.sin(body.angle)
poly.update(
pymunk.Transform(c, s, -s, c, body.position.x,
body.position.y))
# print('check ', poly.radius, shape.get_vertices(), poly.get_vertices())
# poly = pymunk.Poly(None, shape.get_vertices(), radius=0.0)
# shape.cache_bb()
# sqis2 = space.shape_query(shape)
sqis = space.shape_query(poly)
# if len(sqis) < len(sqis2):
# print('what ', sqis, sqis2)
for sqi in sqis:
other_entity = sqi.shape.body.entity
if other_entity == entity:
continue
entity_category = other_entity.category
if entity_category == EntityCategory.dynamic: # or entity_category == EntityCategory.off_road:
# print('can\'t place: ', other_entity.type_, other_entity.category, other_entity.position,
# entity.position)
return False
# print('pass')
return True
def __init__(self, id, position, angle, space, window_height):
self.height = 30 * 3
self.window_height = window_height
self.w_1 = 2
self.w_2 = 10
self.id = id
self.size = (10, 60)
self.mass = 200000000
self.body = pymunk.Body(self.mass, pymunk.inf, body_type=pymunk.Body.DYNAMIC)
self.shape = pymunk.Poly(self.body, ((0, 0), (self.w_1, 0), (self.w_1 + self.w_2, self.height / 3),
(self.w_1 + self.w_2, (self.height * 2) / 3), (self.w_1, self.height),
(0, self.height)),
transform=pymunk.Transform(tx=0, ty=-self.height / 2))
self.shape.body.position = position
self.shape.body.angle = angle
self.shape.elasticity = 1.0
self.shape.friction = 100.0
self.shape.thing = self
space.add(self.body, self.shape)
def create_collision_shape(cls, img, body, threshold=240):
poly_points = []
px_arr = pygame.PixelArray(img)
min_y = img.get_height()
min_x = img.get_width()
for x in range(px_arr.shape[1]):
for y in range(px_arr.shape[0]):
px = 0xFF & px_arr[y][x]
if px < threshold and px != 0:
if x < min_x:
min_x = x
if y < min_y:
min_y = y
poly_points.append((y, x))
hull = autogeometry.to_convex_hull(poly_points, 0.1)
return pymunk.shapes.Poly(body, hull,
transform=pymunk.Transform(d=-1)), (min_y,
min_x)
def add_robot(self, model, q0, w, mu, dynamic=False):
self.q = np.copy(q0)
self.dq = np.zeros_like(q0)
self.a_cmd = np.zeros_like(q0)
self.v_cmd = np.zeros_like(q0)
# ground
# self.add_ground(-model.bh)
# base
base_type = pymunk.Body.DYNAMIC if dynamic else pymunk.Body.KINEMATIC
base_body = pymunk.Body(body_type=base_type)
bx, by = model.base_corners(q0)
by += 0.5*model.bh
base = pymunk.Poly(
base_body,
[(x, y) for x, y in zip(bx, by)],
pymunk.Transform(tx=0, ty=-0.5*model.bh))
base.friction = 0
# add the mass manually but let pymunk figure out the moment
base.mass = model.mb
self.space.add(base.body, base)
# arm link 1
ax, ay = model.arm_points(q0)
dx1 = 0.5*model.l1*np.cos(q0[1])
dy1 = 0.5*model.l1*np.sin(q0[1])
link1_body = pymunk.Body(mass=model.m1, moment=model.I1)
link1_body.position = (ax[0] + dx1, ay[0] + dy1)
link1_body.angle = q0[1]
link1 = pymunk.Segment(link1_body, (-0.5*model.l1, 0),
(0.5*model.l1, 0), radius=0.05)
link1.friction = 0.25
# links don't collide with anything for now
link1.filter = pymunk.ShapeFilter(categories=0)
self.space.add(link1.body, link1)
# arm joint 1
joint1 = pymunk.PinJoint(base.body, link1.body, (0, 0),
(-0.5*model.l1, 0))
joint1.collide_bodies = False
self.space.add(joint1)
# arm link 2
dx2 = 0.5*model.l2*np.cos(q0[1]+q0[2])
dy2 = 0.5*model.l2*np.sin(q0[1]+q0[2])
link2_body = pymunk.Body(mass=model.m2, moment=model.I2)
link2_body.position = (ax[1] + dx2, ay[1] + dy2)
link2_body.angle = q0[1] + q0[2]
link2 = pymunk.Segment(link2_body, (-0.5*model.l2, 0),
(0.5*model.l2, 0), radius=0.05)
link2.friction = 0.25
link2.filter = pymunk.ShapeFilter(categories=0)
self.space.add(link2.body, link2)
# arm joint 2
joint2 = pymunk.PinJoint(link1.body, link2.body, (0.5*model.l1, 0),
(-0.5*model.l2, 0))
joint2.collide_bodies = False
self.space.add(joint2)
# end effector
fr = 0.05
ee_body = pymunk.Body()
ee_body.position = (ax[2], ay[2])
ee_body.angle = np.sum(q0[1:4])
ee_palm = pymunk.Segment(ee_body, (-w, 0), (w, 0), radius=0)
ee_palm.mass = 0.1
ee_palm.filter = pymunk.ShapeFilter(categories=0)
ee_finger1 = pymunk.Circle(ee_body, fr, (-w, 0))
ee_finger2 = pymunk.Circle(ee_body, fr, (w, 0))
# Set the finger friction to 1.0 regardless of the tray's friction,
# because pymunk calculates the friction coefficient between two shapes
# by multiplying each of their friction values together. We want the
# friction to always be the tray's friction.
ee_finger1.friction = 1.0
ee_finger2.friction = 1.0
ee_finger1.mass = 0.1
ee_finger2.mass = 0.1
ee_finger1.collision_type = 1
ee_finger2.collision_type = 1
self.space.add(ee_body, ee_palm, ee_finger1, ee_finger2)
ee_marker = Marker(ee_body)
# arm joint 3: link 2 to EE
joint3 = pymunk.PinJoint(link2.body, ee_body, (0.5*model.l2, 0), (0, 0))
joint3.collide_bodies = False
self.space.add(joint3)
self.model = model
self.links = [base.body, link1.body, link2.body, ee_body]
self.markers = [ee_marker]
motor1 = pymunk.constraints.SimpleMotor(self.links[0], self.links[1], 0)
motor2 = pymunk.constraints.SimpleMotor(self.links[1], self.links[2], 0)
motor3 = pymunk.constraints.SimpleMotor(self.links[2], self.links[3], 0)
self.space.add(motor1, motor2, motor3)
self.motors = [motor1, motor2, motor3]
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
draw_bounding_hull = False
if __name__ == "__main__":
wp_radius = 100
wp_center = (
window_sx / 2.0, window_sy / 2.0
) # only for simulator frame (transform when input and output ing the data)
# transform all the plotted shapes
#
# a c tx
# b d ty
main_transform = pymunk.Transform(a=1,
b=0,
c=0,
d=1,
tx=wp_center[0],
ty=wp_center[1])
# Iteration over all arguments:
print("Parameters: ", len(sys.argv))
if len(sys.argv) <= 1:
'''
cyl1 = Cylinder(280, 300,
[ [0, 200], [-10, 180], [10, 100], [-50, 0], [10, -100], [-10, -180], [0, -200] ],
(5, 0), (0, 0))
cyl2 = Cylinder(520, 300,
[ [0, 200], [10, 180], [-10, 100], [0, 0], [-10, -100], [10, -180], [0, -200] ],
(-5, 0), (0, 0))
'''
print("input error: provide a mat file with profiles")
quit()
def create_shapes(self):
if self.mounted:
transform = pymunk.Transform(ty=self.height/2 + SLOP)
return self.geom.create_shapes(self, transform)
else:
return self.geom.create_shapes(self)
max_y = 0
min_y = 1000
for l in line:
max_x = max(max_x, l.x)
min_x = min(min_x, l.x)
max_y = max(max_y, l.y)
min_y = min(min_y, l.y)
w, h = max_x - min_x, max_y - min_y
# we skip the line which has less than 35 height, since its the "hole" in
# the p in pymunk, and we dont need it.
if h < 35:
continue
center = Vec2d(min_x + w / 2.0, min_y + h / 2.0)
t = pymunk.Transform(a=1.0, d=1.0, tx=-center.x, ty=-center.y)
r += 30
if r > 255:
r = 0
if True:
for i in range(len(line) - 1):
shape = pymunk.Segment(space.static_body, line[i], line[i + 1], 1)
shape.friction = 0.5
shape.color = (255, 255, 255, 255)
space.add(shape)
floor = pymunk.Segment(space.static_body, (-100, 300), (1000, 220), 5)
floor.friction = 1.0
space.add(floor)
def __init__(self, app, data, pos=(0, 0, 0), angle=0):
self.app = app
rads = angle / 57.3
cos = math.cos(rads)
sin = math.sin(rads)
transform = pymunk.Transform(a=cos, b=-sin,
c=sin, d=cos,
tx=pos[0], ty=pos[2])
f = pymunk.ShapeFilter(categories=physics.STATIC_FILTER)
self.colliders = []
for c in data['colliders']:
vertices = zip(c[0][::2], c[0][1::2])
collider = pymunk.Poly(self.app.physics.space.static_body,
vertices=vertices,
transform=transform)
collider.y = pos[1] + c[1]
collider.height = c[2]
collider.collision_type = physics.STATIC
collider.filter = f
self.app.physics.space.add(collider)
self.colliders.append(collider)
for x, y, z in data['stairs']:
stairs_x = pos[0] + (cos * x + sin * z)
stairs_y = pos[1] + y
stairs_z = pos[2] + (-sin * x + cos * z)
d = stairs.Staircase(self.app,
pos=(stairs_x, stairs_y, stairs_z),
angle=angle)
self.app.game_manager.game_objects.add(d)
self.app.renderer.scene.add(d.canvas)
self.doors = []
for x, y, z in data['doors']:
door_x = pos[0] + (cos * x + sin * z)
door_y = pos[1] + y
door_z = pos[2] + (-sin * x + cos * z)
d = door.Door(self.app,
pos=(door_x, door_y, door_z),
angle=angle)
self.doors.append(d)
self.app.game_manager.game_objects.add(d)
self.app.renderer.scene.add(d.canvas)
self.canvas = Canvas()
with self.canvas:
PushMatrix()
Color(1, 1, 1)
self.pos = Translate(*pos)
Rotate(angle, 0, 1, 0)
self.app.graphic_data.draw_mesh(data['mesh_name'],
self.canvas,
texture=data['texture'])
PopMatrix()