def test_polygon_in(self):
p1 = Point(1, 1)
p2 = Point(1, -1)
p3 = Point(-1, -1)
p4 = Point(-1, 1)
p = Polygon([p1, p2, p3, p4])
p0 = Point(0, 0)
pn = Point(1.1, 1)
self.assertEqual(p.contains_point(p0), True)
self.assertNotEqual(p.contains_point(pn), True)
def build_polygons():
polygons = []
with open("../static/states.json", "r") as json_data:
for line in json_data:
# result.append(json.loads(line))
data = json.loads(line)
poly = Polygon(data["state"])
borders = data["border"]
for border in borders:
poly.add_point(Point(border[0], border[1]))
polygons.append(poly)
return polygons
def test_hull_sum(self):
p1 = Point(2, 2)
p2 = Point(2, -2)
p3 = Point(-2, -2)
p4 = Point(-2, 2)
r1 = Polygon((p1, p2, p3, p4))
p1 = Point(3, 0)
p2 = Point(0, -3)
p3 = Point(-3, 0)
p4 = Point(0, 3)
r2 = Polygon((p1, p2, p3, p4))
h = Hull(r1) + Hull(r2)
self.assertEqual(
h,
[
Point(0.0, -3.0),
Point(2.0, -2.0),
Point(3.0, 0.0),
Point(2.0, 2.0),
Point(0.0, 3.0),
Point(-2.0, 2.0),
Point(-3.0, 0.0),
Point(-2.0, -2.0),
],
)
p1 = Point(2, 2)
p2 = Point(2, 0)
p3 = Point(0, 0)
p4 = Point(0, 2)
r1 = Polygon((p1, p2, p3, p4))
p1 = Point(-2, -2)
p2 = Point(-2, 0)
p3 = Point(0, -1)
p4 = Point(0, -2)
r2 = Polygon((p1, p2, p3, p4))
h = Hull(r1) + Hull(r2)
self.assertEqual(h, [
Point(-2.0, -2.0),
Point(0.0, -2.0),
Point(2.0, 0.0),
Point(2.0, 2.0),
Point(0.0, 2.0),
Point(-2.0, 0.0)
])
def test_polygon_area(self):
p1 = Point(0, 0)
p2 = Point(0, 100)
p3 = Point(100, 100)
p4 = Point(100, 0)
p = Polygon((p1, p2, p3, p4))
self.assertAlmostEqual(p.area, 10000)
self.assertAlmostEqual(p.area, 10000)
def __init__(self, robot_cfg):
"""
Initializes a Robot object
:param robot_cfg: The robot configuration
"""
self.robot_cfg = robot_cfg
# geometry
self.geometry = Polygon(robot_cfg["bottom_plate"])
self.global_geometry = Polygon(
robot_cfg["bottom_plate"]) # actual geometry in world space
# wheel arrangement
self.wheel_radius = robot_cfg["wheel"]["radius"]
self.wheel_base_length = robot_cfg["wheel"]["base_length"]
# pose
self.pose = Pose(0.0, 0.0, 0.0)
# wheel encoders
self.left_wheel_encoder = WheelEncoder(
robot_cfg["wheel"]["ticks_per_rev"])
self.right_wheel_encoder = WheelEncoder(
robot_cfg["wheel"]["ticks_per_rev"])
self.wheel_encoders = [
self.left_wheel_encoder, self.right_wheel_encoder
]
# IR sensors
self.ir_sensors = []
for id, _pose in enumerate(robot_cfg["sensor"]["poses"]):
ir_pose = Pose(_pose[0], _pose[1], radians(_pose[2]))
self.ir_sensors.append(
ProximitySensor(self, ir_pose, robot_cfg["sensor"], id))
# Feature detector
self.feature_detector = FeatureDetector()
# dynamics
self.dynamics = DifferentialDriveDynamics(self.wheel_radius,
self.wheel_base_length)
## initialize state
# set wheel drive rates (rad/s)
self.left_wheel_drive_rate = 0.0
self.right_wheel_drive_rate = 0.0
def __init__(self, width, height, pose):
"""
Initializes a RectangleObstacle
:param width: Width of the rectangle
:param height: Height of the rectanle
:param pose: Pose specifying the center of the rectangle
"""
self.pose = pose
self.width = width
self.height = height
# define the geometry
halfwidth_x = width * 0.5
halfwidth_y = height * 0.5
vertexes = [[halfwidth_x, halfwidth_y], [halfwidth_x, -halfwidth_y],
[-halfwidth_x, -halfwidth_y], [-halfwidth_x, halfwidth_y]]
self.geometry = Polygon(vertexes)
self.global_geometry = Polygon(vertexes).get_transformation_to_pose(
self.pose)
def __init__(self, radius, pose):
"""
Initializes a regular OctagonObstacle object
:param radius: The distance from the center to a corner of the octagon
:param pose: Pose specifying the center of the octagon
"""
self.pose = pose
self.radius = radius
# define the geometry
angled_length = radius * (1 / 2 ** 0.5)
vertexes = [[0, radius],
[angled_length, angled_length],
[radius, 0],
[angled_length, -angled_length],
[0, -radius],
[-angled_length, -angled_length],
[-radius, 0],
[-angled_length, angled_length]]
self.geometry = Polygon(vertexes)
self.global_geometry = Polygon(vertexes).get_transformation_to_pose(self.pose)
class Robot: # Khepera III robot
def __init__(self, robot_cfg):
"""
Initializes a Robot object
:param robot_cfg: The robot configuration
"""
self.robot_cfg = robot_cfg
# geometry
self.geometry = Polygon(robot_cfg["bottom_plate"])
self.global_geometry = Polygon(robot_cfg["bottom_plate"]) # actual geometry in world space
# wheel arrangement
self.wheel_radius = robot_cfg["wheel"]["radius"]
self.wheel_base_length = robot_cfg["wheel"]["base_length"]
# pose
self.pose = Pose(0.0, 0.0, 0.0)
# wheel encoders
self.left_wheel_encoder = WheelEncoder(robot_cfg["wheel"]["ticks_per_rev"])
self.right_wheel_encoder = WheelEncoder(robot_cfg["wheel"]["ticks_per_rev"])
self.wheel_encoders = [self.left_wheel_encoder, self.right_wheel_encoder]
# IR sensors
self.ir_sensors = []
for _pose in robot_cfg["sensor"]["poses"]:
ir_pose = Pose(_pose[0], _pose[1], radians(_pose[2]))
self.ir_sensors.append(
ProximitySensor(self, ir_pose, robot_cfg["sensor"]))
# dynamics
self.dynamics = DifferentialDriveDynamics(self.wheel_radius, self.wheel_base_length)
## initialize state
# set wheel drive rates (rad/s)
self.left_wheel_drive_rate = 0.0
self.right_wheel_drive_rate = 0.0
def step_motion(self, dt):
"""
Simulate the robot's motion over the given time interval
:param dt: The time interval for which this motion is executed
"""
v_l = self.left_wheel_drive_rate
v_r = self.right_wheel_drive_rate
# apply the robot dynamics to moving parts
self.dynamics.apply_dynamics(v_l, v_r, dt,
self.pose, self.wheel_encoders)
# update global geometry
self.global_geometry = self.geometry.get_transformation_to_pose(self.pose)
# update all of the sensors
for ir_sensor in self.ir_sensors:
ir_sensor.update_position()
def set_wheel_drive_rates(self, v_l, v_r):
"""
Set the drive rates (angular velocities) for this robot's wheels in rad/s
:param v_l: Velocity of left wheel
:param v_r: Velocity of right while
"""
# simulate physical limit on drive motors
v_l = min(self.robot_cfg["wheel"]["max_speed"], v_l)
v_r = min(self.robot_cfg["wheel"]["max_speed"], v_r)
# set drive rates
self.left_wheel_drive_rate = v_l
self.right_wheel_drive_rate = v_r