def __init__(self, x0, target, render):
self.render = render
if self.render:
super(CarWorld, self).__init__()
else:
self.world = b2.b2World(gravity=(0, 0), doSleep=True)
self.world.gravity = (0.0, 0.0)
self.initial_position = (x0[0], x0[1])
self.initial_angle = 0
ground = self.world.CreateBody(position=(0, 20))
ground.CreateEdgeChain(
[(-10, -30),
(-10, 5),
(-30, 30),]
)
ground.CreateEdgeChain(
[(10, -30),
(10, 5),
(-10, 30),]
)
xf1 = b2.b2Transform()
xf1.angle = 0.3524 * b2.b2_pi
xf1.position = b2.b2Mul(xf1.R, (1.0, 0.0))
xf2 = b2.b2Transform()
xf2.angle = -0.3524 * b2.b2_pi
xf2.position = b2.b2Mul(xf2.R, (-1.0, 0.0))
self.car = TDCar(self.world, position=self.initial_position, angle=self.initial_angle)
self.target = self.world.CreateStaticBody(
position=target[:2],
angle=self.initial_angle,
shapes=[b2.b2PolygonShape(vertices=[xf1*(-1, 0), xf1*(1, 0),
xf1*(0, .5)]),
b2.b2PolygonShape(vertices=[xf2*(-1, 0), xf2*(1, 0),
xf2*(0, .5)])],
)
self.target.active = False
self.start = self.world.CreateStaticBody(
position=self.initial_position,
angle=self.initial_angle,
shapes=[b2.b2PolygonShape(vertices=[xf1*(-1, 0), xf1*(1, 0),
xf1*(0, .5)]),
b2.b2PolygonShape(vertices=[xf2*(-1, 0), xf2*(1, 0),
xf2*(0, .5)])],
)
self.start.active = False
def __init__(self, x0, target, render):
self.render = render
if self.render:
super(PointMassWorld, self).__init__()
else:
self.world = b2.b2World(gravity=(0, -10), doSleep=True)
x0 = np.asfarray(x0)
target = np.asfarray(target)
self.world.gravity = (0.0, 0.0)
self.initial_position = (x0[0], x0[1])
self.initial_angle = b2.b2_pi
self.initial_linear_velocity = (x0[2], x0[3])
self.initial_angular_velocity = 0
ground = self.world.CreateBody(position=(0, 20))
ground.CreateEdgeChain([(-20, -20), (-20, 20), (20, 20), (20, -20),
(-20, -20)])
xf1 = b2.b2Transform()
xf1.angle = 0.3524 * b2.b2_pi
xf1.position = b2.b2Mul(xf1.R, (1.0, 0.0))
xf2 = b2.b2Transform()
xf2.angle = -0.3524 * b2.b2_pi
xf2.position = b2.b2Mul(xf2.R, (-1.0, 0.0))
self.body = self.world.CreateDynamicBody(
position=self.initial_position,
angle=self.initial_angle,
linearVelocity=self.initial_linear_velocity,
angularVelocity=self.initial_angular_velocity,
angularDamping=5,
linearDamping=0.1,
shapes=[
b2.b2PolygonShape(
vertices=[xf1 * (-1, 0), xf1 * (1, 0), xf1 * (0, .5)]),
b2.b2PolygonShape(
vertices=[xf2 * (-1, 0), xf2 * (1, 0), xf2 * (0, .5)])
],
shapeFixture=b2.b2FixtureDef(density=1.0),
)
self.target = self.world.CreateStaticBody(
position=target[:2],
angle=self.initial_angle,
shapes=[
b2.b2PolygonShape(
vertices=[xf1 * (-1, 0), xf1 * (1, 0), xf1 * (0, .5)]),
b2.b2PolygonShape(
vertices=[xf2 * (-1, 0), xf2 * (1, 0), xf2 * (0, .5)])
],
)
self.target.active = False
def __init__(self, x0, target):
super(PointMassWorld, self).__init__()
self.world.gravity = (0.0, 0.0)
self.initial_position = (x0[0], x0[1])
self.initial_angle = b2.b2_pi
self.initial_linear_velocity = (x0[2], x0[3])
self.initial_angular_velocity = 0
ground = self.world.CreateBody(position=(0, 20))
ground.CreateEdgeChain(
[(-20, -20),
(-20, 20),
(20, 20),
(20, -20),
(-20, -20)]
)
xf1 = b2.b2Transform()
xf1.angle = 0.3524 * b2.b2_pi
xf1.position = b2.b2Mul(xf1.R, (1.0, 0.0))
xf2 = b2.b2Transform()
xf2.angle = -0.3524 * b2.b2_pi
xf2.position = b2.b2Mul(xf2.R, (-1.0, 0.0))
self.body = self.world.CreateDynamicBody(
position=self.initial_position,
angle=self.initial_angle,
linearVelocity=self.initial_linear_velocity,
angularVelocity=self.initial_angular_velocity,
angularDamping=5,
linearDamping=0.1,
shapes=[b2.b2PolygonShape(vertices=[xf1*(-1, 0),
xf1*(1, 0), xf1*(0, .5)]),
b2.b2PolygonShape(vertices=[xf2*(-1, 0),
xf2*(1, 0), xf2*(0, .5)])],
shapeFixture=b2.b2FixtureDef(density=1.0),
)
self.target = self.world.CreateStaticBody(
position=target,
angle=self.initial_angle,
shapes=[b2.b2PolygonShape(vertices=[xf1*(-1, 0), xf1*(1, 0),
xf1*(0, .5)]),
b2.b2PolygonShape(vertices=[xf2*(-1, 0), xf2*(1, 0),
xf2*(0, .5)])],
)
self.target.active = False
def b2PolyToPoints(shape):
'''
convert a b2Poly into a list of its points
'''
points = []
for i in xrange(shape.GetVertexCount()):
pt = box2d.b2Mul(shape.GetBody().GetXForm(), shape.getVertex(i))
points.append(pt)
return points
def draw(self):
""" If a drawing method is specified, this function passes the objects
to the module in pixels.
Return: True if the objects were successfully drawn
False if the renderer was not set or another error occurred
"""
self.callbacks.start(CALLBACK_DRAWING_START)
# No need to run through the loop if there's no way to draw
if not self.renderer:
return False
if self.camera.track_body:
# Get Body Center
p1 = self.camera.track_body.GetWorldCenter()
# Center the Camera There, False = Don't stop the tracking
self.camera.center(self.to_screen(
(p1.x * self.ppm, p1.y * self.ppm)),
stopTrack=False)
# Walk through all known elements
self.renderer.start_drawing()
for body in self.world.bodies:
xform = body.transform
shape = body.fixtures
angle = body.angle
if shape:
userdata = body.userData
if 'color' in userdata:
clr = userdata['color']
else:
clr = self.colors[0]
for shape in body.fixtures:
type_ = shape.type
if type_ == box2d.b2Shape.e_circle:
position = box2d.b2Mul(xform, shape.shape.pos)
pos = self.to_screen(
(position.x * self.ppm, position.y * self.ppm))
self.renderer.draw_circle(
clr, pos, self.meter_to_screen(shape.shape.radius),
angle)
elif type_ == box2d.b2Shape.e_polygon:
points = []
for v in shape.shape.vertices:
pt = box2d.b2Mul(xform, v)
x, y = self.to_screen(
(pt.x * self.ppm, pt.y * self.ppm))
points.append([x, y])
self.renderer.draw_polygon(clr, points)
else:
print("unknown shape type:%d" % shape.type)
for joint in self.world.joints:
p2 = joint.anchorA
p2 = self.to_screen((p2.x * self.ppm, p2.y * self.ppm))
p1 = joint.anchorB
p1 = self.to_screen((p1.x * self.ppm, p1.y * self.ppm))
if p1 == p2:
self.renderer.draw_circle((255, 255, 255), p1, 2, 0)
if isinstance(joint, box2d.b2RevoluteJoint):
self.renderer.draw_circle((255, 255, 255), p1,
self.PIN_MOTOR_RADIUS, 0)
else:
self.renderer.draw_lines((0, 0, 0), False, [p1, p2], 3)
self.callbacks.start(CALLBACK_DRAWING_END)
self.renderer.after_drawing()
return True
def draw(self):
""" If a drawing method is specified, this function passes the objects
to the module in pixels.
Return: True if the objects were successfully drawn
False if the renderer was not set or another error occurred
"""
self.callbacks.start(CALLBACK_DRAWING_START)
# No need to run through the loop if there's no way to draw
if not self.renderer:
return False
if self.camera.track_body:
# Get Body Center
p1 = self.camera.track_body.GetWorldCenter()
# Center the Camera There, False = Don't stop the tracking
self.camera.center(self.to_screen((p1.x*self.ppm, p1.y*self.ppm)), stopTrack=False)
# Walk through all known elements
self.renderer.start_drawing()
for body in self.world.bodyList:
xform = body.GetXForm()
shape = body.GetShapeList()
angle = body.GetAngle()
if shape:
userdata = body.GetUserData()
clr = userdata['color']
for shape in body.shapeList:
type = shape.GetType()
if type == box2d.e_circleShape:
position = box2d.b2Mul(xform, shape.GetLocalPosition())
pos = self.to_screen((position.x*self.ppm, position.y*self.ppm))
self.renderer.draw_circle(clr, pos, self.meter_to_screen(shape.radius), angle)
elif type == box2d.e_polygonShape:
points = []
for v in shape.vertices:
pt = box2d.b2Mul(xform, v)
x, y = self.to_screen((pt.x*self.ppm, pt.y*self.ppm))
points.append([x, y])
self.renderer.draw_polygon(clr, points)
else:
print " unknown shape type:%d" % shape.GetType()
for joint in self.world.jointList:
p2 = joint.GetAnchor1()
p2 = self.to_screen((p2.x*self.ppm, p2.y*self.ppm))
p1 = joint.GetAnchor2()
p1 = self.to_screen((p1.x*self.ppm, p1.y*self.ppm))
if p1 == p2:
self.renderer.draw_circle((255,255,255), p1, 2, 0)
else:
self.renderer.draw_lines((0,0,0), False, [p1, p2], 3)
self.callbacks.start(CALLBACK_DRAWING_END)
self.renderer.after_drawing()
return True
def draw(self):
""" If a drawing method is specified, this function passes the objects
to the module in pixels.
Return: True if the objects were successfully drawn
False if the renderer was not set or another error occurred
"""
self.callbacks.start(CALLBACK_DRAWING_START)
# No need to run through the loop if there's no way to draw
if not self.renderer:
return False
if self.camera.track_body:
# Get Body Center
p1 = self.camera.track_body.GetWorldCenter()
# Center the Camera There, False = Don't stop the tracking
self.camera.center(self.to_screen((p1.x * self.ppm,
p1.y * self.ppm)),
stopTrack=False)
# Walk through all known elements
self.renderer.start_drawing()
for body in self.world.bodyList:
xform = body.GetXForm()
shape = body.GetShapeList()
angle = body.GetAngle()
if shape:
userdata = body.GetUserData()
if 'color' in userdata:
clr = userdata['color']
else:
clr = self.colors[0]
for shape in body.shapeList:
type_ = shape.GetType()
if type_ == box2d.e_circleShape:
position = box2d.b2Mul(xform, shape.GetLocalPosition())
pos = self.to_screen((position.x * self.ppm,
position.y * self.ppm))
self.renderer.draw_circle(
clr, pos, self.meter_to_screen(shape.radius), angle)
elif type_ == box2d.e_polygonShape:
points = []
for v in shape.vertices:
pt = box2d.b2Mul(xform, v)
x, y = self.to_screen((pt.x * self.ppm,
pt.y * self.ppm))
points.append([x, y])
self.renderer.draw_polygon(clr, points)
else:
print "unknown shape type:%d" % shape.GetType()
for joint in self.world.jointList:
p2 = joint.GetAnchor1()
p2 = self.to_screen((p2.x * self.ppm, p2.y * self.ppm))
p1 = joint.GetAnchor2()
p1 = self.to_screen((p1.x * self.ppm, p1.y * self.ppm))
if isinstance(joint, box2d.b2RevoluteJoint):
self.renderer.draw_circle((255, 255, 255), p1,
self.PIN_MOTOR_RADIUS, 0)
else:
self.renderer.draw_lines((0, 0, 0), False, [p1, p2], 3)
self.callbacks.start(CALLBACK_DRAWING_END)
self.renderer.after_drawing()
return True
def draw(self):
""" If a drawing method is specified, this function passes the objects
to the module in pixels.
Return: True if the objects were successfully drawn
False if the renderer was not set or another error occurred
"""
self.callbacks.start(CALLBACK_DRAWING_START)
# No need to run through the loop if there's no way to draw
if not self.renderer:
return False
if self.camera.track_body:
# Get Body Center
p1 = self.camera.track_body.GetWorldCenter()
# Center the Camera There, False = Don't stop the tracking
self.camera.center(
self.to_screen(
(p1.x * self.ppm,
p1.y * self.ppm)),
stopTrack=False)
# Walk through all known elements
self.renderer.start_drawing()
for body in self.world.bodies:
xform = body.transform
shape = body.fixtures
angle = body.angle
if shape:
userdata = body.userData
clr = userdata['color']
for fixture in body.fixtures:
type = fixture.type
if type == box2d.b2Shape.e_circle:
position = box2d.b2Mul(xform, fixture.shape.pos)
pos = self.to_screen(
(position.x * self.ppm, position.y * self.ppm))
self.renderer.draw_circle(
clr, pos, self.meter_to_screen(
fixture.shape.radius), angle)
elif type == box2d.b2Shape.e_polygon:
points = []
for v in fixture.shape.vertices:
pt = box2d.b2Mul(xform, v)
x, y = self.to_screen(
(pt.x * self.ppm, pt.y * self.ppm))
points.append([x, y])
self.renderer.draw_polygon(clr, points)
else:
print " unknown shape type:%d" % fixture.type
for joint in self.world.joints:
p2 = joint.anchorA
p2 = self.to_screen((p2.x * self.ppm, p2.y * self.ppm))
p1 = joint.anchorB
p1 = self.to_screen((p1.x * self.ppm, p1.y * self.ppm))
if p1 == p2:
self.renderer.draw_circle((255, 255, 255), p1, 2, 0)
else:
self.renderer.draw_lines((0, 0, 0), False, [p1, p2], 3)
self.callbacks.start(CALLBACK_DRAWING_END)
self.renderer.after_drawing()
return True
def __init__(self, x0, world_info, target, render):
self.render = render
if self.render:
super(PointMassWorldObstacle, self).__init__()
else:
self.world = b2.b2World(gravity=(0, -10), doSleep=True)
self.world.gravity = (0.0, 0.0)
self.initial_position = (x0[0], x0[1])
self.initial_angle = b2.b2_pi
self.initial_linear_velocity = (x0[2], x0[3])
self.initial_angular_velocity = 0
ground = self.world.CreateBody(position=(0, 20))
ground.CreateEdgeChain([(-20, -20), (-20, 20), (20, 20), (20, -20),
(-20, -20)])
xf1 = b2.b2Transform()
xf1.angle = 0.3524 * b2.b2_pi
xf1.position = b2.b2Mul(xf1.R, (1.0, 0.0))
xf2 = b2.b2Transform()
xf2.angle = -0.3524 * b2.b2_pi
xf2.position = b2.b2Mul(xf2.R, (-1.0, 0.0))
"""
self.body_shape = [b2.b2PolygonShape(vertices=[xf1*(-1, 0),
xf1*(1, 0), xf1*(0, .5)]),
b2.b2PolygonShape(vertices=[xf2*(-1, 0),
xf2*(1, 0), xf2*(0, .5)])]
"""
self.body_shape = [b2.b2PolygonShape(box=(1, 1))]
self.body = self.world.CreateDynamicBody(
position=self.initial_position,
angle=self.initial_angle,
linearVelocity=self.initial_linear_velocity,
angularVelocity=self.initial_angular_velocity,
angularDamping=5,
linearDamping=0.1,
shapes=self.body_shape,
shapeFixture=b2.b2FixtureDef(density=1.0),
)
self.initial_pos = self.world.CreateStaticBody(
position=self.initial_position,
angle=self.initial_angle,
shapes=self.body_shape,
)
self.target = self.world.CreateStaticBody(
position=target[:2],
angle=self.initial_angle,
shapes=[
b2.b2PolygonShape(
vertices=[xf1 * (-1, 0), xf1 * (1, 0), xf1 * (0, .5)]),
b2.b2PolygonShape(
vertices=[xf2 * (-1, 0), xf2 * (1, 0), xf2 * (0, .5)])
],
)
self.obstacle_post = []
self.obstacle_shape = []
self.obstacle = []
obstacles = world_info['obstacles']
self.n_obs = len(obstacles)
for i in range(self.n_obs):
self.obstacle_post.append(obstacles[i][:2])
self.obstacle_shape.append(
[b2.b2PolygonShape(box=tuple(obstacles[i][2:]))])
self.obstacle.append(
self.world.CreateStaticBody(position=self.obstacle_post[i],
angle=self.initial_angle,
shapes=self.obstacle_shape[i]))
self.initial_pos.active = False
self.target.active = False
def rotate(vec, angle, center = b2d.b2Vec2(0,0)):
res = vec.copy()
res -= center
res = b2d.b2Mul(b2d.b2Mat22(angle), res)
res += center
return res
