def on_destroy(self):
# Create explosion
for s in self.body.shapes:
bodydef = Body()
bodydef.ccd = True
debris = self.melee.world.append_body(bodydef)
debris.linear_velocity = Vec2(randrange(-100.0, 100.0),
randrange(-100.0, 100.0))
junk = Debris(self.melee, debris)
if isinstance(s, BoundPolygon):
# Polygon
debris.position = self.body.position + s.centroid
polydef = Polygon()
polydef.density = 10
polydef.vertices = s.vertices
shape = debris.append_shape(polydef)
# Register shapes for collision callbacks
self.melee.contact_register[hash(shape)] = junk
else:
# Circle
debris.position = self.body.position + s.local_position
circdef = Circle()
circdef.density = 10
circle.radis = s.radius
shape = debris.append_shape(circdef)
self.melee.contact_register[hash(shape)] = junk
debris.set_mass_from_shapes()
def __init__(self, width, height, pose):
"""Creates a Polygon with the orientation and size inputted
Keywords:
width -> float
height -> float
pose -> 3-item list
"""
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, melee):
Ship.__init__(self, melee)
##
## Manually Create turret
## TODO: Eventually this will be autogenerated via SVG
from utils.squirtle import SVG
svg_turret = SVG('data/ships/nemesis-turret.svg')
svg_turret.init((105.908, 106.821), self.scale)
# Create body
bodydef = Body()
bodydef.ccd = True
bodydef.position = self.body.position
self.turret = melee.world.append_body(bodydef)
self.turret.angular_velocity = self.body.angular_velocity
self.turret.linear_velocity = self.body.linear_velocity
# Create shapes
self.radius = 0.85
density = 2.0
# Base
base = Circle()
base.collision_group = self.group
base.radius = self.radius
base.density = density
# Barrel
verts = [Vec2(0.15, -2), Vec2(0.15, 0), Vec2(-0.15, 0), Vec2( -0.15, -2)]
barrel = Polygon()
barrel.vertices = verts
barrel.collision_group = self.group
barrel.density = density
s1 = self.turret.append_shape(base)
s2 = self.turret.append_shape(barrel)
self.turret.set_mass_from_shapes()
# Create secondary
SecondaryWeapon(self, melee, self.turret, svg_turret)
def fire(self):
# This is a specalized function..
if not self.primary_time() or self.battery <= self.pEnergy:
return
# Drain battery
self.battery_cost(self.pEnergy)
# Create body and shape
bodydef = Body()
bodydef.ccd = True
bodydef.angle = self.body.angle + (pi * 0.5) + self.turret_angle
bodydef.position = self.turret.get_world_point(Vec2(0, -3))
shell = self.melee.world.append_body(bodydef)
angle = vforangle(bodydef.angle)
velocity = rotate(angle, (0.0, -150.0))
vb = self.body.linear_velocity
shell.linear_velocity = Vec2(velocity[0]+vb.x, velocity[1]+vb.y)
polydef = Polygon()
verts = [Vec2(0.5, 0.8), Vec2(-0.5, 0.8), Vec2(-0.5, -0.8), Vec2(0.5, -0.8)]
polydef.vertices = verts
polydef.density = 5
polydef.collision_group = self.group
shell.append_shape(polydef)
shell.set_mass_from_shapes()
# Create projectile
projectile = PrimaryWeapon(self, self.melee, shell)
projectile.group = self.group
projectile.lifetime = 2.5
projectile.damage = 10
projectile.health = 5
projectile.shapes = verts
def __init__(self, melee):
Actor.__init__(self, melee)
# Set max linear and angular velocity
self.max_linear_velocity = 50
self.max_angular_velocity = pi
# Physics (based on SVG shapes)
self.translate = calc_center(self.lines[self.parts.index(self.center_part)])
self.svg.init(self.translate, self.scale)
bodydef = Body()
bodydef.ccd = True
bodydef.position = self.initial_position
self.body = melee.world.append_body(bodydef)
self.body.linear_velocity = self.initial_velocity
self.body.angular_velocity = self.initial_ang_vel
for p in self.lines:
polygondef = Polygon()
polygondef.density = self.density
# Ensure points are oriented ccw
ccw = convex_hull(p)
# Translate and scale points
verts = []
for v in ccw:
x = (v[0] - self.translate[0]) * self.scale
y = (v[1] - self.translate[1]) * self.scale
verts.append(Vec2(x, y))
polygondef.vertices = verts
polygondef.collision_group = self.group
shape = self.body.append_shape(polygondef)
# Register shapes for collision callbacks
melee.contact_register[hash(shape)] = self
self.body.set_mass_from_shapes()
def build_0(self):
pig1 = Pig(960, 110, self.space)
pig2 = Pig(960, 192, self.space)
self.pigs.append(pig1)
self.pigs.append(pig2)
self.beams.append(Polygon((960, 110), 85, 20, self.space))
self.beams.append(Polygon((960, 20), 85, 20, self.space))
self.columns.append(Polygon((930, -50), 20, 85, self.space))
self.columns.append(Polygon((990, -50), 20, 85, self.space))
self.columns.append(Polygon((930, 70), 20, 85, self.space))
self.columns.append(Polygon((990, 70), 20, 85, self.space))
self.number_of_birds = 4
if self.bool_space:
self.number_of_birds = 8
def custom_map(self):
"""Generate a map based on dictionary of obstacle and goal locations"""
# OBSTACLE PARAMS
obs_params = [{
'w': 0.5,
'h': 1.2,
'x': 1.0,
'y': 0.0,
'deg': 0
}, {
'w': 0.3,
'h': 0.6,
'x': 0.5,
'y': 1.0,
'deg': 25
}]
# BUILD CUSTOM ELEMENTS
# generate the goal
x = GOAL_X_DIST
y = GOAL_Y_DIST
goal = [x, y]
# generate a proximity test geometry for the goal
r = MIN_GOAL_CLEARANCE
n = 6
_goal_test_geometry = []
for i in range(n):
_goal_test_geometry.append(
[x + r * cos(i * 2 * pi / n), y + r * sin(i * 2 * pi / n)])
goal_test_geometry = Polygon(_goal_test_geometry)
# generate the obstacles
obstacles = []
obs_index = 0
# collect together robot and goal geometries to check obstacles
# are not on top of them
test_geometries = [r.global_geometry
for r in self.world.robots] + ([goal_test_geometry])
while obs_index < len(obs_params):
# convert orientation to radians
theta = -pi + ((obs_params[obs_index]['deg'] / 360.0) * 2 * pi)
# create the obstacle
obstacle = RectangleObstacle(
obs_params[obs_index]['w'], obs_params[obs_index]['h'],
Pose(obs_params[obs_index]['x'], obs_params[obs_index]['y'],
theta))
# test if the obstacle overlaps the robots or the goal
intersects = False
for test_geometry in test_geometries:
intersects |= self.geometrics.convex_polygon_intersect_test(
test_geometry, obstacle.global_geometry)
if intersects is False:
obstacles.append(obstacle)
else:
print("Obstacle {} collides with robots or goal").format(
obs_index)
obs_index += 1
# update the current obstacles and goal
self.current_obstacles = obstacles
self.current_goal = goal
# apply the new obstacles and goal to the world
self.apply_to_world()
def random_map(self):
"""Generate a random set of obstacles and goal location"""
# OBSTACLE PARAMS
obs_min_dim = OBS_MIN_DIM
obs_max_dim = OBS_MAX_DIM
obs_max_combined_dim = OBS_MAX_COMBINED_DIM
obs_min_count = OBS_MIN_COUNT
obs_max_count = OBS_MAX_COUNT
obs_min_dist = OBS_MIN_DIST
obs_max_dist = OBS_MAX_DIST
# GOAL PARAMS
goal_min_dist = GOAL_MIN_DIST
goal_max_dist = GOAL_MAX_DIST
# BUILD RANDOM ELEMENTS
# generate the goal
goal_dist_range = goal_max_dist - goal_min_dist
dist = goal_min_dist + (random() * goal_dist_range)
phi = -pi + (random() * 2 * pi)
x = dist * sin(phi)
y = dist * cos(phi)
goal = [x, y]
# generate a proximity test geometry for the goal
r = MIN_GOAL_CLEARANCE
n = 6
_goal_test_geometry = []
for i in range(n):
_goal_test_geometry.append(
[x + r * cos(i * 2 * pi / n), y + r * sin(i * 2 * pi / n)])
goal_test_geometry = Polygon(_goal_test_geometry)
# generate the obstacles
obstacles = []
obs_dim_range = obs_max_dim - obs_min_dim
obs_dist_range = obs_max_dist - obs_min_dist
num_obstacles = randrange(obs_min_count, obs_max_count + 1)
test_geometries = [r.global_geometry
for r in self.world.robots] + ([goal_test_geometry])
while len(obstacles) < num_obstacles:
# generate dimensions
width = obs_min_dim + (random() * obs_dim_range)
height = obs_min_dim + (random() * obs_dim_range)
while width + height > obs_max_combined_dim:
height = obs_min_dim + (random() * obs_dim_range)
# generate position
dist = obs_min_dist + (random() * obs_dist_range)
phi = -pi + (random() * 2 * pi)
x = dist * sin(phi)
y = dist * cos(phi)
# generate orientation
theta = -pi + (random() * 2 * pi)
# test if the obstacle overlaps the robots or the goal
obstacle = RectangleObstacle(width, height, Pose(x, y, theta))
intersects = False
for test_geometry in test_geometries:
intersects |= self.geometrics.convex_polygon_intersect_test(
test_geometry, obstacle.global_geometry)
if intersects is False:
obstacles.append(obstacle)
# update the current obstacles and goal
self.current_obstacles = obstacles
self.current_goal = goal
# apply the new obstacles and goal to the world
self.apply_to_world()
class Robot(object): # Robot
"""Class representing a robot simultaneously in the GUI and real world.
Attributes:
id -> int
wheel_radius -> float
wheel_base_length -> float
max_speed -> float
trans_vel_limit -> float
ang_vel_limit -> float
pose -> Pose object
geometry -> Polygon object
global_geometry -> Polygon object
left_wheel_encoder -> WheelEncoder object
right_wheel_encoder -> WheelEncoder object
wheel_encoders -> list
proximity_sensors -> list
dynamics -> DifferentialDriveDynamics object
supervisor -> Supervisor object
use_serial -> boolean
physical_robot -> RobotPhysicalInterface object
left_wheel_drive_rate -> float
right_wheel_drive_rate -> float
Methods:
__init__(ID, x=0.0, y=0.0, deg=90.0)
step_motion(dt)
stop_motion()
set_wheel_drive_rates(v_l, v_r)
"""
def __init__(self, ID, viewer, x=0.0, y=0.0, deg=0.0):
"""Bind robot ID and setup robot geometry, location, supervisor & comms.
Keywords:
id -> int
x -> float
y -> float
deg -> float
robot_params -> list
"""
# robot ID
self.id = ID
# bind the viewer of the robot
self.viewer = viewer
# bind the initial pose of the robot
self.x = x
self.y = y
self.deg = deg
# setup the robot
self.setup_robot(self.x, self.y, self.deg)
# physical robot communication
self.use_serial = use_serial
if self.use_serial:
self.physical_robot = RobotPhysicalInterface(self)
def setup_robot(self, x=0.0, y=0.0, deg=0.0):
"""Apply the robot and sensor config parameters"""
# retrieve robot config parameters
robot_params = self.viewer.get_robot_parameters()
self.wheel_radius = float(robot_params[0][0][1]) # metres
self.wheel_base_length = float(robot_params[0][1][1]) # metres
self.ticks_per_rev = float(robot_params[0][2][1]) # unitless
self.max_speed = float(robot_params[0][3][1]) # rpm
self.body_width = float(robot_params[0][4][1]) # metres
self.body_length = float(robot_params[0][5][1]) # metres
self.payload_width = float(robot_params[0][6][1]) # metres
self.payload_length = float(robot_params[0][7][1]) # metres
self.payload_offset = float(robot_params[0][8][1]) # metres
self.sensor_min_value = float(robot_params[1][0][1]) # millivolts
self.sensor_max_value = float(robot_params[1][1][1]) # millivolts
self.sensor_min_range = float(robot_params[1][2][1]) # metres
self.sensor_max_range = float(robot_params[1][3][1]) # metres
self.sensor_phi_range = float(robot_params[1][4][1]) # metres
# x, y, theta_degrees
self.sensor_poses = ([
[float(robot_params[2][0][1]), float(robot_params[2][1][1]),
float(robot_params[2][2][1])],
[float(robot_params[3][0][1]), float(robot_params[3][1][1]),
float(robot_params[3][2][1])],
[float(robot_params[4][0][1]), float(robot_params[4][1][1]),
float(robot_params[4][2][1])]
])
self.robot_body = ([[self.body_length / 2, self.body_width / 2],
[-self.body_length / 2, self.body_width / 2],
[-self.body_length / 2, -self.body_width / 2],
[self.body_length / 2, -self.body_width / 2]])
self.robot_payload = ([[self.payload_length / 2 + self.payload_offset,
self.payload_width / 2],
[-self.payload_length / 2 + self.payload_offset,
self.payload_width / 2],
[-self.payload_length / 2 + self.payload_offset,
-self.payload_width / 2],
[self.payload_length / 2 + self.payload_offset,
-self.payload_width / 2]])
# drive rates
self.max_speed *= ((2 * pi) / 60) # rpm 2 rad/s
self.trans_vel_limit = self.max_speed * self.wheel_radius # m/s
self.ang_vel_limit = 0.2 * self.max_speed # rad/s
if debug:
print("SPEED {:.3f}m/s").format(self.trans_vel_limit)
# pose
theta = -pi + ((deg / 360.0) * 2 * pi)
self.pose = Pose(x, y, theta)
# geometry
self.geometry = Polygon(self.robot_body)
self.global_geometry = (
self.geometry.get_transformation_to_pose(self.pose))
# wheel encoders
self.left_wheel_encoder = WheelEncoder(self.ticks_per_rev)
self.right_wheel_encoder = WheelEncoder(self.ticks_per_rev)
self.wheel_encoders = (
[self.left_wheel_encoder, self.right_wheel_encoder])
# proximity sensors
self.proximity_sensors = []
for _pose in self.sensor_poses:
sensor_pose = Pose(_pose[0], _pose[1], radians(_pose[2]))
self.proximity_sensors.append(
ProximitySensor(self, sensor_pose, self.sensor_min_range,
self.sensor_max_range, radians(self.sensor_phi_range)))
# dynamics
self.dynamics = DifferentialDriveDynamics(self, self.wheel_radius,
self.wheel_base_length)
# supervisor
self.supervisor = Supervisor(RobotSupervisorInterface(self),
self.wheel_radius, self.wheel_base_length, self.ticks_per_rev,
self.sensor_poses, self.sensor_max_range,
Pose(self.pose.x, self.pose.y, self.pose.theta))
# 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."""
# apply the supervisor velocities & encoder values -> determine new pose
self.dynamics.apply_dynamics(self.left_wheel_drive_rate,
self.right_wheel_drive_rate, dt, self.pose, self.wheel_encoders)
# update global geometry (blue shell)
self.global_geometry = (
self.geometry.get_transformation_to_pose(self.pose))
# update the position all of proximity the sensors
for proximity_sensor in self.proximity_sensors:
proximity_sensor.update_position()
# send wheel speeds to physical robot and retrieve sensor values
if self.physical_robot.robot_comm.connected:
self.physical_robot.step()
def stop_motion(self):
"""Stop the physical robot."""
if self.physical_robot.robot_comm.connected:
self.physical_robot.stop()
def set_wheel_drive_rates(self, v_l, v_r):
"""Set the drive rates (angular vel rad/s) for this robot's wheels."""
# simulate physical limit on drive motors
self.left_wheel_drive_rate = min(self.max_speed, v_l)
self.right_wheel_drive_rate = min(self.max_speed, v_r)
print("left_wheel_drive_rate = {}, right_wheel_drive_rate = {}").format(
self.left_wheel_drive_rate, self.right_wheel_drive_rate)
def read_encoders(self):
self.physical_robot.read_wheel_encoders()
def setup_robot(self, x=0.0, y=0.0, deg=0.0):
"""Apply the robot and sensor config parameters"""
# retrieve robot config parameters
robot_params = self.viewer.get_robot_parameters()
self.wheel_radius = float(robot_params[0][0][1]) # metres
self.wheel_base_length = float(robot_params[0][1][1]) # metres
self.ticks_per_rev = float(robot_params[0][2][1]) # unitless
self.max_speed = float(robot_params[0][3][1]) # rpm
self.body_width = float(robot_params[0][4][1]) # metres
self.body_length = float(robot_params[0][5][1]) # metres
self.payload_width = float(robot_params[0][6][1]) # metres
self.payload_length = float(robot_params[0][7][1]) # metres
self.payload_offset = float(robot_params[0][8][1]) # metres
self.sensor_min_value = float(robot_params[1][0][1]) # millivolts
self.sensor_max_value = float(robot_params[1][1][1]) # millivolts
self.sensor_min_range = float(robot_params[1][2][1]) # metres
self.sensor_max_range = float(robot_params[1][3][1]) # metres
self.sensor_phi_range = float(robot_params[1][4][1]) # metres
# x, y, theta_degrees
self.sensor_poses = ([
[float(robot_params[2][0][1]), float(robot_params[2][1][1]),
float(robot_params[2][2][1])],
[float(robot_params[3][0][1]), float(robot_params[3][1][1]),
float(robot_params[3][2][1])],
[float(robot_params[4][0][1]), float(robot_params[4][1][1]),
float(robot_params[4][2][1])]
])
self.robot_body = ([[self.body_length / 2, self.body_width / 2],
[-self.body_length / 2, self.body_width / 2],
[-self.body_length / 2, -self.body_width / 2],
[self.body_length / 2, -self.body_width / 2]])
self.robot_payload = ([[self.payload_length / 2 + self.payload_offset,
self.payload_width / 2],
[-self.payload_length / 2 + self.payload_offset,
self.payload_width / 2],
[-self.payload_length / 2 + self.payload_offset,
-self.payload_width / 2],
[self.payload_length / 2 + self.payload_offset,
-self.payload_width / 2]])
# drive rates
self.max_speed *= ((2 * pi) / 60) # rpm 2 rad/s
self.trans_vel_limit = self.max_speed * self.wheel_radius # m/s
self.ang_vel_limit = 0.2 * self.max_speed # rad/s
if debug:
print("SPEED {:.3f}m/s").format(self.trans_vel_limit)
# pose
theta = -pi + ((deg / 360.0) * 2 * pi)
self.pose = Pose(x, y, theta)
# geometry
self.geometry = Polygon(self.robot_body)
self.global_geometry = (
self.geometry.get_transformation_to_pose(self.pose))
# wheel encoders
self.left_wheel_encoder = WheelEncoder(self.ticks_per_rev)
self.right_wheel_encoder = WheelEncoder(self.ticks_per_rev)
self.wheel_encoders = (
[self.left_wheel_encoder, self.right_wheel_encoder])
# proximity sensors
self.proximity_sensors = []
for _pose in self.sensor_poses:
sensor_pose = Pose(_pose[0], _pose[1], radians(_pose[2]))
self.proximity_sensors.append(
ProximitySensor(self, sensor_pose, self.sensor_min_range,
self.sensor_max_range, radians(self.sensor_phi_range)))
# dynamics
self.dynamics = DifferentialDriveDynamics(self, self.wheel_radius,
self.wheel_base_length)
# supervisor
self.supervisor = Supervisor(RobotSupervisorInterface(self),
self.wheel_radius, self.wheel_base_length, self.ticks_per_rev,
self.sensor_poses, self.sensor_max_range,
Pose(self.pose.x, self.pose.y, self.pose.theta))
# set wheel drive rates (rad/s)
self.left_wheel_drive_rate = 0.0
self.right_wheel_drive_rate = 0.0
class Robot(object): # Robot
"""Class representing a robot simultaneously in the GUI and real world.
Attributes:
id -> int
wheel_radius -> float
wheel_base_length -> float
max_speed -> float
trans_vel_limit -> float
ang_vel_limit -> float
pose -> Pose object
geometry -> Polygon object
global_geometry -> Polygon object
left_wheel_encoder -> WheelEncoder object
right_wheel_encoder -> WheelEncoder object
wheel_encoders -> list
proximity_sensors -> list
dynamics -> DifferentialDriveDynamics object
supervisor -> Supervisor object
use_serial -> boolean
physical_robot -> RobotPhysicalInterface object
left_wheel_drive_rate -> float
right_wheel_drive_rate -> float
Methods:
__init__(ID, x=0.0, y=0.0, deg=90.0)
step_motion(dt)
stop_motion()
set_wheel_drive_rates(v_l, v_r)
"""
def __init__(self, ID, viewer, x=0.0, y=0.0, deg=0.0):
"""Bind robot ID and setup robot geometry, location, supervisor & comms.
Keywords:
id -> int
x -> float
y -> float
deg -> float
robot_params -> list
"""
# robot ID
self.id = ID
# bind the viewer of the robot
self.viewer = viewer
# bind the initial pose of the robot
self.x = x
self.y = y
self.deg = deg
# setup the robot
self.setup_robot(self.x, self.y, self.deg)
# physical robot communication
self.use_serial = use_serial
if self.use_serial:
self.physical_robot = RobotPhysicalInterface(self)
def setup_robot(self, x=0.0, y=0.0, deg=0.0):
"""Apply the robot and sensor config parameters"""
# retrieve robot config parameters
robot_params = self.viewer.get_robot_parameters()
self.wheel_radius = float(robot_params[0][0][1]) # metres
self.wheel_base_length = float(robot_params[0][1][1]) # metres
self.ticks_per_rev = float(robot_params[0][2][1]) # unitless
self.max_speed = float(robot_params[0][3][1]) # rpm
self.body_width = float(robot_params[0][4][1]) # metres
self.body_length = float(robot_params[0][5][1]) # metres
self.payload_width = float(robot_params[0][6][1]) # metres
self.payload_length = float(robot_params[0][7][1]) # metres
self.payload_offset = float(robot_params[0][8][1]) # metres
self.sensor_min_value = float(robot_params[1][0][1]) # millivolts
self.sensor_max_value = float(robot_params[1][1][1]) # millivolts
self.sensor_min_range = float(robot_params[1][2][1]) # metres
self.sensor_max_range = float(robot_params[1][3][1]) # metres
self.sensor_phi_range = float(robot_params[1][4][1]) # metres
# x, y, theta_degrees
self.sensor_poses = ([[
float(robot_params[2][0][1]),
float(robot_params[2][1][1]),
float(robot_params[2][2][1])
],
[
float(robot_params[3][0][1]),
float(robot_params[3][1][1]),
float(robot_params[3][2][1])
],
[
float(robot_params[4][0][1]),
float(robot_params[4][1][1]),
float(robot_params[4][2][1])
]])
self.robot_body = ([[self.body_length / 2, self.body_width / 2],
[-self.body_length / 2, self.body_width / 2],
[-self.body_length / 2, -self.body_width / 2],
[self.body_length / 2, -self.body_width / 2]])
self.robot_payload = ([
[
self.payload_length / 2 + self.payload_offset,
self.payload_width / 2
],
[
-self.payload_length / 2 + self.payload_offset,
self.payload_width / 2
],
[
-self.payload_length / 2 + self.payload_offset,
-self.payload_width / 2
],
[
self.payload_length / 2 + self.payload_offset,
-self.payload_width / 2
]
])
# drive rates
self.max_speed *= ((2 * pi) / 60) # rpm 2 rad/s
self.trans_vel_limit = self.max_speed * self.wheel_radius # m/s
self.ang_vel_limit = 0.2 * self.max_speed # rad/s
if debug:
print("SPEED {:.3f}m/s").format(self.trans_vel_limit)
# pose
theta = -pi + ((deg / 360.0) * 2 * pi)
self.pose = Pose(x, y, theta)
# geometry
self.geometry = Polygon(self.robot_body)
self.global_geometry = (self.geometry.get_transformation_to_pose(
self.pose))
# wheel encoders
self.left_wheel_encoder = WheelEncoder(self.ticks_per_rev)
self.right_wheel_encoder = WheelEncoder(self.ticks_per_rev)
self.wheel_encoders = ([
self.left_wheel_encoder, self.right_wheel_encoder
])
# proximity sensors
self.proximity_sensors = []
for _pose in self.sensor_poses:
sensor_pose = Pose(_pose[0], _pose[1], radians(_pose[2]))
self.proximity_sensors.append(
ProximitySensor(self, sensor_pose, self.sensor_min_range,
self.sensor_max_range,
radians(self.sensor_phi_range)))
# dynamics
self.dynamics = DifferentialDriveDynamics(self, self.wheel_radius,
self.wheel_base_length)
# supervisor
self.supervisor = Supervisor(
RobotSupervisorInterface(self), self.wheel_radius,
self.wheel_base_length, self.ticks_per_rev, self.sensor_poses,
self.sensor_max_range,
Pose(self.pose.x, self.pose.y, self.pose.theta))
# 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."""
# apply the supervisor velocities & encoder values -> determine new pose
self.dynamics.apply_dynamics(self.left_wheel_drive_rate,
self.right_wheel_drive_rate, dt,
self.pose, self.wheel_encoders)
# update global geometry (blue shell)
self.global_geometry = (self.geometry.get_transformation_to_pose(
self.pose))
# update the position all of proximity the sensors
for proximity_sensor in self.proximity_sensors:
proximity_sensor.update_position()
# send wheel speeds to physical robot and retrieve sensor values
if self.physical_robot.robot_comm.connected:
self.physical_robot.step()
def stop_motion(self):
"""Stop the physical robot."""
if self.physical_robot.robot_comm.connected:
self.physical_robot.stop()
def set_wheel_drive_rates(self, v_l, v_r):
"""Set the drive rates (angular vel rad/s) for this robot's wheels."""
# simulate physical limit on drive motors
self.left_wheel_drive_rate = min(self.max_speed, v_l)
self.right_wheel_drive_rate = min(self.max_speed, v_r)
print(
"left_wheel_drive_rate = {}, right_wheel_drive_rate = {}").format(
self.left_wheel_drive_rate, self.right_wheel_drive_rate)
def read_encoders(self):
self.physical_robot.read_wheel_encoders()
def setup_robot(self, x=0.0, y=0.0, deg=0.0):
"""Apply the robot and sensor config parameters"""
# retrieve robot config parameters
robot_params = self.viewer.get_robot_parameters()
self.wheel_radius = float(robot_params[0][0][1]) # metres
self.wheel_base_length = float(robot_params[0][1][1]) # metres
self.ticks_per_rev = float(robot_params[0][2][1]) # unitless
self.max_speed = float(robot_params[0][3][1]) # rpm
self.body_width = float(robot_params[0][4][1]) # metres
self.body_length = float(robot_params[0][5][1]) # metres
self.payload_width = float(robot_params[0][6][1]) # metres
self.payload_length = float(robot_params[0][7][1]) # metres
self.payload_offset = float(robot_params[0][8][1]) # metres
self.sensor_min_value = float(robot_params[1][0][1]) # millivolts
self.sensor_max_value = float(robot_params[1][1][1]) # millivolts
self.sensor_min_range = float(robot_params[1][2][1]) # metres
self.sensor_max_range = float(robot_params[1][3][1]) # metres
self.sensor_phi_range = float(robot_params[1][4][1]) # metres
# x, y, theta_degrees
self.sensor_poses = ([[
float(robot_params[2][0][1]),
float(robot_params[2][1][1]),
float(robot_params[2][2][1])
],
[
float(robot_params[3][0][1]),
float(robot_params[3][1][1]),
float(robot_params[3][2][1])
],
[
float(robot_params[4][0][1]),
float(robot_params[4][1][1]),
float(robot_params[4][2][1])
]])
self.robot_body = ([[self.body_length / 2, self.body_width / 2],
[-self.body_length / 2, self.body_width / 2],
[-self.body_length / 2, -self.body_width / 2],
[self.body_length / 2, -self.body_width / 2]])
self.robot_payload = ([
[
self.payload_length / 2 + self.payload_offset,
self.payload_width / 2
],
[
-self.payload_length / 2 + self.payload_offset,
self.payload_width / 2
],
[
-self.payload_length / 2 + self.payload_offset,
-self.payload_width / 2
],
[
self.payload_length / 2 + self.payload_offset,
-self.payload_width / 2
]
])
# drive rates
self.max_speed *= ((2 * pi) / 60) # rpm 2 rad/s
self.trans_vel_limit = self.max_speed * self.wheel_radius # m/s
self.ang_vel_limit = 0.2 * self.max_speed # rad/s
if debug:
print("SPEED {:.3f}m/s").format(self.trans_vel_limit)
# pose
theta = -pi + ((deg / 360.0) * 2 * pi)
self.pose = Pose(x, y, theta)
# geometry
self.geometry = Polygon(self.robot_body)
self.global_geometry = (self.geometry.get_transformation_to_pose(
self.pose))
# wheel encoders
self.left_wheel_encoder = WheelEncoder(self.ticks_per_rev)
self.right_wheel_encoder = WheelEncoder(self.ticks_per_rev)
self.wheel_encoders = ([
self.left_wheel_encoder, self.right_wheel_encoder
])
# proximity sensors
self.proximity_sensors = []
for _pose in self.sensor_poses:
sensor_pose = Pose(_pose[0], _pose[1], radians(_pose[2]))
self.proximity_sensors.append(
ProximitySensor(self, sensor_pose, self.sensor_min_range,
self.sensor_max_range,
radians(self.sensor_phi_range)))
# dynamics
self.dynamics = DifferentialDriveDynamics(self, self.wheel_radius,
self.wheel_base_length)
# supervisor
self.supervisor = Supervisor(
RobotSupervisorInterface(self), self.wheel_radius,
self.wheel_base_length, self.ticks_per_rev, self.sensor_poses,
self.sensor_max_range,
Pose(self.pose.x, self.pose.y, self.pose.theta))
# set wheel drive rates (rad/s)
self.left_wheel_drive_rate = 0.0
self.right_wheel_drive_rate = 0.0
